Add Angular Momentum task & use base-estimator entity to achieve walk in torque control

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>.

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).

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>.
 

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).

include/sot/torque_control/admittance-controller.hh

@@ -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;

include/sot/torque_control/base-estimator.hh

@@ -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

include/sot/torque_control/commands-helper.hh

@@ -17,6 +17,7 @@
 /* --- HELPER ---------------------------------------------------------- */
 namespace dynamicgraph {
 namespace sot {
+namespace dg = dynamicgraph;
 namespace torquecontrol {
 using ::dynamicgraph::command::docCommandVerbose;
 using ::dynamicgraph::command::docCommandVoid0;

include/sot/torque_control/control-manager.hh

@@ -130,10 +130,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;

include/sot/torque_control/current-controller.hh

@@ -106,10 +106,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

include/sot/torque_control/device-torque-ctrl.hh

@@ -75,10 +75,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;

include/sot/torque_control/free-flyer-locator.hh

@@ -78,10 +78,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

include/sot/torque_control/imu_offset_compensation.hh

@@ -66,9 +66,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

include/sot/torque_control/inverse-dynamics-balance-controller.hh

@@ -30,13 +30,16 @@
 /* Pinocchio */
 #include <pinocchio/multibody/model.hpp>
 #include <pinocchio/parsers/urdf.hpp>
+#include "pinocchio/algorithm/joint-configuration.hpp"
 
 #include <tsid/robots/robot-wrapper.hpp>
 #include <tsid/solvers/solver-HQP-base.hpp>
 #include <tsid/contacts/contact-6d.hpp>
 #include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
 #include <tsid/tasks/task-com-equality.hpp>
 #include <tsid/tasks/task-joint-posture.hpp>
+#include <tsid/tasks/task-angular-momentum-equality.hpp>
+// #include "tsid/tasks/task-angular-momentum-integral-equality.hpp"
 #include <tsid/trajectories/trajectory-euclidian.hpp>
 
 /* HELPER */
@@ -50,6 +53,10 @@ namespace dynamicgraph {
 namespace sot {
 namespace torque_control {
 
+enum ControlOutput { CONTROL_OUTPUT_VELOCITY = 0, CONTROL_OUTPUT_TORQUE = 1, CONTROL_OUTPUT_SIZE = 2 };
+
+const std::string ControlOutput_s[] = {"velocity", "torque"};
+
 /* --------------------------------------------------------------------- */
 /* --- CLASS ----------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
@@ -66,6 +73,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
 
   void init(const double& dt, const std::string& robotRef);
   void updateComOffset();
+  virtual void setControlOutputType(const std::string& type);
   void removeRightFootContact(const double& transitionTime);
   void removeLeftFootContact(const double& transitionTime);
   void addRightFootContact(const double& transitionTime);
@@ -79,6 +87,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
   DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(am_ref_L, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(am_ref_dL, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector);
@@ -106,6 +116,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
   DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(kp_am, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(kd_am, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(kp_feet, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(kd_feet, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(kp_hands, dynamicgraph::Vector);
@@ -114,8 +126,10 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
   DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(kp_tau, dynamicgraph::Vector);
 
   DECLARE_SIGNAL_IN(w_com, double);
+  DECLARE_SIGNAL_IN(w_am, double);
   DECLARE_SIGNAL_IN(w_feet, double);
   DECLARE_SIGNAL_IN(w_hands, double);
   DECLARE_SIGNAL_IN(w_posture, double);
@@ -140,18 +154,24 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
   DECLARE_SIGNAL_IN(ddq_max, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(dt_joint_pos_limits, double);
 
-  DECLARE_SIGNAL_IN(tau_estimated, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(tau_measured, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(com_measured, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(wrench_base, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector);
   DECLARE_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(ref_phase, int);
 
   DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector);  /// mask with 1 for controlled joints, 0 otherwise
 
   DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(M, dynamicgraph::Matrix);
   DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(tau_pd_des, dynamicgraph::Vector);
+
   DECLARE_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector);
@@ -164,12 +184,19 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
   DECLARE_SIGNAL_OUT(zmp_left_foot, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(zmp, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(com_est, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(com_vel, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(com_acc, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(com_acc_des, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(am_dL, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(am_dL_des, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(right_foot_pos_quat, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(left_foot_pos_quat, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(lf_est, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(rf_est, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector);
   DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector);
@@ -190,10 +217,6 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
   /* --- 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:
   double m_dt;  /// control loop time period
   double m_t;
@@ -243,6 +266,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
   tsid::contacts::Contact6d* m_contactRH;
   tsid::contacts::Contact6d* m_contactLH;
   tsid::tasks::TaskComEquality* m_taskCom;
+  tsid::tasks::TaskAMEquality* m_taskAM;
+  tsid::tasks::TaskSE3Equality* m_taskWaist;
   tsid::tasks::TaskSE3Equality* m_taskRF;
   tsid::tasks::TaskSE3Equality* m_taskLF;
   tsid::tasks::TaskSE3Equality* m_taskRH;
@@ -251,22 +276,33 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
   tsid::tasks::TaskJointPosture* m_taskBlockedJoints;
 
   tsid::trajectories::TrajectorySample m_sampleCom;
+  tsid::trajectories::TrajectorySample m_sampleAM;
   tsid::trajectories::TrajectorySample m_sampleRF;
   tsid::trajectories::TrajectorySample m_sampleLF;
   tsid::trajectories::TrajectorySample m_sampleRH;
   tsid::trajectories::TrajectorySample m_sampleLH;
+  tsid::trajectories::TrajectorySample m_sampleWaist;
   tsid::trajectories::TrajectorySample m_samplePosture;
 
   double m_w_com;
+  double m_w_am;
   double m_w_posture;
   double m_w_hands;
+  double m_w_base_orientation;
+
+  tsid::math::Vector m_dv_sot;    /// desired accelerations (sot order)
+  tsid::math::Vector m_dv_urdf;   /// desired accelerations (urdf order)
+  tsid::math::Vector m_v_sot;     /// desired velocities (sot order)
+  tsid::math::Vector m_v_urdf;    /// desired and current velocities (urdf order) (close the TSID loop on it)
+  tsid::math::Vector m_q_sot;     /// desired positions (sot order)
+  tsid::math::Vector m_q_urdf;    /// desired and current positions (urdf order) (close the TSID loop on it)
+  tsid::math::Vector m_tau_sot;   /// desired torques (sot order)
 
-  tsid::math::Vector m_dv_sot;             /// desired accelerations (sot order)
-  tsid::math::Vector m_dv_urdf;            /// desired accelerations (urdf order)
   tsid::math::Vector m_f;                  /// desired force coefficients (24d)
   tsid::math::Vector6 m_f_RF;              /// desired 6d wrench right foot
   tsid::math::Vector6 m_f_LF;              /// desired 6d wrench left foot
   tsid::math::Vector3 m_com_offset;        /// 3d CoM offset
+  tsid::math::Vector3 m_dcom_offset;        /// 3d CoM offset
   tsid::math::Vector3 m_zmp_des_LF;        /// 3d desired zmp left foot
   tsid::math::Vector3 m_zmp_des_RF;        /// 3d desired zmp left foot
   tsid::math::Vector3 m_zmp_des_LF_local;  /// 3d desired zmp left foot expressed in local frame
@@ -275,20 +311,20 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
   tsid::math::Vector3 m_zmp_LF;            /// 3d zmp left foot
   tsid::math::Vector3 m_zmp_RF;            /// 3d zmp left foot
   tsid::math::Vector3 m_zmp;               /// 3d global zmp
-  tsid::math::Vector m_tau_sot;
-  tsid::math::Vector m_q_urdf;
-  tsid::math::Vector m_v_urdf;
+
 
   typedef pinocchio::Data::Matrix6x Matrix6x;
+  pinocchio::Data m_estim_data;
   Matrix6x m_J_RF;
   Matrix6x m_J_LF;
   Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
   Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
   tsid::math::Vector6 m_v_RF_int;
   tsid::math::Vector6 m_v_LF_int;
 
-  unsigned int m_timeLast;
-  RobotUtilShrPtr m_robot_util;
+  unsigned int m_timeLast;        /// Final time of the control loop
+  RobotUtilShrPtr m_robot_util;   /// Share pointer to the robot utils methods
+  ControlOutput m_ctrlMode;       /// ctrl mode desired for the output (velocity or torque)
 
 };  // class InverseDynamicsBalanceController
 }  // namespace torque_control