diff --git a/cmake b/cmake index 321eb1c..fb4c22c 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e +Subproject commit fb4c22c319ec5320f9a85527eb1a4130954846f5 diff --git a/doc/Overview.md b/doc/Overview.md index c755e6c..904903d 100644 --- 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 installation page. -Quick instructions on how to run a test can be found here. +Instructions for running a simulation of Pyrene executing a CoM sinusoid in position or torque control can be found here. Instructions for running a simulation or an experiment using the DDP on the right elbow of Pyrene can be found here. + +Instructions for running a simulation of Pyrene executing a foot sinusoid in the air in torque control can be found here. + +Instructions for running a simulation of Pyrene walking in torque control can be found here. diff --git a/doc/bellStepRun.md b/doc/bellStepRun.md new file mode 100644 index 0000000..877f4f4 --- /dev/null +++ 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 sot-torque-control, 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 in the robot wiki page (you need LAAS permissions to access this). diff --git a/doc/running.md b/doc/running.md index a273ec1..1b2f8da 100644 --- 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 talos-torque-control. diff --git a/doc/walkRun.md b/doc/walkRun.md new file mode 100644 index 0000000..2a3fc13 --- /dev/null +++ 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 sot-torque-control, and talos-torque-control; using the reference quasistatic trajectories computed by multicontact-api. + +## 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 in the robot wiki page (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 index edbdea9..5f8f67f 100644 --- a/include/sot/torque_control/admittance-controller.hh +++ b/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; diff --git a/include/sot/torque_control/base-estimator.hh b/include/sot/torque_control/base-estimator.hh index d27505c..e91250a 100644 --- a/include/sot/torque_control/base-estimator.hh +++ b/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 diff --git a/include/sot/torque_control/commands-helper.hh b/include/sot/torque_control/commands-helper.hh index 96d56ae..1fbb2a7 100644 --- a/include/sot/torque_control/commands-helper.hh +++ b/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; diff --git a/include/sot/torque_control/control-manager.hh b/include/sot/torque_control/control-manager.hh index 058555d..fa0fccd 100644 --- a/include/sot/torque_control/control-manager.hh +++ b/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; diff --git a/include/sot/torque_control/current-controller.hh b/include/sot/torque_control/current-controller.hh index 4995e8e..23b711c 100644 --- a/include/sot/torque_control/current-controller.hh +++ b/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 diff --git a/include/sot/torque_control/device-torque-ctrl.hh b/include/sot/torque_control/device-torque-ctrl.hh index 5b72328..3b3b40e 100644 --- a/include/sot/torque_control/device-torque-ctrl.hh +++ b/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; diff --git a/include/sot/torque_control/free-flyer-locator.hh b/include/sot/torque_control/free-flyer-locator.hh index 083c7fb..987a374 100644 --- a/include/sot/torque_control/free-flyer-locator.hh +++ b/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 diff --git a/include/sot/torque_control/imu_offset_compensation.hh b/include/sot/torque_control/imu_offset_compensation.hh index 10a512a..86de0a4 100644 --- a/include/sot/torque_control/imu_offset_compensation.hh +++ b/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 diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 0f3a68f..ccb8f2b 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -30,6 +30,7 @@ /* Pinocchio */ #include #include +#include "pinocchio/algorithm/joint-configuration.hpp" #include #include @@ -37,6 +38,8 @@ #include #include #include +#include +// #include "tsid/tasks/task-angular-momentum-integral-equality.hpp" #include /* 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,11 +311,10 @@ 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 m_J_RF_QR; @@ -287,8 +322,9 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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 diff --git a/include/sot/torque_control/joint-torque-controller.hh b/include/sot/torque_control/joint-torque-controller.hh index 0d7b086..d76c1f4 100644 --- a/include/sot/torque_control/joint-torque-controller.hh +++ b/include/sot/torque_control/joint-torque-controller.hh @@ -113,10 +113,6 @@ class SOTJOINTTORQUECONTROLLER_EXPORT JointTorqueController : public ::dynamicgr RobotUtilShrPtr m_robot_util; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/include/sot/torque_control/joint-trajectory-generator.hh b/include/sot/torque_control/joint-trajectory-generator.hh index d8a23e9..803ebdc 100644 --- a/include/sot/torque_control/joint-trajectory-generator.hh +++ b/include/sot/torque_control/joint-trajectory-generator.hh @@ -147,10 +147,6 @@ class SOTJOINTTRAJECTORYGENERATOR_EXPORT JointTrajectoryGenerator : public ::dyn /* --- 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) << ("[JointTrajectoryGenerator-" + name + "] " + msg) << '\n'; - } - protected: enum JTG_Status { JTG_STOP, JTG_SINUSOID, JTG_MIN_JERK, JTG_LIN_CHIRP, JTG_TRIANGLE, JTG_CONST_ACC, JTG_TEXT_FILE }; diff --git a/include/sot/torque_control/numerical-difference.hh b/include/sot/torque_control/numerical-difference.hh index 8bc9761..7ccd8e8 100644 --- a/include/sot/torque_control/numerical-difference.hh +++ b/include/sot/torque_control/numerical-difference.hh @@ -94,11 +94,6 @@ class SOTNUMERICALDIFFERENCE_EXPORT NumericalDifference : public ::dynamicgraph: */ void init(const double& timestep, const int& sigSize, const double& delay, const int& polyOrder); - protected: - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - public: /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; diff --git a/include/sot/torque_control/position-controller.hh b/include/sot/torque_control/position-controller.hh index 34fdafb..e80bb05 100644 --- a/include/sot/torque_control/position-controller.hh +++ b/include/sot/torque_control/position-controller.hh @@ -72,10 +72,6 @@ class SOTPOSITIONCONTROLLER_EXPORT PositionController : public ::dynamicgraph::E /* --- 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) << ("[PositionController-" + name + "] " + msg) << '\n'; - } - protected: RobotUtilShrPtr m_robot_util; /// Robot Util Eigen::VectorXd m_pwmDes; diff --git a/include/sot/torque_control/se3-trajectory-generator.hh b/include/sot/torque_control/se3-trajectory-generator.hh index acf1255..937d0b3 100644 --- a/include/sot/torque_control/se3-trajectory-generator.hh +++ b/include/sot/torque_control/se3-trajectory-generator.hh @@ -121,10 +121,6 @@ class SOTSE3TRAJECTORYGENERATOR_EXPORT SE3TrajectoryGenerator : public ::dynamic /* --- 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) << ("[SE3TrajectoryGenerator-" + name + "] " + msg) << '\n'; - } - protected: enum TG_Status { TG_STOP, diff --git a/include/sot/torque_control/simple-inverse-dyn.hh b/include/sot/torque_control/simple-inverse-dyn.hh index 8c4c0b9..6f816c7 100644 --- a/include/sot/torque_control/simple-inverse-dyn.hh +++ b/include/sot/torque_control/simple-inverse-dyn.hh @@ -41,7 +41,7 @@ /* Pinocchio */ #include #include -#include "pinocchio/algorithm/joint-configuration.hpp" +#include #include #include @@ -92,6 +92,9 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(tau_measured, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_tau, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector); @@ -117,6 +120,7 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit DECLARE_SIGNAL_IN(q, dynamicgraph::Vector); DECLARE_SIGNAL_IN(v, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_measured, dynamicgraph::Vector); DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector); @@ -125,20 +129,19 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); /* --- COMMANDS --- */ void init(const double& dt, const std::string& robotRef); virtual void setControlOutputType(const std::string& type); - void updateComOffset(); + void updateComOffset(const dynamicgraph::Vector& com_measured); /* --- 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; /// current time @@ -146,6 +149,9 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit bool m_enabled; /// True if controler is enabled bool m_firstTime; /// True at the first iteration of the controller + int m_frame_id_rf; /// frame id of right foot + int m_frame_id_lf; /// frame id of left foot + /// TSID /// Robot tsid::robots::RobotWrapper* m_robot; diff --git a/include/sot/torque_control/torque-offset-estimator.hh b/include/sot/torque_control/torque-offset-estimator.hh index addce40..e18fd98 100644 --- a/include/sot/torque_control/torque-offset-estimator.hh +++ b/include/sot/torque_control/torque-offset-estimator.hh @@ -90,10 +90,6 @@ class TORQUEOFFSETESTIMATOR_EXPORT TorqueOffsetEstimator : public ::dynamicgraph // stdAlignedVector stdVecJointTorqueOffsets; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - private: enum { PRECOMPUTATION, INPROGRESS, COMPUTED } sensor_offset_status; diff --git a/include/sot/torque_control/trace-player.hh b/include/sot/torque_control/trace-player.hh index 759cc2c..5b13003 100644 --- a/include/sot/torque_control/trace-player.hh +++ b/include/sot/torque_control/trace-player.hh @@ -74,10 +74,6 @@ class SOTTRACEPLAYER_EXPORT TracePlayer : 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: typedef dynamicgraph::Vector DataType; typedef std::list DataHistoryType; diff --git a/package.xml b/package.xml index cf0ddf6..817903f 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ sot-torque-control - 1.5.2 + 1.5.3 Collection of dynamic-graph entities aimed at implementing torque control on different robots. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 46ce277..09214c4 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,13 +18,10 @@ SET(plugins se3-trajectory-generator torque-offset-estimator trace-player + ddp-actuator-solver + ddp_pyrene_actuator_solver ) -IF(DDP_ACTUATOR_SOLVER_FOUND) - SET(plugins ${plugins} ddp-actuator-solver) - SET(plugins ${plugins} ddp_pyrene_actuator_solver) -ENDIF(DDP_ACTUATOR_SOLVER_FOUND) - FOREACH(plugin ${plugins}) GET_FILENAME_COMPONENT(LIBRARY_NAME ${plugin} NAME) ADD_LIBRARY(${LIBRARY_NAME} SHARED ${plugin}) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 018dd15..7f389f2 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -7,6 +7,10 @@ #include +#include +#include +#include + #include #include #include @@ -82,11 +86,13 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; #define PROFILE_PREPARE_INV_DYN "InvDynBalCtrl: prepare inv-dyn" #define PROFILE_READ_INPUT_SIGNALS "InvDynBalCtrl: read input signals" -#define ZERO_FORCE_THRESHOLD 1e-3 +#define ZERO_FORCE_THRESHOLD 10 #define INPUT_SIGNALS m_com_ref_posSIN \ << m_com_ref_velSIN \ << m_com_ref_accSIN \ + << m_am_ref_LSIN \ + << m_am_ref_dLSIN \ << m_rf_ref_posSIN \ << m_rf_ref_velSIN \ << m_rf_ref_accSIN \ @@ -113,6 +119,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_kd_constraintsSIN \ << m_kp_comSIN \ << m_kd_comSIN \ + << m_kp_amSIN \ + << m_kd_amSIN \ << m_kp_feetSIN \ << m_kd_feetSIN \ << m_kp_handsSIN \ @@ -121,7 +129,9 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_kd_postureSIN \ << m_kp_posSIN \ << m_kd_posSIN \ + << m_kp_tauSIN \ << m_w_comSIN \ + << m_w_amSIN \ << m_w_feetSIN \ << m_w_handsSIN \ << m_w_postureSIN \ @@ -143,17 +153,22 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_dq_maxSIN \ << m_ddq_maxSIN \ << m_dt_joint_pos_limitsSIN \ - << m_tau_estimatedSIN \ + << m_tau_measuredSIN \ + << m_com_measuredSIN \ << m_qSIN \ << m_vSIN \ << m_wrench_baseSIN \ << m_wrench_left_footSIN \ << m_wrench_right_footSIN \ + << m_ref_phaseSIN \ << m_active_jointsSIN #define OUTPUT_SIGNALS m_tau_desSOUT \ << m_MSOUT \ << m_dv_desSOUT \ + << m_v_desSOUT \ + << m_q_desSOUT \ + << m_tau_pd_desSOUT \ << m_f_des_right_footSOUT \ << m_f_des_left_footSOUT \ << m_zmp_des_right_footSOUT \ @@ -166,12 +181,19 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_zmp_left_footSOUT \ << m_zmpSOUT \ << m_comSOUT \ + << m_com_estSOUT \ << m_com_velSOUT \ << m_com_accSOUT \ << m_com_acc_desSOUT \ + << m_am_dLSOUT \ + << m_am_dL_desSOUT \ << m_base_orientationSOUT \ << m_left_foot_posSOUT \ + << m_left_foot_pos_quatSOUT \ << m_right_foot_posSOUT \ + << m_right_foot_pos_quatSOUT \ + << m_lf_estSOUT \ + << m_rf_estSOUT \ << m_left_hand_posSOUT \ << m_right_hand_posSOUT \ << m_left_foot_velSOUT \ @@ -183,7 +205,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_left_hand_accSOUT \ << m_right_hand_accSOUT \ << m_right_foot_acc_desSOUT \ - << m_left_foot_acc_desSOUT + << m_left_foot_acc_desSOUT \ /// Define EntityClassName here rather than in the header file /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. @@ -203,6 +225,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(am_ref_L, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(am_ref_dL, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector), @@ -229,6 +253,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(kd_constraints, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_com, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_com, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kp_am, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kd_am, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_feet, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_feet, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_hands, dynamicgraph::Vector), @@ -237,7 +263,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kp_tau, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(w_com, double), + CONSTRUCT_SIGNAL_IN(w_am, double), CONSTRUCT_SIGNAL_IN(w_feet, double), CONSTRUCT_SIGNAL_IN(w_hands, double), CONSTRUCT_SIGNAL_IN(w_posture, double), @@ -259,16 +287,21 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(dq_max, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(ddq_max, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(dt_joint_pos_limits, double), - CONSTRUCT_SIGNAL_IN(tau_estimated, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(tau_measured, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(com_measured, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(wrench_base, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(ref_phase, int), CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector), CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS), CONSTRUCT_SIGNAL_OUT(M, dg::Matrix, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(dv_des, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(v_des, dg::Vector, m_dv_desSOUT), + CONSTRUCT_SIGNAL_OUT(q_des, dg::Vector, m_v_desSOUT), + CONSTRUCT_SIGNAL_OUT(tau_pd_des, dg::Vector, INPUT_SIGNALS << m_q_desSOUT), CONSTRUCT_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector, m_f_des_right_footSOUT), @@ -281,12 +314,19 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_OUT(zmp_left_foot, dg::Vector, m_wrench_left_footSIN), CONSTRUCT_SIGNAL_OUT(zmp, dg::Vector, m_wrench_left_footSIN << m_wrench_right_footSIN << m_zmp_left_footSOUT << m_zmp_right_footSOUT), CONSTRUCT_SIGNAL_OUT(com, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(com_est, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(com_vel, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(com_acc, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(com_acc_des, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(am_dL, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(am_dL_des, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(right_foot_pos_quat, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(left_foot_pos_quat, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(lf_est, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(rf_est, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_hand_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_hand_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_vel, dg::Vector, m_tau_desSOUT), @@ -308,7 +348,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st m_rightHandState(TASK_RIGHT_HAND_OFF), m_leftHandState(TASK_LEFT_HAND_OFF), m_timeLast(0), - m_robot_util(RefVoidRobotUtil()) { + m_robot_util(RefVoidRobotUtil()), + m_ctrlMode(CONTROL_OUTPUT_TORQUE) { RESETDEBUG5(); Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); @@ -321,6 +362,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st m_zmp_LF.setZero(); m_zmp.setZero(); m_com_offset.setZero(); + m_dcom_offset.setZero(); m_v_RF_int.setZero(); m_v_LF_int.setZero(); @@ -333,6 +375,11 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st makeCommandVoid0(*this, &InverseDynamicsBalanceController::updateComOffset, docCommandVoid0("Update the offset on the CoM based on the CoP measurement."))); + /* SET of control output type. */ + addCommand("setControlOutputType", + makeCommandVoid1(*this, &InverseDynamicsBalanceController::setControlOutputType, + docCommandVoid1("Set the type of control output.", + "Control type: velocity or torque (string)"))); addCommand("removeRightFootContact", makeCommandVoid1( *this, &InverseDynamicsBalanceController::removeRightFootContact, @@ -341,6 +388,14 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st addCommand("removeLeftFootContact", makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeLeftFootContact, docCommandVoid1("Remove the contact at the left foot.", "Transition time in seconds (double)"))); + addCommand("addRightFootContact", + makeCommandVoid1( + *this, &InverseDynamicsBalanceController::addRightFootContact, + docCommandVoid1("Add the contact at the right foot.", "Transition time in seconds (double)"))); + + addCommand("addLeftFootContact", makeCommandVoid1(*this, &InverseDynamicsBalanceController::addLeftFootContact, + docCommandVoid1("Add the contact at the left foot.", + "Transition time in seconds (double)"))); addCommand("addTaskRightHand", makeCommandVoid0(*this, &InverseDynamicsBalanceController::addTaskRightHand, docCommandVoid0("Adds an SE3 task for the right hand."))); addCommand("removeTaskRightHand", makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeTaskRightHand, @@ -360,6 +415,16 @@ void InverseDynamicsBalanceController::updateComOffset() { SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO); } +void InverseDynamicsBalanceController::setControlOutputType(const std::string& type) { + for (int i = 0; i < CONTROL_OUTPUT_SIZE; i++) + if (type == ControlOutput_s[i]) { + m_ctrlMode = (ControlOutput)i; + sotDEBUG(25) << "Control output type: " << ControlOutput_s[i] << endl; + return; + } + sotDEBUG(25) << "Unrecognized control output type: " << type << endl; +} + void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); @@ -370,12 +435,13 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran MSG_TYPE_ERROR); } const double& w_feet = m_w_feetSIN.accessCopy(); - m_invDyn->addMotionTask(*m_taskRF, w_feet, 1); + m_invDyn->addMotionTask(*m_taskRF, w_feet, 0); if (transitionTime > m_dt) { m_contactState = LEFT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; - } else + } else { m_contactState = LEFT_SUPPORT; + } } } @@ -389,12 +455,13 @@ void InverseDynamicsBalanceController::removeLeftFootContact(const double& trans MSG_TYPE_ERROR); } const double& w_feet = m_w_feetSIN.accessCopy(); - m_invDyn->addMotionTask(*m_taskLF, w_feet, 1); + m_invDyn->addMotionTask(*m_taskLF, w_feet, 0); if (transitionTime > m_dt) { m_contactState = RIGHT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; - } else + } else { m_contactState = RIGHT_SUPPORT; + } } } @@ -434,8 +501,18 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit if (m_contactState == LEFT_SUPPORT) { SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); const double& w_forces = m_w_forcesSIN.accessCopy(); - m_invDyn->addRigidContact(*m_contactRF, w_forces); + // TrajectorySample traj_ref = m_taskRF->getReference(); + pinocchio::SE3 ref; + // vectorToSE3(traj_ref.pos, ref); + ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); + m_contactRF->setReference(ref); m_invDyn->removeTask(m_taskRF->name(), transitionTime); + bool res = m_invDyn->addRigidContact(*m_contactRF, w_forces); + if (!res) { + const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); + SEND_MSG("Error while adding right foot contact." + tsid::solvers::HQPDataToString(hqpData, false), + MSG_TYPE_ERROR); + } m_contactState = DOUBLE_SUPPORT; } } @@ -444,8 +521,18 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti if (m_contactState == RIGHT_SUPPORT) { SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); const double& w_forces = m_w_forcesSIN.accessCopy(); - m_invDyn->addRigidContact(*m_contactLF, w_forces); + // TrajectorySample traj_ref = m_taskLF->getReference(); + pinocchio::SE3 ref; + // vectorToSE3(traj_ref.pos, ref); + ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); + m_contactLF->setReference(ref); m_invDyn->removeTask(m_taskLF->name(), transitionTime); + bool res = m_invDyn->addRigidContact(*m_contactLF, w_forces); + if (!res) { + const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); + SEND_MSG("Error while adding left foot contact." + tsid::solvers::HQPDataToString(hqpData, false), + MSG_TYPE_ERROR); + } m_contactState = DOUBLE_SUPPORT; } } @@ -485,25 +572,26 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& const dg::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0); const Eigen::Vector3d& kp_com = m_kp_comSIN(0); const Eigen::Vector3d& kd_com = m_kd_comSIN(0); + const Eigen::Vector3d& kp_am = m_kp_amSIN(0); + const Eigen::Vector3d& kd_am = m_kd_amSIN(0); const dg::sot::Vector6d& kd_hands = m_kd_handsSIN(0); const dg::sot::Vector6d& kp_hands = m_kp_handsSIN(0); const dg::sot::Vector6d& kp_feet = m_kp_feetSIN(0); const dg::sot::Vector6d& kd_feet = m_kd_feetSIN(0); const VectorN& kp_posture = m_kp_postureSIN(0); const VectorN& kd_posture = m_kd_postureSIN(0); - const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0); - const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0); + const dg::sot::Vector6d& kp_base_orientation = m_kp_base_orientationSIN(0); + const dg::sot::Vector6d& kd_base_orientation = m_kd_base_orientationSIN(0); assert(kp_posture.size() == static_cast(m_robot_util->m_nbJoints)); assert(kd_posture.size() == static_cast(m_robot_util->m_nbJoints)); - assert(rotor_inertias_sot.size() == static_cast(m_robot_util->m_nbJoints)); - assert(gear_ratios_sot.size() == static_cast(m_robot_util->m_nbJoints)); m_w_hands = m_w_handsSIN(0); m_w_com = m_w_comSIN(0); + m_w_am = m_w_amSIN(0); m_w_posture = m_w_postureSIN(0); + m_w_base_orientation = m_w_base_orientationSIN(0); const double& w_forces = m_w_forcesSIN(0); - // const double & w_base_orientation = m_w_base_orientationSIN(0); // const double & w_torques = m_w_torquesSIN(0); const double& mu = m_muSIN(0); const double& fMin = m_f_minSIN(0); @@ -517,13 +605,21 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& assert(m_robot->nv() >= 6); m_robot_util->m_nbJoints = m_robot->nv() - 6; - Vector rotor_inertias_urdf(rotor_inertias_sot.size()); - Vector gear_ratios_urdf(gear_ratios_sot.size()); - m_robot_util->joints_sot_to_urdf(rotor_inertias_sot, rotor_inertias_urdf); - m_robot_util->joints_sot_to_urdf(gear_ratios_sot, gear_ratios_urdf); - m_robot->rotor_inertias(rotor_inertias_urdf); - m_robot->gear_ratios(gear_ratios_urdf); - + if (m_rotor_inertiasSIN.isPlugged() && m_gear_ratiosSIN.isPlugged()){ + const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0); + const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0); + assert(rotor_inertias_sot.size() == static_cast(m_robot_util->m_nbJoints)); + assert(gear_ratios_sot.size() == static_cast(m_robot_util->m_nbJoints)); + Vector rotor_inertias_urdf(rotor_inertias_sot.size()); + Vector gear_ratios_urdf(gear_ratios_sot.size()); + m_robot_util->joints_sot_to_urdf(rotor_inertias_sot, rotor_inertias_urdf); + m_robot_util->joints_sot_to_urdf(gear_ratios_sot, gear_ratios_urdf); + m_robot->rotor_inertias(rotor_inertias_urdf); + m_robot->gear_ratios(gear_ratios_urdf); + } + + m_q_sot.setZero(m_robot->nv()); + m_v_sot.setZero(m_robot->nv()); m_dv_sot.setZero(m_robot->nv()); m_tau_sot.setZero(m_robot->nv() - 6); m_f.setZero(24); @@ -532,8 +628,11 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_J_RF.setZero(6, m_robot->nv()); m_J_LF.setZero(6, m_robot->nv()); + m_estim_data = pinocchio::Data(m_robot->model()); + m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot); + // CONTACT 6D TASKS m_contactRF = new Contact6d("contact_rfoot", *m_robot, m_robot_util->m_foot_util.m_Right_Foot_Frame_Name, contactPoints, contactNormal, mu, fMin, fMaxRF); m_contactRF->Kp(kp_contact); @@ -546,16 +645,37 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_contactLF->Kd(kd_contact); m_invDyn->addRigidContact(*m_contactLF, w_forces); - if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { - m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones()); - m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones()); - } + if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { + m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones()); + m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones()); + } + // TASK COM m_taskCom = new TaskComEquality("task-com", *m_robot); m_taskCom->Kp(kp_com); m_taskCom->Kd(kd_com); - m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1); - + m_invDyn->addMotionTask(*m_taskCom, m_w_com, 0); + + // TASK ANGULAR MOMENTUM + m_taskAM = new TaskAMEquality("task-am", *m_robot); + m_taskAM->Kp(kp_am); + m_taskAM->Kd(kd_am); + m_invDyn->addMotionTask(*m_taskAM, m_w_am, 1); + + // TASK BASE ORIENTATION + m_taskWaist = new TaskSE3Equality("task-waist", *m_robot, "root_joint"); + m_taskWaist->Kp(kp_base_orientation); + m_taskWaist->Kd(kd_base_orientation); + // Add a Mask to the task which will select the vector dimensions on which the task will act. + // In this case the waist configuration is a vector 6d (position and orientation -> SE3) + // Here we set a mask = [0 0 0 1 1 1] so the task on the waist will act on the orientation of the robot + Eigen::VectorXd mask_orientation(6); + mask_orientation << 0, 0, 0, 1, 1, 1; + m_taskWaist->setMask(mask_orientation); + // Add the task to the HQP with weight = 1.0, priority level = 1 (in the cost function) and a transition duration = 0.0 + m_invDyn->addMotionTask(*m_taskWaist, m_w_base_orientation, 1); + + // FEET TASKS (not added yet to the invDyn pb, only when contacts are removed) m_taskRF = new TaskSE3Equality("task-rf", *m_robot, m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); m_taskRF->Kp(kp_feet); m_taskRF->Kd(kd_feet); @@ -564,11 +684,13 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskLF->Kp(kp_feet); m_taskLF->Kd(kd_feet); + // POSTURE TASK m_taskPosture = new TaskJointPosture("task-posture", *m_robot); m_taskPosture->Kp(kp_posture); m_taskPosture->Kd(kd_posture); m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1); + // HANDS TASKS (not added yet to the invDyn pb, only when the command addTaskXHand is called) m_taskRH = new TaskSE3Equality("task-rh", *m_robot, m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_taskRH->Kp(kp_hands); m_taskRH->Kd(kd_hands); @@ -577,7 +699,10 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskLH->Kp(kp_hands); m_taskLH->Kd(kd_hands); + // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); + // m_sampleAM = TrajectorySample(3); + m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); m_sampleRH = TrajectorySample(3); @@ -587,6 +712,23 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_frame_id_rh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_frame_id_lh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); + // COM OFFSET + if (m_com_measuredSIN.isPlugged()){ + const VectorN6& q_robot = m_qSIN(0); + assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_robot = m_vSIN(0); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const dg::Vector& com_measured = m_com_measuredSIN(0); + assert(com_measured.size() == 3); + + m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf); + + pinocchio::Data data = pinocchio::Data(m_robot->model()); + pinocchio::centerOfMass(m_robot->model(), data, m_q_urdf, m_v_urdf); + m_com_offset = com_measured - data.com[0]; + } + m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog-fast"); m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn()); m_hqpSolver_60_36_34 = @@ -660,7 +802,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_contactState == DOUBLE_SUPPORT) { if (f_ref_left_foot.norm() < ZERO_FORCE_THRESHOLD) { removeLeftFootContact(0.0); - } else if (f_ref_right_foot.norm() < ZERO_FORCE_THRESHOLD) { + } else if (f_ref_right_foot.norm() < ZERO_FORCE_THRESHOLD) { removeRightFootContact(0.0); } } else if (m_contactState == LEFT_SUPPORT && f_ref_right_foot.norm() > ZERO_FORCE_THRESHOLD) { @@ -669,6 +811,22 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { addLeftFootContact(0.0); } } + // use reference phases (if plugged) to determine contact phase + if (m_ref_phaseSIN.isPlugged()) { + ContactState ref_phase = ContactState(m_ref_phaseSIN(iter)); + + if (m_contactState == DOUBLE_SUPPORT && ref_phase != DOUBLE_SUPPORT) { + if (ref_phase == LEFT_SUPPORT) { + removeRightFootContact(0.0); + } else { + removeLeftFootContact(0.0); + } + } else if (m_contactState == LEFT_SUPPORT && ref_phase == DOUBLE_SUPPORT) { + addRightFootContact(0.0); + } else if (m_contactState == RIGHT_SUPPORT && ref_phase == DOUBLE_SUPPORT) { + addLeftFootContact(0.0); + } + } if (m_contactState == RIGHT_SUPPORT_TRANSITION && m_t >= m_contactTransitionTime) { m_contactState = RIGHT_SUPPORT; @@ -691,9 +849,15 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { assert(q_sot.size() == static_cast(m_robot_util->m_nbJoints + 6)); const VectorN6& v_sot = m_vSIN(iter); assert(v_sot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const Vector3& x_com_ref = m_com_ref_posSIN(iter); const Vector3& dx_com_ref = m_com_ref_velSIN(iter); const Vector3& ddx_com_ref = m_com_ref_accSIN(iter); + const Vector3& L_am_ref = m_am_ref_LSIN(iter); + const Vector3& dL_am_ref = m_am_ref_dLSIN(iter); + const VectorN& x_waist_ref = m_base_orientation_ref_posSIN(iter); + const Vector6& dx_waist_ref = m_base_orientation_ref_velSIN(iter); + const Vector6& ddx_waist_ref = m_base_orientation_ref_accSIN(iter); const VectorN& q_ref = m_posture_ref_posSIN(iter); assert(q_ref.size() == static_cast(m_robot_util->m_nbJoints)); const VectorN& dq_ref = m_posture_ref_velSIN(iter); @@ -705,20 +869,40 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const Vector6& kd_contact = m_kd_constraintsSIN(iter); const Vector3& kp_com = m_kp_comSIN(iter); const Vector3& kd_com = m_kd_comSIN(iter); + const Vector3& kp_am = m_kp_amSIN(iter); + const Vector3& kd_am = m_kd_amSIN(iter); + const dg::sot::Vector6d& kp_base_orientation = m_kp_base_orientationSIN(0); + const dg::sot::Vector6d& kd_base_orientation = m_kd_base_orientationSIN(0); const VectorN& kp_posture = m_kp_postureSIN(iter); assert(kp_posture.size() == static_cast(m_robot_util->m_nbJoints)); const VectorN& kd_posture = m_kd_postureSIN(iter); assert(kd_posture.size() == static_cast(m_robot_util->m_nbJoints)); - const VectorN& kp_pos = m_kp_posSIN(iter); - assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); - const VectorN& kd_pos = m_kd_posSIN(iter); - assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); + // const VectorN& kp_pos = m_kp_posSIN(iter); + // assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); + // const VectorN& kd_pos = m_kd_posSIN(iter); + // assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); /*const double & w_hands = m_w_handsSIN(iter);*/ const double& w_com = m_w_comSIN(iter); + const double& w_am = m_w_amSIN(iter); const double& w_posture = m_w_postureSIN(iter); const double& w_forces = m_w_forcesSIN(iter); + const double & w_base_orientation = m_w_base_orientationSIN(iter); + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // BASE ESTIMATOR computation to have com and feet estimations + // Used to regulate on estimation (desired value of tasks are the estimations) + // -> only in velocity because close the TSID loop on itself (v_des, q_des) + // In torque close loop on the (q,v) of the base estimator (with freeflyer) -> not needed + Vector q_base_estimator, v_base_estimator; + q_base_estimator.setZero(m_robot->nq()); + v_base_estimator.setZero(m_robot->nv()); + m_robot_util->config_sot_to_urdf(q_sot, q_base_estimator); + m_robot_util->velocity_sot_to_urdf(q_base_estimator, v_sot, v_base_estimator); + pinocchio::computeAllTerms(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); + pinocchio::centerOfMass(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); + } if (m_contactState == LEFT_SUPPORT || m_contactState == LEFT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); @@ -726,25 +910,53 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); const Vector6& kp_feet = m_kp_feetSIN(iter); const Vector6& kd_feet = m_kd_feetSIN(iter); - m_sampleRF.pos = x_rf_ref; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // Right foot estimation + Eigen::Matrix rf_estim, rf_data; + pinocchio::SE3 rf_estim_se3, rf_data_se3; + m_robot->framePosition(m_estim_data, m_frame_id_rf, rf_estim_se3); + tsid::math::SE3ToVector(rf_estim_se3, rf_estim); + m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, rf_data_se3); + tsid::math::SE3ToVector(rf_data_se3, rf_data); + m_sampleRF.pos = x_rf_ref - rf_estim + rf_data; + } else { + m_sampleRF.pos = x_rf_ref; + } + m_sampleRF.vel = dx_rf_ref; m_sampleRF.acc = ddx_rf_ref; m_taskRF->setReference(m_sampleRF); m_taskRF->Kp(kp_feet); m_taskRF->Kd(kd_feet); + } else if (m_contactState == RIGHT_SUPPORT || m_contactState == RIGHT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_lf_ref = m_lf_ref_posSIN(iter); const Vector6& dx_lf_ref = m_lf_ref_velSIN(iter); const Vector6& ddx_lf_ref = m_lf_ref_accSIN(iter); const Vector6& kp_feet = m_kp_feetSIN(iter); const Vector6& kd_feet = m_kd_feetSIN(iter); - m_sampleLF.pos = x_lf_ref; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // Left foot estimation + Eigen::Matrix lf_estim, lf_data; + pinocchio::SE3 lf_estim_se3, lf_data_se3; + m_robot->framePosition(m_estim_data, m_frame_id_lf, lf_estim_se3); + tsid::math::SE3ToVector(lf_estim_se3, lf_estim); + m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, lf_data_se3); + tsid::math::SE3ToVector(lf_data_se3, lf_data); + m_sampleLF.pos = x_lf_ref - lf_estim + lf_data; + } else { + m_sampleLF.pos = x_lf_ref; + } + m_sampleLF.vel = dx_lf_ref; m_sampleLF.acc = ddx_lf_ref; m_taskLF->setReference(m_sampleLF); m_taskLF->Kp(kp_feet); m_taskLF->Kd(kd_feet); } + if (m_rightHandState == TASK_RIGHT_HAND_ON /*|| m_rightHandState == TASK_RIGHT_HAND_TRANSITION*/) { const Eigen::Matrix& x_rh_ref = m_rh_ref_posSIN(iter); const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter); @@ -775,10 +987,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); getProfiler().start(PROFILE_PREPARE_INV_DYN); - m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); - m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); - - m_sampleCom.pos = x_com_ref - m_com_offset; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // COM estimation + const Vector3& com = m_robot->com(m_invDyn->data()); + // const Vector3& dcom = m_robot->com_vel(m_invDyn->data()); + // m_dcom_offset = dcom - data.vcom[0]; + m_sampleCom.pos = x_com_ref - m_estim_data.com[0] + com; //- m_com_offset + } else { + m_sampleCom.pos = x_com_ref - m_com_offset; + } m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; m_taskCom->setReference(m_sampleCom); @@ -790,6 +1008,28 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskCom->name(), w_com); } + m_sampleAM.vel = L_am_ref; + m_sampleAM.acc = dL_am_ref; + m_taskAM->setReference(m_sampleAM); + m_taskAM->Kp(kp_am); + m_taskAM->Kd(kd_am); + if (m_w_am != w_am) { + // SEND_MSG("Change w_am from "+toString(m_w_am)+" to "+toString(w_am), MSG_TYPE_INFO); + m_w_am = w_am; + m_invDyn->updateTaskWeight(m_taskAM->name(), w_am); + } + + m_sampleWaist.pos = x_waist_ref; + m_sampleWaist.vel = dx_waist_ref; + m_sampleWaist.acc = ddx_waist_ref; + m_taskWaist->setReference(m_sampleWaist); + m_taskWaist->Kp(kp_base_orientation); + m_taskWaist->Kd(kd_base_orientation); + if (m_w_base_orientation != w_base_orientation) { + m_w_base_orientation = w_base_orientation; + m_invDyn->updateTaskWeight(m_taskWaist->name(), w_base_orientation); + } + m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos); m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel); m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc); @@ -825,6 +1065,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_firstTime) { m_firstTime = false; + m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); // m_robot->computeAllTerms(m_invDyn->data(), q, v); pinocchio::SE3 H_lf = m_robot->position( @@ -842,7 +1084,12 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { s = m_tau_sot; return s; } + } else if (m_ctrlMode == CONTROL_OUTPUT_TORQUE){ + // In velocity close the TSID loop on itself (v_des, q_des), in torque on the (q,v) of the robot. + m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); } + m_timeLast = static_cast(iter); const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -864,10 +1111,10 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().stop(PROFILE_HQP_SOLUTION); if (sol.status != HQP_STATUS_OPTIMAL) { - SEND_ERROR_STREAM_MSG("HQP solver failed to find a solution: " + toString(sol.status)); - SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData, false)); - SEND_DEBUG_STREAM_MSG("q=" + toString(q_sot.transpose(), 1, 5)); - SEND_DEBUG_STREAM_MSG("v=" + toString(v_sot.transpose(), 1, 5)); + SEND_ERROR_STREAM_MSG("HQP solver failed to find a solution at iter : "+ toString(iter) + "error " + toString(sol.status)); + SEND_ERROR_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData, false)); + SEND_ERROR_STREAM_MSG("q=" + toString(q_sot.transpose())); + SEND_ERROR_STREAM_MSG("v=" + toString(v_sot.transpose())); s.setZero(); return s; } @@ -884,8 +1131,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_invDyn->getContactForces(m_contactLF->name(), sol, tmp)) m_f_LF = m_contactLF->getForceGeneratorMatrix() * tmp; m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot); - m_tau_sot += kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) + - kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints)); + // m_tau_sot += kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) + + // kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints)); getProfiler().stop(PROFILE_TAU_DES_COMPUTATION); m_t += m_dt; @@ -917,6 +1164,69 @@ DEFINE_SIGNAL_OUT_FUNCTION(dv_des, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(v_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal v_des before initialization!"); + return s; + } + if (s.size() != m_robot->nv()) + s.resize(m_robot->nv()); + m_dv_desSOUT(iter); + tsid::math::Vector v_mean; + v_mean = m_v_urdf + 0.5 * m_dt * m_dv_urdf; + m_v_urdf = m_v_urdf + m_dt * m_dv_urdf; + m_q_urdf = pinocchio::integrate(m_robot->model(), m_q_urdf, m_dt * v_mean); + m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_v_urdf, m_v_sot); + s = m_v_sot; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(q_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal q_des before initialization!"); + return s; + } + if (s.size() != m_robot->nv()) + s.resize(m_robot->nv()); + m_v_desSOUT(iter); + m_robot_util->config_urdf_to_sot(m_q_urdf, m_q_sot); + s = m_q_sot; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal tau_pd_des before initialization!"); + return s; + } + if (s.size() != static_cast(m_robot_util->m_nbJoints)) + s.resize(m_robot_util->m_nbJoints); + + const VectorN& kp_pos = m_kp_posSIN(iter); + assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); + const VectorN& kd_pos = m_kd_posSIN(iter); + assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); + const VectorN& kp_tau = m_kp_tauSIN(iter); + assert(kp_tau.size() == static_cast(m_robot_util->m_nbJoints)); + + const VectorN6& q_robot = m_qSIN(iter); + assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_robot = m_vSIN(iter); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN& tau_measured = m_tau_measuredSIN(iter); + assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); + + m_q_desSOUT(iter); + + s = m_tau_sot + + kp_pos.cwiseProduct(m_q_sot.tail(m_robot_util->m_nbJoints) - q_robot.tail(m_robot_util->m_nbJoints)) + + kd_pos.cwiseProduct(m_v_sot.tail(m_robot_util->m_nbJoints) - v_robot.tail(m_robot_util->m_nbJoints)); + + s += kp_tau.cwiseProduct(tau_measured - s); + + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(f_des_right_foot, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal f_des_right_foot before initialization!"); @@ -969,6 +1279,28 @@ DEFINE_SIGNAL_OUT_FUNCTION(com_acc, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(am_dL_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal am_dL_des before initialization!"); + return s; + } + if (s.size() != 3) s.resize(3); + m_tau_desSOUT(iter); + s = m_taskAM->getDesiredMomentumDerivative(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(am_dL, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal am_dL before initialization!"); + return s; + } + if (s.size() != 3) s.resize(3); + m_tau_desSOUT(iter); + s = m_taskAM->getdMomentum(m_dv_urdf); + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_right_foot_local, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal zmp_des_right_foot_local before initialization!"); @@ -1171,6 +1503,32 @@ DEFINE_SIGNAL_OUT_FUNCTION(com, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(com_est, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal com_est before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + if (s.size() != 3) s.resize(3); + + const VectorN6& q_sot = m_qSIN(iter); + assert(q_sot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_sot = m_vSIN(iter); + assert(v_sot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + + Vector q_base_estimator, v_base_estimator; + q_base_estimator.setZero(m_robot->nq()); + v_base_estimator.setZero(m_robot->nv()); + m_robot_util->config_sot_to_urdf(q_sot, q_base_estimator); + m_robot_util->velocity_sot_to_urdf(q_base_estimator, v_sot, v_base_estimator); + + pinocchio::Data data = pinocchio::Data(m_robot->model()); + pinocchio::centerOfMass(m_robot->model(), data, q_base_estimator, v_base_estimator); + s = data.com[0]; + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(com_vel, dynamicgraph::Vector) { if (!m_initSucceeded) { std::ostringstream oss("Cannot compute signal com_vel before initialization! iter:"); @@ -1186,7 +1544,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(com_vel, dynamicgraph::Vector) { DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, dynamicgraph::Vector) { if (!m_initSucceeded) { - std::ostringstream oss("Cannot compute signal com_vel before initialization! iter:"); + std::ostringstream oss("Cannot compute signal base_orientation before initialization! iter:"); oss << iter; SEND_WARNING_STREAM_MSG(oss.str()); return s; @@ -1212,6 +1570,41 @@ DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos_quat, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal left_foot_pos before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + // m_tau_desSOUT(iter); + const dg::Vector& x_lf_ref = m_lf_ref_posSIN(iter); + pinocchio::SE3 oMi; + oMi.translation(x_lf_ref.head<3>() ); + oMi.rotation( Eigen::Map >(&x_lf_ref(3), 3, 3) ); + s.resize(7); + // m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, oMi); + tsid::math::SE3ToXYZQUAT(oMi, s); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(lf_est, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal lf_est before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + if (s.size() != 3) s.resize(3); + + m_tau_desSOUT(iter); + pinocchio::SE3 lf_se3; + s.resize(12); + m_robot->framePosition(m_estim_data, m_frame_id_lf, lf_se3); + tsid::math::SE3ToVector(lf_se3, s); + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(left_hand_pos, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal left hand_pos before initialization!"); @@ -1240,6 +1633,41 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos_quat, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal rigt_foot_pos before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + // m_tau_desSOUT(iter); + const dg::Vector& x_rf_ref = m_rf_ref_posSIN(iter); + pinocchio::SE3 oMi; + oMi.translation(x_rf_ref.head<3>() ); + oMi.rotation( Eigen::Map >(&x_rf_ref(3), 3, 3) ); + s.resize(7); + // m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, oMi); + tsid::math::SE3ToXYZQUAT(oMi, s); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(rf_est, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal rf_est before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + if (s.size() != 3) s.resize(3); + + m_tau_desSOUT(iter); + pinocchio::SE3 rf_se3; + s.resize(12); + m_robot->framePosition(m_estim_data, m_frame_id_rf, rf_se3); + tsid::math::SE3ToVector(rf_se3, s); + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(right_hand_pos, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal right_hand_pos before initialization!"); diff --git a/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index 6321fee..2f1e73d 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -25,6 +25,8 @@ #include #include +#include "pinocchio/algorithm/center-of-mass.hpp" +#include #include #include @@ -89,6 +91,8 @@ using namespace dg::sot; << m_w_postureSIN \ << m_kp_posSIN \ << m_kd_posSIN \ + << m_tau_measuredSIN \ + << m_kp_tauSIN \ << m_com_ref_posSIN \ << m_com_ref_velSIN \ << m_com_ref_accSIN \ @@ -111,9 +115,11 @@ using namespace dg::sot; << m_w_waistSIN \ << m_qSIN \ << m_vSIN \ + << m_com_measuredSIN \ << m_active_jointsSIN -#define OUTPUT_SIGNALS m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT +#define OUTPUT_SIGNALS m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT << m_comSOUT \ + << m_right_foot_posSOUT << m_left_foot_posSOUT /// Define EntityClassName here rather than in the header file @@ -142,6 +148,8 @@ SimpleInverseDyn(const std::string& name) , CONSTRUCT_SIGNAL_IN(w_posture, double) , CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(tau_measured, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(kp_tau, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector) @@ -164,6 +172,7 @@ SimpleInverseDyn(const std::string& name) , CONSTRUCT_SIGNAL_IN(w_waist, double) , CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(com_measured, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN) , CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS) @@ -171,6 +180,9 @@ SimpleInverseDyn(const std::string& name) , CONSTRUCT_SIGNAL_OUT(v_des, dg::Vector, m_dv_desSOUT) , CONSTRUCT_SIGNAL_OUT(q_des, dg::Vector, m_v_desSOUT) , CONSTRUCT_SIGNAL_OUT(u, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT << m_v_desSOUT << m_q_desSOUT) + , CONSTRUCT_SIGNAL_OUT(com, dg::Vector, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT) , m_t(0.0) , m_initSucceeded(false) , m_enabled(false) @@ -196,14 +208,15 @@ SimpleInverseDyn(const std::string& name) "Control type: velocity or torque (string)"))); addCommand("updateComOffset", - makeCommandVoid0(*this, &SimpleInverseDyn::updateComOffset, - docCommandVoid0("Update the offset on the CoM based on the CoP measurement."))); + makeCommandVoid1(*this, &SimpleInverseDyn::updateComOffset, + docCommandVoid1("Update the offset on the CoM based on its measurement.", + "Measured CoM"))); } -void SimpleInverseDyn::updateComOffset() { +void SimpleInverseDyn::updateComOffset(const dg::Vector& com_measured) { const Vector3 & com = m_robot->com(m_invDyn->data()); - m_com_offset = com; + m_com_offset = com_measured - com; SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO); } @@ -319,6 +332,28 @@ void SimpleInverseDyn::init(const double& dt, const std::string& robotRef) { m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); + m_frame_id_rf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); + m_frame_id_lf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name); + + // COM OFFSET + if (m_com_measuredSIN.isPlugged()){ + const VectorN6& q_robot = m_qSIN(0); + assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_robot = m_vSIN(0); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const dg::Vector& com_measured = m_com_measuredSIN(0); + assert(com_measured.size() == 3); + SEND_MSG("COM_measured: " + toString(com_measured), MSG_TYPE_ERROR); + + m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf); + + pinocchio::Data data = pinocchio::Data(m_robot->model()); + pinocchio::centerOfMass(m_robot->model(), data, m_q_urdf, m_v_urdf); + m_com_offset = com_measured - data.com[0]; + SEND_MSG("Update COM: " + toString(m_com_offset), MSG_TYPE_ERROR); + } + m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog-fast"); m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn()); @@ -425,6 +460,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().start(PROFILE_PREPARE_INV_DYN); + // Update tasks m_sampleCom.pos = x_com_ref - m_com_offset; m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; @@ -472,21 +508,21 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces); if (m_firstTime) { - m_firstTime = false; + m_firstTime = false; m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf); m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); - // m_robot->computeAllTerms(m_invDyn->data(), q, v); + pinocchio::SE3 H_lf = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); m_contactLF->setReference(H_lf); - SEND_MSG("Setting left foot reference to " + toString(H_lf), MSG_TYPE_DEBUG); + SEND_MSG("Setting left foot reference to " + toString(H_lf), MSG_TYPE_INFO); pinocchio::SE3 H_rf = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); m_contactRF->setReference(H_rf); - SEND_MSG("Setting right foot reference to " + toString(H_rf), MSG_TYPE_DEBUG); + SEND_MSG("Setting right foot reference to " + toString(H_rf), MSG_TYPE_INFO); } else if (m_timeLast != static_cast(iter - 1)) { SEND_MSG("Last time " + toString(m_timeLast) + " is not current time-1: " + toString(iter), MSG_TYPE_ERROR); if (m_timeLast == static_cast(iter)) { @@ -494,11 +530,12 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { return s; } } - // else if (m_ctrlMode == CONTROL_OUTPUT_TORQUE){ - // // In velocity close the TSID loop on itself (v_des, q_des), in torque on the (q,v) of the robot. - // m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); - // m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf); - // } + else if (m_ctrlMode == CONTROL_OUTPUT_TORQUE){ + // In velocity close the TSID loop on itself (v_des, q_des), in torque on the (q,v) of the robot. + m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf); + } + m_timeLast = static_cast(iter); const HQPData & hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -513,10 +550,10 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().stop(PROFILE_HQP_SOLUTION); if (sol.status != HQP_STATUS_OPTIMAL) { - SEND_ERROR_STREAM_MSG("HQP solver failed to find a solution: " + toString(sol.status)); - SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData, false)); - SEND_DEBUG_STREAM_MSG("q=" + toString(q_robot.transpose(), 1, 5)); - SEND_DEBUG_STREAM_MSG("v=" + toString(v_robot.transpose(), 1, 5)); + SEND_ERROR_STREAM_MSG("HQP solver failed to find a solution at iter " + toString(iter) + " : "+ toString(sol.status)); + SEND_ERROR_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData, false)); + SEND_ERROR_STREAM_MSG("q=" + toString(q_robot)); + SEND_ERROR_STREAM_MSG("v=" + toString(v_robot)); s.setZero(); return s; } @@ -590,17 +627,69 @@ DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) { assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); const VectorN& kd_pos = m_kd_posSIN(iter); assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); + const VectorN& kp_tau = m_kp_tauSIN(iter); + assert(kp_tau.size() == static_cast(m_robot_util->m_nbJoints)); const VectorN6& q_robot = m_qSIN(iter); assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); const VectorN6& v_robot = m_vSIN(iter); assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& tau_measured = m_tau_measuredSIN(iter); + assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); m_q_desSOUT(iter); - s = m_tau_sot + kp_pos.cwiseProduct(m_q_sot.tail(m_robot_util->m_nbJoints) - q_robot.tail(m_robot_util->m_nbJoints)) + + s = m_tau_sot + + kp_pos.cwiseProduct(m_q_sot.tail(m_robot_util->m_nbJoints) - q_robot.tail(m_robot_util->m_nbJoints)) + kd_pos.cwiseProduct(m_v_sot.tail(m_robot_util->m_nbJoints) - v_robot.tail(m_robot_util->m_nbJoints)); + s += kp_tau.cwiseProduct(tau_measured - s); + + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(com, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal com before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + if (s.size() != 3) s.resize(3); + const Vector3& com = m_robot->com(m_invDyn->data()); + s = com + m_com_offset; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal left_foot_pos before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + m_tau_desSOUT(iter); + pinocchio::SE3 oMi; + s.resize(7); + m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, oMi); + tsid::math::SE3ToXYZQUAT(oMi, s); + // tsid::math::SE3ToVector(oMi, s); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal rigt_foot_pos before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + m_tau_desSOUT(iter); + pinocchio::SE3 oMi; + s.resize(7); + m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, oMi); + tsid::math::SE3ToXYZQUAT(oMi, s); + // tsid::math::SE3ToVector(oMi, s); return s; } diff --git a/tests/test-control-manager.cpp b/tests/test-control-manager.cpp index fd7ea57..acae3cf 100644 --- a/tests/test-control-manager.cpp +++ b/tests/test-control-manager.cpp @@ -40,5 +40,5 @@ BOOST_AUTO_TEST_CASE(testControlManager) { dynamicgraph::Vector av; a_control_manager.m_uSOUT.needUpdate(6); - RealTimeLogger::destroy(); + dynamicgraph::RealTimeLogger::destroy(); }