Merge pull request #63 from olivier-stasse/devel

This PR fix bugs related to changes push in sot-core and dynamic-graph

.travis @ 679949e2

-Subproject commit dc8b946d456d2c41ad12b819111b005148c68031
+Subproject commit 679949e28a91c334edf3588753cf294aec2fc4c7

cmake @ 61e5574a

-Subproject commit 1d9aeca25e970d2d967fd5be0fb93fe961db121b
+Subproject commit 61e5574a0615706aab06986f6aecf665ddc31141

include/sot/torque_control/admittance-controller.hh

@@ -108,7 +108,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       protected:
@@ -146,7 +146,7 @@ namespace dynamicgraph {
         tsid::math::Vector6 m_v_RF_int;
         tsid::math::Vector6 m_v_LF_int;
 
-        RobotUtil * m_robot_util;
+        RobotUtilShrPtr m_robot_util;
 
 //        tsid::math::Vector3 m_zmp_des_LF;          /// 3d desired zmp left foot
 //        tsid::math::Vector3 m_zmp_des_RF;          /// 3d desired zmp left foot

include/sot/torque_control/base-estimator.hh

@@ -174,14 +174,14 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       protected:
         bool              m_initSucceeded;    /// true if the entity has been successfully initialized
         bool              m_reset_foot_pos;   /// true after the command resetFootPositions is called
         double            m_dt;               /// sampling time step
-        RobotUtil *       m_robot_util;
+        RobotUtilShrPtr       m_robot_util;
 
         bool              m_left_foot_is_stable;    /// True if left foot as been stable for the last 'm_fz_stable_windows_size' samples
         bool              m_right_foot_is_stable;   /// True if right foot as been stable for the last 'm_fz_stable_windows_size' samples

include/sot/torque_control/control-manager.hh

@@ -88,9 +88,9 @@ namespace dynamicgraph {
         /* --- CONSTRUCTOR ---- */
         ControlManager( const std::string & name);
 
-	/// Initialize
-	/// @param dt: control interval
-	/// @param urdfFile: path to the URDF model of the robot
+        /// Initialize
+        /// @param dt: control interval
+        /// @param urdfFile: path to the URDF model of the robot
         void init(const double & dt,
                   const std::string & urdfFile,
                   const std::string & robotRef);
@@ -112,35 +112,39 @@ namespace dynamicgraph {
 
         /* --- COMMANDS --- */
 
-	/// Commands related to the control mode.
+        /// Commands related to the control mode.
         void addCtrlMode(const std::string& name);
         void ctrlModes();
         void getCtrlMode(const std::string& jointName);
         void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
         void setCtrlMode(const int jid, const CtrlMode& cm);
-	
+
         void resetProfiler();
-	
-	/// Commands related to joint name and joint id
-	void setNameToId(const std::string& jointName, const double & jointId);
-	void setJointLimitsFromId(const double &jointId, 
-				const double &lq, const double &uq);
-
-	/// Command related to ForceUtil
-	void setForceLimitsFromId(const double &jointId, 
-				  const dynamicgraph::Vector &lq, 
-				  const dynamicgraph::Vector &uq);
-	void setForceNameToForceId(const std::string& forceName, 
-				   const double & forceId);
-	
-	/// Commands related to FootUtil
-	void setRightFootSoleXYZ(const dynamicgraph::Vector &);
+
+        /// Commands related to joint name and joint id
+        void setNameToId(const std::string& jointName, const double & jointId);
+        void setJointLimitsFromId(const double &jointId,
+                                const double &lq, const double &uq);
+
+        /// Command related to ForceUtil
+        void setForceLimitsFromId(const double &jointId,
+                                  const dynamicgraph::Vector &lq,
+                                  const dynamicgraph::Vector &uq);
+        void setForceNameToForceId(const std::string& forceName,
+                                   const double & forceId);
+
+        /// Commands related to FootUtil
+        void setRightFootSoleXYZ(const dynamicgraph::Vector &);
         void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
-	void setFootFrameName(const std::string &, const std::string &);
+        void setFootFrameName(const std::string &, const std::string &);
         void setImuJointName(const std::string &);
-	void displayRobotUtil();
-	/// Set the mapping between urdf and sot.
-	void setJoints(const dynamicgraph::Vector &);
+        void displayRobotUtil();
+
+        /// Commands related to HandUtil
+        void setHandFrameName(const std::string &, const std::string &);
+
+        /// Set the mapping between urdf and sot.
+        void setJoints(const dynamicgraph::Vector &);
 
         void setStreamPrintPeriod(const double & s);
         void setSleepTime(const double &seconds);
@@ -151,11 +155,11 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[ControlManager-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[ControlManager-"+name+"] "+msg, t, file, line);
         }
 
       protected:
-        RobotUtil *                   m_robot_util;
+        RobotUtilShrPtr               m_robot_util;
         tsid::robots::RobotWrapper *  m_robot;
         bool    m_initSucceeded;    /// true if the entity has been successfully initialized
         double  m_dt;               /// control loop time period

include/sot/torque_control/current-controller.hh

@@ -115,11 +115,11 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[CurrentController-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[CurrentController-"+name+"] "+msg, t, file, line);
         }
 
       protected:
-	RobotUtil * m_robot_util;
+	RobotUtilShrPtr m_robot_util;
         bool    m_initSucceeded;    /// true if the entity has been successfully initialized
         bool    m_emergency_stop_triggered;
         double  m_dt;               /// control loop time period

include/sot/torque_control/ddp-actuator-solver.hh

@@ -34,7 +34,7 @@ namespace dynamicgraph {
 
 #define ALL_INPUT_SIGNALS m_pos_desSIN << m_pos_motor_measureSIN \
       << m_pos_joint_measureSIN << m_dx_measureSIN		 \
-      << m_tau_measureSIN << m_temp_measureSIN 
+      << m_tau_measureSIN << m_temp_measureSIN << m_tau_desSIN
 
 #define ALL_OUTPUT_SIGNALS  m_tauSOUT
       
@@ -49,7 +49,8 @@ namespace dynamicgraph {
 	  DECLARE_SIGNAL_IN(pos_joint_measure,  dynamicgraph::Vector);
 	  DECLARE_SIGNAL_IN(dx_measure,         dynamicgraph::Vector);
 	  DECLARE_SIGNAL_IN(tau_measure,        dynamicgraph::Vector);
-	  DECLARE_SIGNAL_IN(temp_measure,        dynamicgraph::Vector);	  
+          DECLARE_SIGNAL_IN(tau_des,            dynamicgraph::Vector);
+          DECLARE_SIGNAL_IN(temp_measure,       dynamicgraph::Vector);
 	  DECLARE_SIGNAL_OUT(tau,               dynamicgraph::Vector);
 
 	protected:

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

@@ -98,7 +98,7 @@ namespace dynamicgraph
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[DeviceTorqueCtrl] "+msg, t, file, line);
+	  Entity::sendMsg("[DeviceTorqueCtrl] "+msg, t, file, line);
         }
 
         /// \brief Current integration step.
@@ -176,7 +176,7 @@ namespace dynamicgraph
         DIST normalDistribution_;
         GEN  normalRandomNumberGenerator_;
 
-        RobotUtil * m_robot_util;
+        RobotUtilShrPtr m_robot_util;
       };
 
     }   // end namespace torque_control

include/sot/torque_control/filter-differentiator.hh

@@ -106,7 +106,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       public: /* --- ENTITY INHERITANCE --- */

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

@@ -97,7 +97,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[FreeFlyerLocator-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[FreeFlyerLocator-"+name+"] "+msg, t, file, line);
         }
 
       protected:
@@ -115,7 +115,7 @@ namespace dynamicgraph {
         Eigen::VectorXd   m_v_pin;            /// robot velocities according to pinocchio convention
         Eigen::VectorXd   m_v_sot;            /// robot velocities according to SoT convention
 
-	RobotUtil * m_robot_util;
+	RobotUtilShrPtr m_robot_util;
 
       }; // class FreeFlyerLocator
 

include/sot/torque_control/imu_offset_compensation.hh

@@ -84,7 +84,7 @@ namespace dynamicgraph {
         void update_offset_impl(int iter);
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[ImuOffsetCompensation-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[ImuOffsetCompensation-"+name+"] "+msg, t, file, line);
         }
 
       protected:

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

@@ -85,6 +85,10 @@ namespace dynamicgraph {
         void removeLeftFootContact(const double& transitionTime);
         void addRightFootContact(const double& transitionTime);
         void addLeftFootContact(const double& transitionTime);
+        void addTaskRightHand(/*const double& transitionTime*/);
+        void removeTaskRightHand(const double& transitionTime);
+        void addTaskLeftHand(/*const double& transitionTime*/);
+        void removeTaskLeftHand(const double& transitionTime);
 
         /* --- SIGNALS --- */
         DECLARE_SIGNAL_IN(com_ref_pos,                dynamicgraph::Vector);
@@ -96,6 +100,12 @@ namespace dynamicgraph {
         DECLARE_SIGNAL_IN(lf_ref_pos,                 dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(lf_ref_vel,                 dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(lf_ref_acc,                 dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(rh_ref_pos,                 dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(rh_ref_vel,                 dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(rh_ref_acc,                 dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(lh_ref_pos,                 dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(lh_ref_vel,                 dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(lh_ref_acc,                 dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(posture_ref_pos,            dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(posture_ref_vel,            dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(posture_ref_acc,            dynamicgraph::Vector);
@@ -113,6 +123,8 @@ namespace dynamicgraph {
         DECLARE_SIGNAL_IN(kd_com,                     dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(kp_feet,                    dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(kd_feet,                    dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(kp_hands,                   dynamicgraph::Vector);
+        DECLARE_SIGNAL_IN(kd_hands,                   dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(kp_posture,                 dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(kd_posture,                 dynamicgraph::Vector);
         DECLARE_SIGNAL_IN(kp_pos,                     dynamicgraph::Vector);
@@ -120,6 +132,7 @@ namespace dynamicgraph {
 
         DECLARE_SIGNAL_IN(w_com,                      double);
         DECLARE_SIGNAL_IN(w_feet,                     double);
+        DECLARE_SIGNAL_IN(w_hands,                    double);
         DECLARE_SIGNAL_IN(w_posture,                  double);
         DECLARE_SIGNAL_IN(w_base_orientation,         double);
         DECLARE_SIGNAL_IN(w_torques,                  double);
@@ -172,10 +185,16 @@ namespace dynamicgraph {
         DECLARE_SIGNAL_OUT(base_orientation,          dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(right_foot_pos,            dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(left_foot_pos,             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);
         DECLARE_SIGNAL_OUT(left_foot_vel,             dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(right_hand_vel,            dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(left_hand_vel,             dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(right_foot_acc,            dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(left_foot_acc,             dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(right_hand_acc,            dynamicgraph::Vector);
+        DECLARE_SIGNAL_OUT(left_hand_acc,             dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(right_foot_acc_des,        dynamicgraph::Vector);
         DECLARE_SIGNAL_OUT(left_foot_acc_des,         dynamicgraph::Vector);
 
@@ -188,7 +207,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       protected:
@@ -210,9 +229,29 @@ namespace dynamicgraph {
         ContactState      m_contactState;
         double            m_contactTransitionTime;  /// end time of the current contact transition (if any)
 
+        enum RightHandState
+        {
+          TASK_RIGHT_HAND_ON = 0,
+          /*TASK_RIGHT_HAND_TRANSITION = 1,*/
+          TASK_RIGHT_HAND_OFF = 1
+        };
+        RightHandState         m_rightHandState;
+
+        enum LeftHandState
+        {
+          TASK_LEFT_HAND_ON = 0,
+          /*TASK_LEFT_HAND_TRANSITION = 1,*/
+          TASK_LEFT_HAND_OFF = 1
+        };
+        LeftHandState         m_leftHandState;
+        /*double            m_handsTransitionTime;*/  /// end time of the current transition (if any)
+
         int m_frame_id_rf;  /// frame id of right foot
         int m_frame_id_lf;  /// frame id of left foot
 
+        int m_frame_id_rh;  /// frame id of right hand
+        int m_frame_id_lh;  /// frame id of left hand
+
         /// tsid
         tsid::robots::RobotWrapper *                       m_robot;
         tsid::solvers::SolverHQPBase *           m_hqpSolver;
@@ -221,19 +260,26 @@ namespace dynamicgraph {
         tsid::InverseDynamicsFormulationAccForce * m_invDyn;
         tsid::contacts::Contact6d *                m_contactRF;
         tsid::contacts::Contact6d *                m_contactLF;
+        tsid::contacts::Contact6d *                m_contactRH;
+        tsid::contacts::Contact6d *                m_contactLH;
         tsid::tasks::TaskComEquality *             m_taskCom;
         tsid::tasks::TaskSE3Equality *             m_taskRF;
         tsid::tasks::TaskSE3Equality *             m_taskLF;
+        tsid::tasks::TaskSE3Equality *             m_taskRH;
+        tsid::tasks::TaskSE3Equality *             m_taskLH;
         tsid::tasks::TaskJointPosture *            m_taskPosture;
         tsid::tasks::TaskJointPosture *            m_taskBlockedJoints;
 
         tsid::trajectories::TrajectorySample       m_sampleCom;
         tsid::trajectories::TrajectorySample       m_sampleRF;
         tsid::trajectories::TrajectorySample       m_sampleLF;
+        tsid::trajectories::TrajectorySample       m_sampleRH;
+        tsid::trajectories::TrajectorySample       m_sampleLH;
         tsid::trajectories::TrajectorySample       m_samplePosture;
 
         double m_w_com;
         double m_w_posture;
+        double m_w_hands;
 
         tsid::math::Vector  m_dv_sot;              /// desired accelerations (sot order)
         tsid::math::Vector  m_dv_urdf;             /// desired accelerations (urdf order)
@@ -262,7 +308,7 @@ namespace dynamicgraph {
         tsid::math::Vector6 m_v_LF_int;
 
         unsigned int m_timeLast;
-        RobotUtil * m_robot_util;
+        RobotUtilShrPtr m_robot_util;
 
       }; // class InverseDynamicsBalanceController
     }    // namespace torque_control

include/sot/torque_control/joint-torque-controller.hh

@@ -124,11 +124,11 @@ namespace dynamicgraph {
 //        Eigen::VectorXd m_dqDesIntegral; /// integral of the desired velocity
         Eigen::VectorXd m_dqErrIntegral; /// integral of the velocity error
 
-	RobotUtil * m_robot_util;
+	RobotUtilShrPtr m_robot_util;
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       public:

include/sot/torque_control/joint-trajectory-generator.hh

@@ -163,7 +163,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[JointTrajectoryGenerator-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[JointTrajectoryGenerator-"+name+"] "+msg, t, file, line);
         }
 
       protected:
@@ -182,7 +182,7 @@ namespace dynamicgraph {
         bool              m_firstIter;        /// true if it is the first iteration, false otherwise
         double            m_dt;               /// control loop time period
 
-	RobotUtil     *m_robot_util;
+	RobotUtilShrPtr m_robot_util;
 
         std::vector<int>  m_iterForceSignals;
 

include/sot/torque_control/madgwickahrs.hh

@@ -97,7 +97,7 @@ namespace dynamicgraph {
         //void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[MadgwickAHRS-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[MadgwickAHRS-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/nd-trajectory-generator.hh

@@ -148,7 +148,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[NdTrajectoryGenerator-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[NdTrajectoryGenerator-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/numerical-difference.hh

@@ -117,7 +117,7 @@ namespace dynamicgraph {
       protected:
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       public: /* --- ENTITY INHERITANCE --- */

include/sot/torque_control/position-controller.hh

@@ -89,11 +89,11 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[PositionController-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[PositionController-"+name+"] "+msg, t, file, line);
         }
 
       protected:
-	RobotUtil *       m_robot_util;        /// Robot Util
+	RobotUtilShrPtr       m_robot_util;        /// Robot Util
         Eigen::VectorXd   m_pwmDes;
         bool              m_initSucceeded;    /// true if the entity has been successfully initialized
         double            m_dt;               /// control loop time period

include/sot/torque_control/se3-trajectory-generator.hh

@@ -140,7 +140,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[SE3TrajectoryGenerator-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[SE3TrajectoryGenerator-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/torque-offset-estimator.hh

@@ -94,7 +94,7 @@ namespace dynamicgraph {
         DECLARE_SIGNAL_OUT(jointTorquesEstimated,   dynamicgraph::Vector);	
 
       protected:
-        RobotUtil *       m_robot_util;
+        RobotUtilShrPtr       m_robot_util;
         pinocchio::Model  m_model;            /// Pinocchio robot model
         pinocchio::Data   *m_data;            /// Pinocchio robot data 
         int n_iterations;   //Number of iterations to consider
@@ -114,7 +114,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       private: