stack-of-tasks · NoelieRamuzat · Apr 6, 2020 · Apr 6, 2020 · May 12, 2020 · May 25, 2020
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -79,6 +79,8 @@ SET(${PROJECT_NAME}_HEADERS
   include/${CUSTOM_HEADER_DIR}/se3-trajectory-generator.hh
   include/${CUSTOM_HEADER_DIR}/free-flyer-locator.hh
   include/${CUSTOM_HEADER_DIR}/inverse-dynamics-balance-controller.hh
+  include/${CUSTOM_HEADER_DIR}/simple-inverse-dyn.hh
+  include/${CUSTOM_HEADER_DIR}/posture-task.hh
   include/${CUSTOM_HEADER_DIR}/position-controller.hh
   include/${CUSTOM_HEADER_DIR}/control-manager.hh
   include/${CUSTOM_HEADER_DIR}/current-controller.hh

diff --git a/doc/Overview.md b/doc/Overview.md
@@ -51,6 +51,10 @@ Pay attention not to install ROS using robotpkg though, because it would install
 
 You can find the full installation procedure in the <a href="md_doc_installation.html">installation page</a>.
 
-Quick instructions on how to run a test can be found <a href="md_doc_running.html">here</a>.
+Instructions for running a simulation of Pyrene executing a CoM sinusoid in position or torque control can be found <a href="md_doc_running.html">here</a>.
 
 Instructions for running a simulation or an experiment using the DDP on the right elbow of Pyrene can be found <a href="md_doc_ddpRun.html">here</a>.
+
+Instructions for running a simulation of Pyrene executing a foot sinusoid in the air in torque control can be found <a href="md_doc_bellStepRun.html">here</a>.
+
+Instructions for running a simulation of Pyrene walking in torque control can be found <a href="md_doc_walkRun.html">here</a>.
diff --git a/doc/bellStepRun.md b/doc/bellStepRun.md
@@ -0,0 +1,40 @@
+# Pyrene step in the air in torque control
+
+In the following, we demonstrate how to run the foot sinusoid simulation with  <a href="https://github.com/stack-of-tasks/sot-torque-control">sot-torque-control</a>, and talos-torque-control.
+
+## Start the simulation
+
+Start the simulation with the robot in the half-sitting position:
+```
+roslaunch talos_data talos_gazebo.launch start_half_sitting:=true
+```
+
+## Start the SoT in torque mode
+
+To start the SoT in simulation in torque mode: 
+```
+roslaunch roscontrol_sot_talos sot_talos_controller_gazebo_effort.launch
+```
+
+## Run the test
+
+First of all, you need to go to the folder where your script is.
+For running the ddp test of talos-torque-control, assuming you are in the root directory:
+
+```
+cd script
+```
+
+Then, you can just run the test :
+
+```
+python sim_torque_bellStep.py
+```
+
+This will launch the simulation making the robot executing a sinusoid movement of its left foot in the air (a "bell step") in torque control. 
+
+The script also saves the dynamic graph in /tmp/sot_talos_tsid_bellStep.pdf.
+
+## Other
+
+More information on how to use the SoT and how to work on Talos can be found <a href="https://wiki.laas.fr/robots/Pyrene">in the robot wiki page</a> (you need LAAS permissions to access this).
diff --git a/doc/running.md b/doc/running.md
@@ -1,4 +1,4 @@
-# Running a test
+# Pyrene CoM sinusoid in position or torque control
 
 In the following, we quickly demonstrate how to run a test with sot-torque-control and <a href="https://gitlab.laas.fr/pyrene-dev/talos-torque-control.git">talos-torque-control</a>.
 

diff --git a/doc/walkRun.md b/doc/walkRun.md
@@ -0,0 +1,59 @@
+# Make Pyrene walk in torque control (quasistatic trajectories)
+
+In the following, we demonstrate how to run the walking simulation with  <a href="https://github.com/stack-of-tasks/sot-torque-control">sot-torque-control</a>, and talos-torque-control; using the reference quasistatic trajectories computed by <a href="https://github.com/loco-3d/multicontact-api">multicontact-api</a>.
+
+## Start the simulation
+
+Start the simulation with the robot in the half-sitting position:
+```
+roslaunch talos_data talos_gazebo.launch start_half_sitting:=true
+```
+
+## Start the SoT in torque mode
+
+To start the SoT in simulation in torque mode: 
+```
+roslaunch roscontrol_sot_talos sot_talos_controller_gazebo_effort.launch
+```
+
+## Run the test
+
+First of all, you need to go to the folder where your script is.
+For running the ddp test of talos-torque-control, assuming you are in the root directory:
+
+```
+cd script
+```
+
+Then, you can just run the test, specifying which type of walk you want the robot to execute (on spot or 20cm steps):
+
+```
+Usage: python sim_walk_torque.py walk_type:=[on_spot|walk_20] {path_folder_of_the_reference_trajectories}
+```
+For instance, for the walk on spot simulation, just run:
+
+```
+python sim_walk_torque.py on_spot
+```
+
+This will launch the simulation making the robot walk on spot in torque control (for now only a quasistatic movement). 
+
+The script also saves the dynamic graph in /tmp/sot_talos_tsid_walk.pdf.
+
+For the 20cm walk just specify "walk_20" instead of "on_spot" in the command line.
+
+If you have some reference trajectories which are not the ones of the talos-torque-control package, you can test them by specifying the absolute path of their folder:
+
+```
+python sim_walk_torque.py walk_20 path_to_folder_of_ref_trajectories
+```
+
+The trajectories must have a .dat extension and the following names:
+* am.dat -> angular momentum trajectory (3D vector to 9D vector if derivatives)
+* com.dat -> center of Mass trajectory (3D vector to 9D vector if derivatives)
+* leftFoot.dat and rightFoot.dat -> feet trajectories (12D SE3 vector to 36D SE3 vector if derivatives)
+* leftForceFoot.dat and rightForceFoot.dat -> feet forces trajectories (6D vector to 18D vector if derivatives)
+
+## Other
+
+More information on how to use the SoT and how to work on Talos can be found <a href="https://wiki.laas.fr/robots/Pyrene">in the robot wiki page</a> (you need LAAS permissions to access this).
diff --git a/include/sot/torque_control/admittance-controller.hh b/include/sot/torque_control/admittance-controller.hh
@@ -24,17 +24,17 @@
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-#include <map>
-#include "boost/assign.hpp"
-
+#include <pinocchio/spatial/fwd.hpp>
+#include <sot/core/robot-utils.hh>
 #include <tsid/robots/robot-wrapper.hpp>
 #include <tsid/tasks/task-se3-equality.hpp>
 
 /* HELPER */
 #include <dynamic-graph/signal-helper.h>
 #include <sot/core/matrix-geometry.hh>
-#include <sot/core/robot-utils.hh>
 #include <sot/torque_control/utils/vector-conversions.hh>
+#include <map>
+#include "boost/assign.hpp"
 
 namespace dynamicgraph {
 namespace sot {
@@ -90,10 +90,6 @@ class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController : public ::dynamicgrap
   /* --- ENTITY INHERITANCE --- */
   virtual void display(std::ostream& os) const;
 
-  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
-    logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
-  }
-
  protected:
   Eigen::VectorXd m_u;  /// control (i.e. motor currents)
   bool m_firstIter;

diff --git a/include/sot/torque_control/base-estimator.hh b/include/sot/torque_control/base-estimator.hh
@@ -23,7 +23,8 @@
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
-
+#include <pinocchio/spatial/fwd.hpp>
+#include <sot/core/robot-utils.hh>
 #include <map>
 #include "boost/assign.hpp"
 //#include <boost/random/normal_distribution.hpp>
@@ -37,7 +38,6 @@
 /* HELPER */
 #include <dynamic-graph/signal-helper.h>
 #include <sot/core/matrix-geometry.hh>
-#include <sot/core/robot-utils.hh>
 #include <sot/torque_control/utils/vector-conversions.hh>
 
 namespace dynamicgraph {
@@ -160,10 +160,6 @@ class SOTBASEESTIMATOR_EXPORT BaseEstimator : public ::dynamicgraph::Entity {
   /* --- ENTITY INHERITANCE --- */
   virtual void display(std::ostream& os) const;
 
-  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
-    logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
-  }
-
  protected:
   bool m_initSucceeded;   /// true if the entity has been successfully initialized
   bool m_reset_foot_pos;  /// true after the command resetFootPositions is called

diff --git a/include/sot/torque_control/control-manager.hh b/include/sot/torque_control/control-manager.hh
@@ -24,16 +24,19 @@
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
+#include <pinocchio/spatial/fwd.hpp>
+#include <sot/core/robot-utils.hh>
+#include <tsid/robots/robot-wrapper.hpp>
+
 #include <map>
 #include "boost/assign.hpp"
 
 #include <pinocchio/multibody/model.hpp>
 #include <pinocchio/parsers/urdf.hpp>
 
-#include <tsid/robots/robot-wrapper.hpp>
+
 #include <dynamic-graph/signal-helper.h>
 #include <sot/core/matrix-geometry.hh>
-#include <sot/core/robot-utils.hh>
 #include <sot/torque_control/utils/vector-conversions.hh>
 
 namespace dynamicgraph {
@@ -130,10 +133,6 @@ class SOTCONTROLMANAGER_EXPORT ControlManager : public ::dynamicgraph::Entity {
   /* --- ENTITY INHERITANCE --- */
   virtual void display(std::ostream& os) const;
 
-  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
-    logger_.stream(t) << ("[ControlManager-" + name + "] " + msg) << '\n';
-  }
-
  protected:
   RobotUtilShrPtr m_robot_util;
   tsid::robots::RobotWrapper* m_robot;

diff --git a/include/sot/torque_control/current-controller.hh b/include/sot/torque_control/current-controller.hh
@@ -27,15 +27,16 @@
 #include <map>
 #include "boost/assign.hpp"
 
-#include <pinocchio/multibody/model.hpp>
-#include <pinocchio/parsers/urdf.hpp>
-
+#include <pinocchio/spatial/fwd.hpp>
+#include <sot/core/robot-utils.hh>
 #include <tsid/robots/robot-wrapper.hpp>
 #include <dynamic-graph/signal-helper.h>
 #include <sot/core/matrix-geometry.hh>
-#include <sot/core/robot-utils.hh>
 #include <sot/torque_control/utils/vector-conversions.hh>
 
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+
 namespace dynamicgraph {
 namespace sot {
 namespace torque_control {
@@ -106,10 +107,6 @@ class SOTCURRENTCONTROLLER_EXPORT CurrentController : public ::dynamicgraph::Ent
   /* --- ENTITY INHERITANCE --- */
   virtual void display(std::ostream& os) const;
 
-  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
-    logger_.stream(t) << ("[CurrentController-" + name + "] " + msg) << '\n';
-  }
-
  protected:
   RobotUtilShrPtr m_robot_util;
   bool m_initSucceeded;  /// true if the entity has been successfully initialized

diff --git a/include/sot/torque_control/ddp-actuator-solver.hh b/include/sot/torque_control/ddp-actuator-solver.hh
@@ -15,10 +15,10 @@
 #define SOTDDPACTUATORSOLVER_EXPORT
 #endif
 
+#include <sot/core/robot-utils.hh>
 #include <tsid/utils/stop-watch.hpp>
 #include <dynamic-graph/signal-helper.h>
 #include <sot/core/matrix-geometry.hh>
-#include <sot/core/robot-utils.hh>
 #include <sot/core/causal-filter.hh>
 
 #include <ddp-actuator-solver/ddpsolver.hh>

diff --git a/include/sot/torque_control/ddp_pyrene_actuator_solver.hh b/include/sot/torque_control/ddp_pyrene_actuator_solver.hh
@@ -28,11 +28,11 @@
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
+#include <sot/core/robot-utils.hh>
 #include <vector>
 #include <tsid/utils/stop-watch.hpp>
 #include <dynamic-graph/signal-helper.h>
 #include <sot/core/matrix-geometry.hh>
-#include <sot/core/robot-utils.hh>
 #include <sot/core/causal-filter.hh>
 
 #include <ddp-actuator-solver/ddpsolver.hh>

diff --git a/include/sot/torque_control/device-torque-ctrl.hh b/include/sot/torque_control/device-torque-ctrl.hh
@@ -12,6 +12,8 @@
 #include <boost/random/normal_distribution.hpp>
 #include <boost/random/variate_generator.hpp>
 
+#include <pinocchio/spatial/fwd.hpp>
+#include <sot/core/robot-utils.hh>
 #include <tsid/robots/robot-wrapper.hpp>
 #include <tsid/tasks/task-se3-equality.hpp>
 
@@ -24,7 +26,6 @@
 
 /* HELPER */
 #include <dynamic-graph/signal-helper.h>
-#include <sot/core/robot-utils.hh>
 
 namespace dgsot = dynamicgraph::sot;
 
@@ -75,10 +76,6 @@ class DeviceTorqueCtrl : public dgsot::Device {
   virtual void integrate(const double& dt);
   void computeForwardDynamics();
 
-  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
-    logger_.stream(t) << ("[DeviceTorqueCtrl] " + msg) << '\n';
-  }
-
   /// \brief Current integration step.
   double timestep_;
   bool m_initSucceeded;

diff --git a/include/sot/torque_control/free-flyer-locator.hh b/include/sot/torque_control/free-flyer-locator.hh
@@ -23,9 +23,11 @@
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
+
 #include <map>
+#include <pinocchio/spatial/fwd.hpp>
 #include "boost/assign.hpp"
-
+#include <sot/core/robot-utils.hh>
 /* Pinocchio */
 #include <pinocchio/multibody/model.hpp>
 #include <pinocchio/parsers/urdf.hpp>
@@ -37,7 +39,6 @@
 /* HELPER */
 #include <dynamic-graph/signal-helper.h>
 #include <sot/core/matrix-geometry.hh>
-#include <sot/core/robot-utils.hh>
 #include <sot/torque_control/utils/vector-conversions.hh>
 
 namespace dynamicgraph {
@@ -78,10 +79,6 @@ class SOTFREEFLYERLOCATOR_EXPORT FreeFlyerLocator : public ::dynamicgraph::Entit
   /* --- ENTITY INHERITANCE --- */
   virtual void display(std::ostream& os) const;
 
-  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
-    logger_.stream(t) << ("[FreeFlyerLocator-" + name + "] " + msg) << '\n';
-  }
-
  protected:
   bool m_initSucceeded;       /// true if the entity has been successfully initialized
   pinocchio::Model* m_model;  /// Pinocchio robot model

diff --git a/include/sot/torque_control/imu_offset_compensation.hh b/include/sot/torque_control/imu_offset_compensation.hh
@@ -70,9 +70,6 @@ class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation : public ::dynamicgr
 
   /* --- METHODS --- */
   void update_offset_impl(int iter);
-  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
-    logger_.stream(t) << ("[ImuOffsetCompensation-" + name + "] " + msg, t, file, line);
-  }
 
  protected:
   bool m_initSucceeded;        /// true if the entity has been successfully initialized