diff --git a/CMakeLists.txt b/CMakeLists.txt index 11373d9..e94b65d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -79,6 +79,8 @@ SET(${PROJECT_NAME}_HEADERS include/${CUSTOM_HEADER_DIR}/se3-trajectory-generator.hh include/${CUSTOM_HEADER_DIR}/free-flyer-locator.hh include/${CUSTOM_HEADER_DIR}/inverse-dynamics-balance-controller.hh + include/${CUSTOM_HEADER_DIR}/simple-inverse-dyn.hh + include/${CUSTOM_HEADER_DIR}/posture-task.hh include/${CUSTOM_HEADER_DIR}/position-controller.hh include/${CUSTOM_HEADER_DIR}/control-manager.hh include/${CUSTOM_HEADER_DIR}/current-controller.hh diff --git a/doc/Overview.md b/doc/Overview.md 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..9fd7882 100644 --- a/include/sot/torque_control/admittance-controller.hh +++ b/include/sot/torque_control/admittance-controller.hh @@ -24,17 +24,17 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -#include -#include "boost/assign.hpp" - +#include +#include #include #include /* HELPER */ #include #include -#include #include +#include +#include "boost/assign.hpp" namespace dynamicgraph { namespace sot { @@ -90,10 +90,6 @@ class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController : public ::dynamicgrap /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: Eigen::VectorXd m_u; /// control (i.e. motor currents) bool m_firstIter; diff --git a/include/sot/torque_control/base-estimator.hh b/include/sot/torque_control/base-estimator.hh index d27505c..458b769 100644 --- a/include/sot/torque_control/base-estimator.hh +++ b/include/sot/torque_control/base-estimator.hh @@ -23,7 +23,8 @@ /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ - +#include +#include #include #include "boost/assign.hpp" //#include @@ -37,7 +38,6 @@ /* HELPER */ #include #include -#include #include namespace dynamicgraph { @@ -160,10 +160,6 @@ class SOTBASEESTIMATOR_EXPORT BaseEstimator : public ::dynamicgraph::Entity { /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: bool m_initSucceeded; /// true if the entity has been successfully initialized bool m_reset_foot_pos; /// true after the command resetFootPositions is called diff --git a/include/sot/torque_control/control-manager.hh b/include/sot/torque_control/control-manager.hh index 058555d..d32d1ad 100644 --- a/include/sot/torque_control/control-manager.hh +++ b/include/sot/torque_control/control-manager.hh @@ -24,16 +24,19 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include +#include +#include + #include #include "boost/assign.hpp" #include #include -#include + #include #include -#include #include namespace dynamicgraph { @@ -130,10 +133,6 @@ class SOTCONTROLMANAGER_EXPORT ControlManager : public ::dynamicgraph::Entity { /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[ControlManager-" + name + "] " + msg) << '\n'; - } - protected: RobotUtilShrPtr m_robot_util; tsid::robots::RobotWrapper* m_robot; diff --git a/include/sot/torque_control/current-controller.hh b/include/sot/torque_control/current-controller.hh index 4995e8e..a11f842 100644 --- a/include/sot/torque_control/current-controller.hh +++ b/include/sot/torque_control/current-controller.hh @@ -27,15 +27,16 @@ #include #include "boost/assign.hpp" -#include -#include - +#include +#include #include #include #include -#include #include +#include +#include + namespace dynamicgraph { namespace sot { namespace torque_control { @@ -106,10 +107,6 @@ class SOTCURRENTCONTROLLER_EXPORT CurrentController : public ::dynamicgraph::Ent /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[CurrentController-" + name + "] " + msg) << '\n'; - } - protected: RobotUtilShrPtr m_robot_util; bool m_initSucceeded; /// true if the entity has been successfully initialized diff --git a/include/sot/torque_control/ddp-actuator-solver.hh b/include/sot/torque_control/ddp-actuator-solver.hh index abef635..38db951 100644 --- a/include/sot/torque_control/ddp-actuator-solver.hh +++ b/include/sot/torque_control/ddp-actuator-solver.hh @@ -15,10 +15,10 @@ #define SOTDDPACTUATORSOLVER_EXPORT #endif +#include #include #include #include -#include #include #include diff --git a/include/sot/torque_control/ddp_pyrene_actuator_solver.hh b/include/sot/torque_control/ddp_pyrene_actuator_solver.hh index 5f8ee1b..cb82b28 100644 --- a/include/sot/torque_control/ddp_pyrene_actuator_solver.hh +++ b/include/sot/torque_control/ddp_pyrene_actuator_solver.hh @@ -28,11 +28,11 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include #include #include #include #include -#include #include #include diff --git a/include/sot/torque_control/device-torque-ctrl.hh b/include/sot/torque_control/device-torque-ctrl.hh index 5b72328..1d5abb7 100644 --- a/include/sot/torque_control/device-torque-ctrl.hh +++ b/include/sot/torque_control/device-torque-ctrl.hh @@ -12,6 +12,8 @@ #include #include +#include +#include #include #include @@ -24,7 +26,6 @@ /* HELPER */ #include -#include namespace dgsot = dynamicgraph::sot; @@ -75,10 +76,6 @@ class DeviceTorqueCtrl : public dgsot::Device { virtual void integrate(const double& dt); void computeForwardDynamics(); - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[DeviceTorqueCtrl] " + msg) << '\n'; - } - /// \brief Current integration step. double timestep_; bool m_initSucceeded; diff --git a/include/sot/torque_control/free-flyer-locator.hh b/include/sot/torque_control/free-flyer-locator.hh index 083c7fb..677f419 100644 --- a/include/sot/torque_control/free-flyer-locator.hh +++ b/include/sot/torque_control/free-flyer-locator.hh @@ -23,9 +23,11 @@ /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ + #include +#include #include "boost/assign.hpp" - +#include /* Pinocchio */ #include #include @@ -37,7 +39,6 @@ /* HELPER */ #include #include -#include #include namespace dynamicgraph { @@ -78,10 +79,6 @@ class SOTFREEFLYERLOCATOR_EXPORT FreeFlyerLocator : public ::dynamicgraph::Entit /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[FreeFlyerLocator-" + name + "] " + msg) << '\n'; - } - protected: bool m_initSucceeded; /// true if the entity has been successfully initialized pinocchio::Model* m_model; /// Pinocchio robot model diff --git a/include/sot/torque_control/imu_offset_compensation.hh b/include/sot/torque_control/imu_offset_compensation.hh index e9c8414..eb59243 100644 --- a/include/sot/torque_control/imu_offset_compensation.hh +++ b/include/sot/torque_control/imu_offset_compensation.hh @@ -70,9 +70,6 @@ class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation : public ::dynamicgr /* --- METHODS --- */ void update_offset_impl(int iter); - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) { - logger_.stream(t) << ("[ImuOffsetCompensation-" + name + "] " + msg, t, file, line); - } protected: bool m_initSucceeded; /// true if the entity has been successfully initialized diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh old mode 100644 new mode 100755 index 0f3a68f..101aeff --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -24,25 +24,37 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -#include -#include "boost/assign.hpp" +#include +#include +#include /* Pinocchio */ #include #include +#include "pinocchio/algorithm/joint-configuration.hpp" + +#include +#include "boost/assign.hpp" -#include #include #include +#include #include #include +#include #include +#include +#include +// #include "tsid/tasks/task-angular-momentum-integral-equality.hpp" + +#include "tsid/tasks/task-contact-force-equality.hpp" +#include +#include #include /* HELPER */ #include #include -#include #include @@ -50,6 +62,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 ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ @@ -65,7 +81,9 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle InverseDynamicsBalanceController(const std::string& name); void init(const double& dt, const std::string& robotRef); + dynamicgraph::Vector actFrame(pinocchio::SE3 frame, dynamicgraph::Vector vec); void updateComOffset(); + virtual void setControlOutputType(const std::string& type); void removeRightFootContact(const double& transitionTime); void removeLeftFootContact(const double& transitionTime); void addRightFootContact(const double& transitionTime); @@ -74,11 +92,21 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle void removeTaskRightHand(const double& transitionTime); void addTaskLeftHand(/*const double& transitionTime*/); void removeTaskLeftHand(const double& transitionTime); + void addTaskLeftHandContact(/*const double& transitionTime*/); + void removeTaskLeftHandContact(const double& transitionTime); + void addTaskEnergy(const double& transitionTime); + void removeTaskEnergy(const double& transitionTime); + void setEnergyTank(const double& tankValue); /* --- SIGNALS --- */ 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(com_adm_ref_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_adm_ref_vel, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_adm_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); @@ -99,6 +127,10 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector); DECLARE_SIGNAL_IN(f_ref_right_foot, dynamicgraph::Vector); DECLARE_SIGNAL_IN(f_ref_left_foot, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(f_ref_left_arm, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(df_ref_left_arm, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(f_ext_left_arm, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(df_ext_left_arm, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector); @@ -106,16 +138,25 @@ 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); - DECLARE_SIGNAL_IN(kd_hands, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kd_hands, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_hands_force, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kd_hands_force, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(ki_hands_force, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector); 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(kff_tau, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kff_dq, 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 +181,25 @@ 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_IN(ref_pos_final, dynamicgraph::Vector); 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 +212,23 @@ 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(dcm, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(am_L, 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(right_foot_pos_ref_quat, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(left_foot_pos_quat, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(left_foot_pos_ref_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); @@ -182,6 +241,18 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); + DECLARE_SIGNAL_OUT(LH_force_world, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(energy, double); + DECLARE_SIGNAL_OUT(energy_derivative, double); + DECLARE_SIGNAL_OUT(energy_tank, double); + DECLARE_SIGNAL_OUT(denergy_tank, double); + DECLARE_SIGNAL_OUT(energy_bound, double); + DECLARE_SIGNAL_OUT(task_energy_alpha, double); + DECLARE_SIGNAL_OUT(task_energy_beta, double); + DECLARE_SIGNAL_OUT(task_energy_gamma, double); + DECLARE_SIGNAL_OUT(task_energy_S, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(task_energy_dS, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(task_energy_A, double); /// This signal copies active_joints only if it changes from a all false or to an all false value DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector); @@ -190,16 +261,13 @@ 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; bool m_initSucceeded; /// true if the entity has been successfully initialized bool m_enabled; /// True if controler is enabled bool m_firstTime; /// True at the first iteration of the controller + bool m_taskLHContactOn; enum ContactState { DOUBLE_SUPPORT = 0, @@ -243,30 +311,57 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::contacts::Contact6d* m_contactRH; tsid::contacts::Contact6d* m_contactLH; tsid::tasks::TaskComEquality* m_taskCom; + tsid::tasks::TaskComEquality* m_taskComAdm; + 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; tsid::tasks::TaskSE3Equality* m_taskLH; tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; + tsid::tasks::TaskActuationBounds* m_taskActBounds; + tsid::tasks::TaskContactForceEquality* m_taskForceLH; + tsid::tasks::TaskJointBounds* m_taskJointBounds; + tsid::tasks::TaskCopEquality* m_taskCoP; + tsid::tasks::TaskEnergy* m_taskEnergy; tsid::trajectories::TrajectorySample m_sampleCom; + tsid::trajectories::TrajectorySample m_sampleComAdm; + 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; + tsid::trajectories::TrajectorySample m_sampleLHForceRef; + tsid::trajectories::TrajectorySample m_sampleLHForceExt; + tsid::trajectories::TrajectorySample m_sampleEnergy; double m_w_com; + double m_w_am; double m_w_posture; double m_w_hands; + double m_w_base_orientation; + double m_omega; /// sqrt(g/h) + pinocchio::SE3 m_transformFrameFeet; + pinocchio::SE3 m_transformFrameCom; + + 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::Vector m_JF; /// desired Jacobian_contacts * force coefficients (size m_v) 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 +370,13 @@ 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; + double m_previous_energy; + tsid::math::Vector m_previous_vel; + tsid::math::Vector m_previous_q; + 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 +384,10 @@ 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) + bool m_energyTask_enabled; /// Boolean to add/remove energyTask }; // 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 8fea238..d039a8d 100644 --- a/include/sot/torque_control/joint-torque-controller.hh +++ b/include/sot/torque_control/joint-torque-controller.hh @@ -32,8 +32,8 @@ /* HELPER */ #include #include -#include #include +#include /*Motor model*/ #include @@ -115,10 +115,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 59a13e9..a83069a 100644 --- a/include/sot/torque_control/joint-trajectory-generator.hh +++ b/include/sot/torque_control/joint-trajectory-generator.hh @@ -23,6 +23,7 @@ /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ + #include #include @@ -32,6 +33,7 @@ /* HELPER */ #include #include +#include #include #include @@ -150,10 +152,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 28138bf..dbedbf5 100644 --- a/include/sot/torque_control/position-controller.hh +++ b/include/sot/torque_control/position-controller.hh @@ -24,6 +24,7 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include #include #include @@ -32,7 +33,6 @@ #include #include -#include #include @@ -75,10 +75,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/posture-task.hh b/include/sot/torque_control/posture-task.hh new file mode 100644 index 0000000..77fbea7 --- /dev/null +++ b/include/sot/torque_control/posture-task.hh @@ -0,0 +1,203 @@ +/* + * Copyright 2021, Noëlie Ramuzat, LAAS-CNRS + * + * This file is part of sot-torque-control. + * sot-torque-control is free software: you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 of + * the License, or (at your option) any later version. + * sot-torque-control is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty + * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. You should + * have received a copy of the GNU Lesser General Public License along + * with sot-torque-control. If not, see . + */ + +#ifndef __sot_torque_control_posture_task_H__ +#define __sot_torque_control_posture_task_H__ + +/* --------------------------------------------------------------------- */ +/* --- API ------------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +#if defined(WIN32) +#if defined(posture_task_EXPORTS) +#define SOTPOSTURETASK_EXPORT __declspec(dllexport) +#else +#define SOTPOSTURETASK_EXPORT __declspec(dllimport) +#endif +#else +#define SOTPOSTURETASK_EXPORT +#endif + +/* --------------------------------------------------------------------- */ +/* --- INCLUDE --------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +#include +#include +#include + +/* Pinocchio */ +#include +#include +#include + +#include +#include "boost/assign.hpp" + +#include +#include +#include +#include +#include +#include +#include + +/* HELPER */ +#include +#include + +#include + +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 ----------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { + typedef PostureTask EntityClassName; + DYNAMIC_GRAPH_ENTITY_DECL(); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /* --- CONSTRUCTOR ---- */ + PostureTask(const std::string& name); + + /* --- SIGNALS --- */ + + DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(w_posture, double); + + DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(w_forces, double); + DECLARE_SIGNAL_IN(mu, double); + DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix); + DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(f_min, double); + DECLARE_SIGNAL_IN(f_max_right_foot, double); + DECLARE_SIGNAL_IN(f_max_left_foot, double); + + DECLARE_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(w_base_orientation, double); + + 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); + + DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); + + DECLARE_SIGNAL_OUT(energy, double); + DECLARE_SIGNAL_OUT(energy_derivative, double); + DECLARE_SIGNAL_OUT(energy_tank, double); + DECLARE_SIGNAL_OUT(denergy_tank, double); + DECLARE_SIGNAL_OUT(energy_bound, double); + DECLARE_SIGNAL_OUT(task_energy_alpha, double); + DECLARE_SIGNAL_OUT(task_energy_beta, double); + DECLARE_SIGNAL_OUT(task_energy_gamma, double); + DECLARE_SIGNAL_OUT(task_energy_S, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(task_energy_dS, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(task_energy_A, double); + DECLARE_SIGNAL_OUT(base_orientation, double); + + /* --- COMMANDS --- */ + + void init(const double& dt, const std::string& robotRef); + virtual void setControlOutputType(const std::string& type); + void updateComOffset(const dynamicgraph::Vector& com_measured); + + /* --- ENTITY INHERITANCE --- */ + virtual void display(std::ostream& os) const; + + protected: + double m_dt; /// control loop time period + double m_t; /// current time + bool m_initSucceeded; /// true if the entity has been successfully initialized + 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; + + /// Solver and problem formulation + tsid::solvers::SolverHQPBase* m_hqpSolver; + tsid::InverseDynamicsFormulationAccForce* m_invDyn; + tsid::contacts::Contact6d* m_contactRF; + tsid::contacts::Contact6d* m_contactLF; + + /// TASKS + tsid::tasks::TaskJointPosture* m_taskPosture; + tsid::tasks::TaskJointPosture* m_taskBlockedJoints; + tsid::tasks::TaskActuationBounds* m_taskActBounds; + tsid::tasks::TaskEnergy* m_taskEnergy; + tsid::tasks::TaskSE3Equality* m_taskWaist; + + /// Trajectories of the tasks + tsid::trajectories::TrajectorySample m_samplePosture; + tsid::trajectories::TrajectorySample m_sampleWaist; + + /// Weights of the Tasks (which can be changed) + double m_w_posture; + double m_w_base_orientation; + + /// Computed solutions (accelerations and torques) and their derivatives + 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::Vector3 m_com_offset; /// 3d CoM offset + + 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 PostureTask +} // namespace torque_control +} // namespace sot +} // namespace dynamicgraph + +#endif // #ifndef __sot_torque_control_posture_task_H__ 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..e7ffb7a 100644 --- a/include/sot/torque_control/simple-inverse-dyn.hh +++ b/include/sot/torque_control/simple-inverse-dyn.hh @@ -35,26 +35,29 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -#include -#include "boost/assign.hpp" +#include +#include +#include /* Pinocchio */ #include #include -#include "pinocchio/algorithm/joint-configuration.hpp" +#include + +#include +#include "boost/assign.hpp" -#include #include #include #include #include #include +#include #include /* HELPER */ #include #include -#include #include @@ -92,6 +95,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 +123,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 +132,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 +152,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; @@ -161,6 +170,7 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit tsid::tasks::TaskSE3Equality* m_taskWaist; tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; + tsid::tasks::TaskActuationBounds* m_taskActBounds; /// Trajectories of the tasks tsid::trajectories::TrajectorySample m_sampleCom; diff --git a/include/sot/torque_control/torque-offset-estimator.hh b/include/sot/torque_control/torque-offset-estimator.hh index addce40..5855251 100644 --- a/include/sot/torque_control/torque-offset-estimator.hh +++ b/include/sot/torque_control/torque-offset-estimator.hh @@ -25,7 +25,8 @@ /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ - +#include +#include #include #include @@ -38,7 +39,6 @@ /* HELPER */ #include #include -#include #include /*Motor model*/ @@ -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 e3fe01d..381d3e5 100644 --- a/include/sot/torque_control/trace-player.hh +++ b/include/sot/torque_control/trace-player.hh @@ -79,10 +79,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/src/CMakeLists.txt b/src/CMakeLists.txt index 4e170ae..d45db14 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -11,6 +11,7 @@ SET(plugins imu_offset_compensation inverse-dynamics-balance-controller simple-inverse-dyn + posture-task joint-torque-controller joint-trajectory-generator numerical-difference @@ -18,13 +19,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}.cpp") diff --git a/src/base-estimator.cpp b/src/base-estimator.cpp index bc959ed..6814747 100644 --- a/src/base-estimator.cpp +++ b/src/base-estimator.cpp @@ -3,11 +3,11 @@ * */ +#include #include "pinocchio/algorithm/frames.hpp" #include #include -#include #include #include diff --git a/src/control-manager.cpp b/src/control-manager.cpp index 280e13b..935a76a 100644 --- a/src/control-manager.cpp +++ b/src/control-manager.cpp @@ -3,13 +3,12 @@ * */ -#include +#include #include #include #include #include -#include #include using namespace tsid; diff --git a/src/device-torque-ctrl.cpp b/src/device-torque-ctrl.cpp index 968585c..eaf5dc2 100644 --- a/src/device-torque-ctrl.cpp +++ b/src/device-torque-ctrl.cpp @@ -6,6 +6,7 @@ #include #include +#include "sot/torque_control/device-torque-ctrl.hh" #include // integrate #include #include @@ -14,8 +15,6 @@ #include #include -#include "sot/torque_control/device-torque-ctrl.hh" - using namespace std; using namespace dynamicgraph; using namespace sot::torque_control; @@ -187,13 +186,17 @@ void DeviceTorqueCtrl::setState(const dynamicgraph::Vector& q) { pinocchio::SE3 H_lf = m_robot->position(*m_data, m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); tsid::trajectories::TrajectorySample s(12, 6); - tsid::math::SE3ToVector(H_lf, s.pos); + Vector vec_H_lf; + tsid::math::SE3ToVector(H_lf, vec_H_lf); + s.setValue(vec_H_lf); m_contactLF->setReference(s); SEND_MSG("Setting left foot reference to " + toString(H_lf), MSG_TYPE_DEBUG); pinocchio::SE3 H_rf = m_robot->position(*m_data, m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); - tsid::math::SE3ToVector(H_rf, s.pos); + Vector vec_H_rf; + tsid::math::SE3ToVector(H_rf, vec_H_rf); + s.setValue(vec_H_rf); m_contactRF->setReference(s); SEND_MSG("Setting right foot reference to " + toString(H_rf), MSG_TYPE_DEBUG); setVelocity(m_v_sot); diff --git a/src/free-flyer-locator.cpp b/src/free-flyer-locator.cpp index fbb2b9d..d57f2e8 100644 --- a/src/free-flyer-locator.cpp +++ b/src/free-flyer-locator.cpp @@ -3,11 +3,11 @@ * */ +#include #include "pinocchio/algorithm/frames.hpp" #include #include -#include #include #include diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 018dd15..093302f 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -5,7 +5,11 @@ #define EIGEN_RUNTIME_NO_MALLOC -#include +#include + +#include +#include +#include #include #include @@ -20,7 +24,6 @@ #include #include -#include #if DEBUG #define ODEBUG(x) std::cout << x << std::endl @@ -82,11 +85,16 @@ 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_com_adm_ref_posSIN \ + << m_com_adm_ref_velSIN \ + << m_com_adm_ref_accSIN \ + << m_am_ref_LSIN \ + << m_am_ref_dLSIN \ << m_rf_ref_posSIN \ << m_rf_ref_velSIN \ << m_rf_ref_accSIN \ @@ -107,21 +115,34 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_base_orientation_ref_accSIN \ << m_f_ref_right_footSIN \ << m_f_ref_left_footSIN \ + << m_f_ref_left_armSIN \ + << m_df_ref_left_armSIN \ + << m_f_ext_left_armSIN \ + << m_df_ext_left_armSIN \ << m_kp_base_orientationSIN \ << m_kd_base_orientationSIN \ << m_kp_constraintsSIN \ << m_kd_constraintsSIN \ << m_kp_comSIN \ << m_kd_comSIN \ + << m_kp_amSIN \ + << m_kd_amSIN \ << m_kp_feetSIN \ << m_kd_feetSIN \ << m_kp_handsSIN \ << m_kd_handsSIN \ + << m_kp_hands_forceSIN \ + << m_kd_hands_forceSIN \ + << m_ki_hands_forceSIN \ << m_kp_postureSIN \ << m_kd_postureSIN \ << m_kp_posSIN \ << m_kd_posSIN \ + << m_kp_tauSIN \ + << m_kff_tauSIN \ + << m_kff_dqSIN \ << m_w_comSIN \ + << m_w_amSIN \ << m_w_feetSIN \ << m_w_handsSIN \ << m_w_postureSIN \ @@ -143,17 +164,23 @@ 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_active_jointsSIN + << m_ref_phaseSIN \ + << m_active_jointsSIN \ + // << m_ref_pos_finalSIN #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 +193,23 @@ 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_dcmSOUT \ + << m_am_LSOUT \ + << m_am_dLSOUT \ + << m_am_dL_desSOUT \ << m_base_orientationSOUT \ << m_left_foot_posSOUT \ + << m_left_foot_pos_quatSOUT \ + << m_left_foot_pos_ref_quatSOUT \ << m_right_foot_posSOUT \ + << m_right_foot_pos_quatSOUT \ + << m_right_foot_pos_ref_quatSOUT \ + << m_lf_estSOUT \ + << m_rf_estSOUT \ << m_left_hand_posSOUT \ << m_right_hand_posSOUT \ << m_left_foot_velSOUT \ @@ -183,7 +221,19 @@ 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 \ + << m_LH_force_worldSOUT \ + << m_energySOUT \ + << m_energy_derivativeSOUT \ + << m_energy_tankSOUT \ + << m_denergy_tankSOUT \ + << m_energy_boundSOUT \ + << m_task_energy_alphaSOUT \ + << m_task_energy_betaSOUT \ + << m_task_energy_gammaSOUT \ + << m_task_energy_SSOUT \ + << m_task_energy_dSSOUT \ + << m_task_energy_ASOUT /// Define EntityClassName here rather than in the header file /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. @@ -203,6 +253,11 @@ 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(com_adm_ref_pos, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(com_adm_ref_vel, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(com_adm_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), @@ -223,21 +278,34 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(f_ref_right_foot, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(f_ref_left_foot, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(f_ref_left_arm, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(df_ref_left_arm, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(f_ext_left_arm, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(df_ext_left_arm, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_constraints, dynamicgraph::Vector), 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), CONSTRUCT_SIGNAL_IN(kd_hands, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kp_hands_force, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kd_hands_force, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(ki_hands_force, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_posture, dynamicgraph::Vector), 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(kff_tau, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kff_dq, 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 +327,22 @@ 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(active_joints, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(ref_phase, int), + CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector), + // CONSTRUCT_SIGNAL_IN(ref_pos_final, 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 +355,23 @@ 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(dcm, dg::Vector, m_tau_desSOUT << m_comSOUT << m_com_velSOUT), + CONSTRUCT_SIGNAL_OUT(am_L, 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(right_foot_pos_ref_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(left_foot_pos_ref_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), @@ -299,16 +384,31 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_OUT(left_hand_acc, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_acc_des, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_foot_acc_des, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(LH_force_world, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(energy, double, INPUT_SIGNALS << m_q_desSOUT), + CONSTRUCT_SIGNAL_OUT(energy_derivative, double, m_energySOUT), + CONSTRUCT_SIGNAL_OUT(energy_tank, double, INPUT_SIGNALS << m_q_desSOUT), + CONSTRUCT_SIGNAL_OUT(denergy_tank, double, INPUT_SIGNALS << m_q_desSOUT), + CONSTRUCT_SIGNAL_OUT(energy_bound, double, INPUT_SIGNALS), + CONSTRUCT_SIGNAL_OUT(task_energy_alpha, double, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(task_energy_beta, double, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(task_energy_gamma, double, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(task_energy_S, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(task_energy_dS, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(task_energy_A, double, m_tau_desSOUT), CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN), m_t(0.0), m_initSucceeded(false), m_enabled(false), m_firstTime(true), + m_taskLHContactOn(false), m_contactState(DOUBLE_SUPPORT), 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), + m_energyTask_enabled(false) { RESETDEBUG5(); Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); @@ -321,6 +421,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 +434,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 +447,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, @@ -351,6 +465,51 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st addCommand("removeTaskLeftHand", makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeTaskLeftHand, docCommandVoid1("lowers the left hand.", "Transition time in seconds (double)"))); + + addCommand("addTaskEnergy", makeCommandVoid1(*this, &InverseDynamicsBalanceController::addTaskEnergy, + docCommandVoid1("Add the energy task.", "Transition time in seconds (double)"))); + addCommand("removeTaskEnergy", + makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeTaskEnergy, + docCommandVoid1("Remove the energy task.", "Transition time in seconds (double)"))); + addCommand("setEnergyTank", + makeCommandVoid1(*this, &InverseDynamicsBalanceController::setEnergyTank, + docCommandVoid1("Set the value of the energyTank", "Value of the tank in Joule (double)"))); + + addCommand("addTaskLeftHandContact", + makeCommandVoid0(*this, &InverseDynamicsBalanceController::addTaskLeftHandContact, + docCommandVoid0("add left hand contact."))); + addCommand("removeTaskLeftHandContact", + makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeTaskLeftHandContact, + docCommandVoid1("remove left hand contact.", "Transition time in seconds (double)"))); +} + +Vector InverseDynamicsBalanceController::actFrame(pinocchio::SE3 frame, Vector vec) { + Vector res; + pinocchio::SE3 vectorSE3, resSE3; + vectorSE3.translation(vec.head<3>()); + if (vec.size() == 12){ // Feet Positions -> directly have Rotation Matrix + res.resize(12); + res.head<3>() = frame.rotation() * (vec.head<3>() - frame.translation()); + MatrixRotation R; + R.row(0) = vec.segment(3, 3); + R.row(1) = vec.segment(6, 3); + R.row(2) = vec.segment(9, 3); + Vector3 euler = R.eulerAngles(2, 1, 0).reverse(); + R = (Eigen::AngleAxisd(-euler(2), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(-euler(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX())) + .toRotationMatrix(); + Eigen::Matrix M_ordered(R); + Eigen::Map vectorRotation(M_ordered.data(), M_ordered.size()); + res.tail<9>() = vectorRotation; + + } else if (vec.size() == 3){ // COM + res.resize(3); + res = frame.rotation() * (vec - frame.translation()); + } else{ + SEND_MSG("ERROR on actFrame() wrong size of vector : " + toString(vec.size()), MSG_TYPE_ERROR); + } + return res; } void InverseDynamicsBalanceController::updateComOffset() { @@ -360,6 +519,17 @@ 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); @@ -374,8 +544,9 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran if (transitionTime > m_dt) { m_contactState = LEFT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; - } else + } else { m_contactState = LEFT_SUPPORT; + } } } @@ -393,8 +564,9 @@ void InverseDynamicsBalanceController::removeLeftFootContact(const double& trans if (transitionTime > m_dt) { m_contactState = RIGHT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; - } else + } else { m_contactState = RIGHT_SUPPORT; + } } } @@ -434,8 +606,16 @@ 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); + pinocchio::SE3 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, 100, 1); + 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 +624,16 @@ 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); + pinocchio::SE3 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, 100, 1); + 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; } } @@ -466,6 +654,55 @@ void InverseDynamicsBalanceController::removeTaskLeftHand(const double& transiti } } +void InverseDynamicsBalanceController::addTaskEnergy(const double& transitionTime) { + // Handling not yet done with the energyTask + m_invDyn->removeTask(m_taskJointBounds->name(), transitionTime); + m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0, transitionTime); + m_energyTask_enabled = true; +} + +void InverseDynamicsBalanceController::removeTaskEnergy(const double& transitionTime) { + m_invDyn->removeTask(m_taskEnergy->name(), transitionTime); + // Handling not yet done with the energyTask + m_invDyn->addMotionTask(*m_taskJointBounds, 1.0, 0); + m_energyTask_enabled = false; +} + +void InverseDynamicsBalanceController::setEnergyTank(const double& tankValue) { + if (m_energyTask_enabled) { + m_taskEnergy->set_E_tank(tankValue); + } else { + SEND_MSG("No task energy in QP, add it with method addTaskEnergy(transitionTime)", MSG_TYPE_ERROR); + } + +} + +void InverseDynamicsBalanceController::addTaskLeftHandContact(/*const double& transitionTime*/) { + if (m_f_ext_left_armSIN.isPlugged()){ + int sensorFrameId = (int)m_robot->model().getFrameId("arm_left_7_link"); + pinocchio::framesForwardKinematics(m_robot->model(), m_invDyn->data(), m_q_urdf); + pinocchio::SE3 sensorPlacement = (m_invDyn->data()).oMf[sensorFrameId]; + m_contactLH->setReference(sensorPlacement); + const double & w_hands = m_w_handsSIN.accessCopy(); + m_invDyn->addRigidContact(*m_contactLH, 1e-3, 10, 1); + m_invDyn->addForceTask(*m_taskForceLH, w_hands, 1); + m_taskLHContactOn = true; + } else { + SEND_MSG("Error while adding left hand contact. No signal for external force", MSG_TYPE_ERROR); + } +} + +void InverseDynamicsBalanceController::removeTaskLeftHandContact(const double& transitionTime) { + bool res = m_invDyn->removeRigidContact(m_contactLH->name(), transitionTime); + if (!res) { + const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); + SEND_MSG("Error while remove left hand contact." + tsid::solvers::HQPDataToString(hqpData, false), + MSG_TYPE_ERROR); + } + m_invDyn->removeTask(m_taskForceLH->name(), 0.0); + m_taskLHContactOn = false; +} + void InverseDynamicsBalanceController::init(const double& dt, const std::string& robotRef) { if (dt <= 0.0) return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR); @@ -485,25 +722,29 @@ 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& kd_hands_force = m_kd_hands_forceSIN(0); + const dg::sot::Vector6d& kp_hands_force = m_kp_hands_forceSIN(0); + const dg::sot::Vector6d& ki_hands_force = m_ki_hands_forceSIN(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,45 +758,101 @@ 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); + m_JF.setZero(m_robot->nv()); m_q_urdf.setZero(m_robot->nq()); m_v_urdf.setZero(m_robot->nv()); + m_dv_urdf.setZero(m_robot->nv()); m_J_RF.setZero(6, m_robot->nv()); m_J_LF.setZero(6, m_robot->nv()); + m_previous_vel.setZero(m_robot->nv()); + m_previous_q.setZero(m_robot->nv()); + m_previous_energy = 0.0; + m_estim_data = pinocchio::Data(m_robot->model()); + + 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); + + 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); + 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); m_contactRF->Kd(kd_contact); - m_invDyn->addRigidContact(*m_contactRF, w_forces); + m_invDyn->addRigidContact(*m_contactRF, w_forces, 100, 1); m_contactLF = new Contact6d("contact_lfoot", *m_robot, m_robot_util->m_foot_util.m_Left_Foot_Frame_Name, contactPoints, contactNormal, mu, fMin, fMaxLF); m_contactLF->Kp(kp_contact); m_contactLF->Kd(kd_contact); - m_invDyn->addRigidContact(*m_contactLF, w_forces); + m_invDyn->addRigidContact(*m_contactLF, w_forces, 100, 1); - 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); + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + Eigen::VectorXd mask_com_height(3); + mask_com_height << 0, 0, 1; + m_taskCom->setMask(mask_com_height); + + m_taskComAdm = new TaskComEquality("task-com-adm", *m_robot); + m_taskComAdm->Kp(kp_com); + m_taskComAdm->Kd(kd_com); + Eigen::VectorXd mask_com_adm(3); + mask_com_adm << 1, 1, 0; + m_taskComAdm->setMask(mask_com_adm); + m_invDyn->addMotionTask(*m_taskComAdm, m_w_com, 1); + } m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1); + // 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 +861,28 @@ 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); + // ACTUATION BOUNDS TASK + Vector tau_max = 0.9 * m_robot->model().effortLimit.tail(m_robot->nv() - 6); + m_taskActBounds = new TaskActuationBounds("task-actuation-bounds", *m_robot); + m_taskActBounds->setBounds(-tau_max, tau_max); + m_invDyn->addActuationTask(*m_taskActBounds, 1.0, 0); + + // JOINT BOUNDS TASK + m_taskJointBounds = new TaskJointBounds("task-joint-bounds", *m_robot, dt); + Vector v_max = 0.8 * m_robot->model().velocityLimit.tail(m_robot->nv() - 6); + m_taskJointBounds->setVelocityBounds(-v_max, v_max); + // Handling not yet done with the energyTask + if (!m_energyTask_enabled) { + m_invDyn->addMotionTask(*m_taskJointBounds, 1.0, 0); + } + + // 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,15 +891,88 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskLH->Kp(kp_hands); m_taskLH->Kd(kd_hands); + // HANDS CONTACTS (not yet added, only when the command addTaskLeftHandContact is called) + // SPECIFIC TASKS FOR LEFT HAND CONTACT & FORCE APPLICATION on a bloc in Gazebo + Eigen::Vector3d contactNormalHand = Vector::Zero(3); + contactNormalHand[2] = 1.0; + // Square Contact points at the end of a "tool" (cylinder in Gazebo) + // Approx at 15cm from the wrist sensor + Eigen::Matrix contactPointsHand; + contactPointsHand.row(0) << 0.01, 0.01, -0.01, -0.01; + contactPointsHand.row(1) << 0.01, 0.01, -0.01, -0.01; + contactPointsHand.row(2) << -0.15, -0.15, -0.15, -0.15; + + m_contactLH = new Contact6d("contact-lh", *m_robot, "arm_left_7_link", + contactPointsHand, contactNormalHand, 0.01, fMin, fMaxRF); + m_contactLH->Kp(kp_contact); + m_contactLH->Kd(kd_contact); + + m_taskForceLH = new TaskContactForceEquality("task-force-lh", *m_robot, dt, *m_contactLH); + m_taskForceLH->Kp(kp_hands_force); + m_taskForceLH->Kd(kd_hands_force); + m_taskForceLH->Ki(ki_hands_force); + // END OF SPECIFIC TASKS FOR LEFT HAND CONTACT + + 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)); + + // ENERGY TASK, Added in the QP only if command addTaskEnergy() is called + m_taskEnergy = new TaskEnergy("task-energy", *m_robot, dt); + + // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + m_sampleComAdm = TrajectorySample(3); + } + m_sampleAM = TrajectorySample(3); + m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); - m_sampleRH = TrajectorySample(3); + m_sampleLHForceRef = TrajectorySample(6); + m_sampleLHForceExt = TrajectorySample(6); + m_sampleRH = TrajectorySample(6); + m_sampleLH = TrajectorySample(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); - m_frame_id_rh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); + m_frame_id_rh = (int)m_robot->model().getFrameId("gripper_right_base_link"); m_frame_id_lh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); + + 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); + Vector3 com_estim = data.com[0]; + m_omega = std::sqrt(9.81 / com_estim[2]); + + m_transformFrameCom = pinocchio::SE3::Identity(); + const Vector3& com_file = m_com_ref_posSIN(0); + if (std::abs(com_file.sum() - com_estim.sum()) > 0.001){ + m_transformFrameCom.translation() = com_file - com_estim; + SEND_MSG("m_transformFrameCom: " + toString(m_transformFrameCom), MSG_TYPE_INFO); + } + + m_transformFrameFeet = pinocchio::SE3::Identity(); + const VectorN& foot_file = m_lf_ref_posSIN(0); + pinocchio::SE3 oMi; + m_robot->framePosition(data, m_frame_id_lf, oMi); + Eigen::Matrix left_foot; + tsid::math::SE3ToVector(oMi, left_foot); + if (std::abs(foot_file.sum() - left_foot.sum()) > 0.001){ + m_transformFrameFeet.translation() = foot_file.head<3>() - left_foot.head<3>() ; + SEND_MSG("m_transformFrameFeet: " + toString(m_transformFrameFeet), MSG_TYPE_INFO); + } + + // COM OFFSET + if (m_com_measuredSIN.isPlugged()){ + const dg::Vector& com_measured = m_com_measuredSIN(0); + assert(com_measured.size() == 3); + SEND_MSG("COM_measured: " + toString(com_measured), MSG_TYPE_INFO); + m_com_offset = com_measured - com_estim; + SEND_MSG("Update COM: " + toString(m_com_offset), MSG_TYPE_INFO); + } m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog-fast"); m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn()); @@ -627,10 +1014,10 @@ DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector) { else blocked_joints(i) = 0.0; SEND_MSG("Blocked joints: " + toString(blocked_joints.transpose()), MSG_TYPE_INFO); - m_taskBlockedJoints->mask(blocked_joints); + m_taskBlockedJoints->setMask(blocked_joints); TrajectorySample ref_zero(static_cast(m_robot_util->m_nbJoints)); m_taskBlockedJoints->setReference(ref_zero); - m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0); + //m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0); } } else if (!active_joints_sot.any()) { /* from some ON to all OFF */ @@ -660,7 +1047,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 +1056,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 +1094,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 +1114,24 @@ 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(iter); + const dg::sot::Vector6d& kd_base_orientation = m_kd_base_orientationSIN(iter); 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 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); + + getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); if (m_contactState == LEFT_SUPPORT || m_contactState == LEFT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); @@ -726,34 +1139,37 @@ 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; - m_sampleRF.vel = dx_rf_ref; - m_sampleRF.acc = ddx_rf_ref; + m_sampleRF.setValue(actFrame(m_transformFrameFeet, x_rf_ref)); + m_sampleRF.setDerivative(dx_rf_ref); + m_sampleRF.setSecondDerivative(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; - m_sampleLF.vel = dx_lf_ref; - m_sampleLF.acc = ddx_lf_ref; + m_sampleLF.setValue(actFrame(m_transformFrameFeet, x_lf_ref)); + m_sampleLF.setDerivative(dx_lf_ref); + m_sampleLF.setSecondDerivative(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*/) { + // std::cout << "m_rightHandState == TASK_RIGHT_HAND_ON" << std::endl; const Eigen::Matrix& x_rh_ref = m_rh_ref_posSIN(iter); const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter); const Vector6& ddx_rh_ref = m_rh_ref_accSIN(iter); const Vector6& kp_hands = m_kp_handsSIN(iter); const Vector6& kd_hands = m_kd_handsSIN(iter); - m_sampleRH.pos = x_rh_ref; - m_sampleRH.vel = dx_rh_ref; - m_sampleRH.acc = ddx_rh_ref; + m_sampleRH.setValue(x_rh_ref); + m_sampleRH.setDerivative(dx_rh_ref); + m_sampleRH.setSecondDerivative(ddx_rh_ref); m_taskRH->setReference(m_sampleRH); m_taskRH->Kp(kp_hands); m_taskRH->Kd(kd_hands); @@ -764,23 +1180,70 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const Vector6& ddx_lh_ref = m_lh_ref_accSIN(iter); const Vector6& kp_hands = m_kp_handsSIN(iter); const Vector6& kd_hands = m_kd_handsSIN(iter); - m_sampleLH.pos = x_lh_ref; - m_sampleLH.vel = dx_lh_ref; - m_sampleLH.acc = ddx_lh_ref; + m_sampleLH.setValue(x_lh_ref); + m_sampleLH.setDerivative(dx_lh_ref); + m_sampleLH.setSecondDerivative(ddx_lh_ref); m_taskLH->setReference(m_sampleLH); m_taskLH->Kp(kp_hands); m_taskLH->Kd(kd_hands); } + if (m_taskLHContactOn) { + const Vector6& f_ref_LH = m_f_ref_left_armSIN(iter); + const Vector6& f_ext_LH = m_f_ext_left_armSIN(iter); + Vector6 f_ref_tool_LH, f_ext_tool_LH; + pinocchio::SE3 toolPlacement = pinocchio::SE3::Identity(); + Vector translation = Vector::Zero(3); + translation[2] = -0.15; + toolPlacement.translation() = translation; + f_ref_tool_LH = f_ref_LH; + f_ext_tool_LH = toolPlacement.act(pinocchio::Force(f_ext_LH)).toVector(); + + const Vector6d& kd_hands_force = m_kd_hands_forceSIN(iter); + const Vector6d& kp_hands_force = m_kp_hands_forceSIN(iter); + const Vector6d& ki_hands_force = m_ki_hands_forceSIN(iter); + m_sampleLHForceRef.setValue(f_ref_tool_LH); + m_sampleLHForceExt.setValue(f_ext_tool_LH); + + if (m_df_ext_left_armSIN.isPlugged()) { + const Vector6& df_ext_LH = m_df_ext_left_armSIN(iter); + const Vector6& df_ref_LH = m_df_ref_left_armSIN(iter); + Vector6 df_ext_tool_LH; + df_ext_tool_LH = toolPlacement.act(pinocchio::Force(df_ext_LH)).toVector(); + m_sampleLHForceExt.setDerivative(df_ext_tool_LH); + m_sampleLHForceRef.setDerivative(df_ref_LH); + } + + m_taskForceLH->setReference(m_sampleLHForceRef); + m_taskForceLH->setExternalForce(m_sampleLHForceExt); + m_taskForceLH->Kp(kp_hands_force); + m_taskForceLH->Kd(kd_hands_force); + m_taskForceLH->Ki(ki_hands_force); + } + 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; - m_sampleCom.vel = dx_com_ref; - m_sampleCom.acc = ddx_com_ref; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + const Vector3& x_com_adm_ref = m_com_adm_ref_posSIN(iter); + const Vector3& dx_com_adm_ref = m_com_adm_ref_velSIN(iter); + const Vector3& ddx_com_adm_ref = m_com_adm_ref_accSIN(iter); + m_sampleComAdm.setValue(x_com_adm_ref); + m_sampleComAdm.setDerivative(dx_com_adm_ref); + m_sampleCom.setSecondDerivative(ddx_com_adm_ref); + m_taskComAdm->setReference(m_sampleComAdm); + m_taskComAdm->Kp(kp_com); + m_taskComAdm->Kd(kd_com); + if (m_w_com != w_com) { + // SEND_MSG("Change w_com from "+toString(m_w_com)+" to "+toString(w_com), MSG_TYPE_INFO); + m_w_com = w_com; + m_invDyn->updateTaskWeight(m_taskComAdm->name(), w_com); + } + } + m_sampleCom.setValue(actFrame(m_transformFrameCom, x_com_ref) - m_com_offset); + m_sampleCom.setDerivative(dx_com_ref); + m_sampleCom.setSecondDerivative(ddx_com_ref); m_taskCom->setReference(m_sampleCom); m_taskCom->Kp(kp_com); m_taskCom->Kd(kd_com); @@ -790,9 +1253,38 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskCom->name(), w_com); } - 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); + m_sampleAM.setDerivative(L_am_ref); + m_sampleAM.setSecondDerivative(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.setValue(x_waist_ref); + m_sampleWaist.setDerivative(dx_waist_ref); + m_sampleWaist.setSecondDerivative(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); + } + + Vector q_urdf, dq_urdf, ddq_urdf; + q_urdf.setZero(m_robot_util->m_nbJoints); + dq_urdf.setZero(m_robot_util->m_nbJoints); + ddq_urdf.setZero(m_robot_util->m_nbJoints); + m_robot_util->joints_sot_to_urdf(q_ref, q_urdf); + m_robot_util->joints_sot_to_urdf(dq_ref, dq_urdf); + m_robot_util->joints_sot_to_urdf(ddq_ref, ddq_urdf); + m_samplePosture.setValue(q_urdf); + m_samplePosture.setDerivative(dq_urdf); + m_samplePosture.setSecondDerivative(ddq_urdf); m_taskPosture->setReference(m_samplePosture); m_taskPosture->Kp(kp_posture); m_taskPosture->Kd(kd_posture); @@ -802,13 +1294,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture); } - /*m_sampleRH.pos = x_rh_ref; - m_sampleRH.vel = dx_rh_ref; - m_sampleRH.acc = ddx_rh_ref; - m_taskRH->setReference(m_sampleRH); - m_taskRH->Kp(kp_hands); - m_taskRH->Kd(kd_hands);*/ - const double& fMin = m_f_minSIN(0); const double& fMaxRF = m_f_max_right_footSIN(iter); const double& fMaxLF = m_f_max_left_footSIN(iter); @@ -825,7 +1310,10 @@ 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_previous_q = q_sot; // 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)); @@ -842,7 +1330,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 +1357,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; } @@ -878,15 +1371,13 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getStatistics().store("com ff ratio", ddx_com_ref.norm() / m_taskCom->getConstraint().vector().norm()); m_dv_urdf = m_invDyn->getAccelerations(sol); + m_JF = m_invDyn->getJContactForces(sol); m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot); Eigen::Matrix tmp; if (m_invDyn->getContactForces(m_contactRF->name(), sol, tmp)) m_f_RF = m_contactRF->getForceGeneratorMatrix() * tmp; 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)); - getProfiler().stop(PROFILE_TAU_DES_COMPUTATION); m_t += m_dt; @@ -917,6 +1408,76 @@ 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)); + VectorN kp_tau = m_kp_tauSIN(iter); + assert(kp_tau.size() == static_cast(m_robot_util->m_nbJoints)); + const VectorN& kff_tau = m_kff_tauSIN(iter); + assert(kff_tau.size() == static_cast(m_robot_util->m_nbJoints)); + const VectorN& kff_dq = m_kff_dqSIN(iter); + assert(kff_dq.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); + if (tau_measured.sum() < 100) { + kp_tau.setZero(); + } + + s = kff_tau.cwiseProduct(m_tau_sot) + + kp_tau.cwiseProduct(tau_measured - m_tau_sot) + + kff_dq.cwiseProduct(m_v_sot.tail(m_robot_util->m_nbJoints)) + + 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)); + + 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 +1530,39 @@ 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(am_L, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal am_L before initialization!"); + return s; + } + if (s.size() != 3) s.resize(3); + m_tau_desSOUT(iter); + s = m_taskAM->momentum(); + 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!"); @@ -1058,7 +1652,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(zmp_des, dynamicgraph::Vector) { m_zmp_des_right_footSOUT(iter); m_zmp_des = (m_f_RF(2) * m_zmp_des_RF + m_f_LF(2) * m_zmp_des_LF) / (m_f_LF(2) + m_f_RF(2)); - s = m_zmp_des.head<2>(); + // s = m_taskCoP->getReference().head<2>(); //m_zmp_des.head<2>(); return s; } @@ -1147,7 +1741,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(zmp, dynamicgraph::Vector) { SEND_WARNING_STREAM_MSG(oss.str()); return s; } - if (s.size() != 2) s.resize(2); + if (s.size() != 3) s.resize(3); const Vector6& f_LF = m_wrench_left_footSIN(iter); const Vector6& f_RF = m_wrench_right_footSIN(iter); m_zmp_left_footSOUT(iter); @@ -1155,6 +1749,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(zmp, dynamicgraph::Vector) { if (f_LF(2) + f_RF(2) > 1.0) m_zmp = (f_RF(2) * m_zmp_RF + f_LF(2) * m_zmp_LF) / (f_LF(2) + f_RF(2)); s = m_zmp.head<2>(); + s(2) = 0.0; return s; } @@ -1168,6 +1763,35 @@ DEFINE_SIGNAL_OUT_FUNCTION(com, dynamicgraph::Vector) { if (s.size() != 3) s.resize(3); const Vector3& com = m_robot->com(m_invDyn->data()); s = com + m_com_offset; + pinocchio::SE3 invCom = m_transformFrameCom; + invCom.translation() = -m_transformFrameCom.translation(); + s = actFrame(invCom, com + m_com_offset); + 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; } @@ -1184,16 +1808,33 @@ DEFINE_SIGNAL_OUT_FUNCTION(com_vel, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(dcm, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal dcm before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + if (s.size() != 3) s.resize(3); + const Vector3& com = m_comSOUT(iter); + const Vector3& com_vel = m_com_velSOUT(iter); + s = com + com_vel / m_omega; + return s; +} + 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; } - /* - * Code - */ + m_tau_desSOUT(iter); + pinocchio::SE3 oMi; + int frame_id_waist = (int)m_robot->model().getFrameId("root_joint"); + m_robot->framePosition(m_invDyn->data(), frame_id_waist, oMi); + s.resize(12); + tsid::math::SE3ToVector(oMi, s); return s; } @@ -1212,6 +1853,57 @@ 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); + pinocchio::SE3 oMi; + 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(left_foot_pos_ref_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); + dg::Vector x_lf_ref = m_lf_ref_posSIN(iter); + x_lf_ref = actFrame(m_transformFrameFeet, x_lf_ref); + 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 +1932,57 @@ 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); + pinocchio::SE3 oMi; + 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(right_foot_pos_ref_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); + dg::Vector x_rf_ref = m_rf_ref_posSIN(iter); + x_rf_ref = actFrame(m_transformFrameFeet, x_rf_ref); + 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!"); @@ -1373,6 +2116,127 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc_des, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(LH_force_world, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal LH_force_world before initialization!"); + return s; + } + const Vector6& f_ext_LH = m_f_ext_left_armSIN.accessCopy(); + pinocchio::SE3 toolPlacement = pinocchio::SE3::Identity(); + Vector translation = Vector::Zero(3); + translation[2] = -0.23; + toolPlacement.translation() = translation; + s = toolPlacement.act(pinocchio::Force(f_ext_LH)).toVector(); + // s = sensorPlacement.act(pinocchio::Force(f_ext_LH)).toVector(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(energy, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy before initialization!"); + return s; + } + s = m_taskEnergy->get_H(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(energy_derivative, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy_derivative before initialization!"); + return s; + } + s = m_taskEnergy->get_dH(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(energy_tank, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy_tank before initialization!"); + return s; + } + s = m_taskEnergy->get_E_tank(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(denergy_tank, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal denergy_tank before initialization!"); + return s; + } + s = m_taskEnergy->get_dE_tank(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(energy_bound, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy_bound before initialization!"); + return s; + } + const VectorN6& v_robot = m_vSIN(iter); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + m_tau_desSOUT(iter); + s = - v_robot.tail(m_robot_util->m_nbJoints).transpose() * m_tau_sot; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_alpha, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_alpha before initialization!"); + return s; + } + s = m_taskEnergy->get_alpha(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_beta, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_beta before initialization!"); + return s; + } + s = m_taskEnergy->get_beta(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_gamma, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_gamma before initialization!"); + return s; + } + s = m_taskEnergy->get_gamma(); + return s; +} + + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_S, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_S before initialization!"); + return s; + } + s = m_taskEnergy->get_S(); + // s = S.sum(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_dS, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_dS before initialization!"); + return s; + } + s = m_taskEnergy->get_dS(); + // s = dS.sum(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_A, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_A before initialization!"); + return s; + } + Vector A = m_taskEnergy->get_A_vector(); + s = A.sum(); + return s; +} + /* --- COMMANDS ---------------------------------------------------------- */ /* ------------------------------------------------------------------- */ diff --git a/src/posture-task-python.hh b/src/posture-task-python.hh new file mode 100644 index 0000000..59b4e22 --- /dev/null +++ b/src/posture-task-python.hh @@ -0,0 +1,3 @@ +#include "sot/torque_control/posture-task.hh" + +typedef boost::mpl::vector< dynamicgraph::sot::torque_control::PostureTask > entities_t; diff --git a/src/posture-task.cpp b/src/posture-task.cpp new file mode 100644 index 0000000..d44a460 --- /dev/null +++ b/src/posture-task.cpp @@ -0,0 +1,751 @@ +/* + * Copyright 2021, Noëlie Ramuzat, LAAS-CNRS + * + * This file is part of sot-torque-control. + * sot-torque-control is free software: you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 of + * the License, or (at your option) any later version. + * sot-torque-control is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty + * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. You should + * have received a copy of the GNU Lesser General Public License along + * with sot-torque-control. If not, see . + */ + +#include + +#include +#include +#include + +#include + +#include +#include "pinocchio/algorithm/center-of-mass.hpp" +#include + +#include + +#if DEBUG +#define ODEBUG(x) std::cout << x << std::endl +#else +#define ODEBUG(x) +#endif +#define ODEBUG3(x) std::cout << x << std::endl + +#define DBGFILE "/tmp/debug-sot-torque-control.dat" + +#define RESETDEBUG5() { std::ofstream DebugFile; \ + DebugFile.open(DBGFILE,std::ofstream::out); \ + DebugFile.close();} +#define ODEBUG5FULL(x) { std::ofstream DebugFile; \ + DebugFile.open(DBGFILE,std::ofstream::app); \ + DebugFile << __FILE__ << ":" \ + << __FUNCTION__ << "(#" \ + << __LINE__ << "):" << x << std::endl; \ + DebugFile.close();} +#define ODEBUG5(x) { std::ofstream DebugFile; \ + DebugFile.open(DBGFILE,std::ofstream::app); \ + DebugFile << x << std::endl; \ + DebugFile.close();} + +#define RESETDEBUG4() +#define ODEBUG4FULL(x) +#define ODEBUG4(x) + +namespace dynamicgraph { +namespace sot { +namespace torque_control { +namespace dg = ::dynamicgraph; +using namespace dg; +using namespace dg::command; +using namespace std; +using namespace tsid; +using namespace tsid::trajectories; +using namespace tsid::math; +using namespace tsid::contacts; +using namespace tsid::tasks; +using namespace tsid::solvers; +using namespace dg::sot; + +#define REQUIRE_FINITE(A) assert(is_finite(A)) + + +#define INPUT_SIGNALS m_posture_ref_posSIN \ + << m_posture_ref_velSIN \ + << m_posture_ref_accSIN \ + << m_kp_postureSIN \ + << m_kd_postureSIN \ + << m_w_postureSIN \ + << m_w_forcesSIN \ + << m_kp_constraintsSIN \ + << m_kd_constraintsSIN \ + << m_muSIN \ + << m_contact_pointsSIN \ + << m_contact_normalSIN \ + << m_f_minSIN \ + << m_f_max_right_footSIN \ + << m_f_max_left_footSIN \ + << m_base_orientation_ref_posSIN \ + << m_base_orientation_ref_velSIN \ + << m_base_orientation_ref_accSIN \ + << m_kp_base_orientationSIN \ + << m_kd_base_orientationSIN \ + << m_w_base_orientationSIN \ + << 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_comSOUT \ + << m_energySOUT \ + << m_energy_derivativeSOUT \ + << m_energy_tankSOUT \ + << m_denergy_tankSOUT \ + << m_energy_boundSOUT \ + << m_task_energy_alphaSOUT \ + << m_task_energy_betaSOUT \ + << m_task_energy_gammaSOUT \ + << m_task_energy_SSOUT \ + << m_task_energy_dSSOUT \ + << m_task_energy_ASOUT \ + << m_right_foot_posSOUT \ + << m_left_foot_posSOUT \ + << m_base_orientationSOUT + + +/// Define EntityClassName here rather than in the header file +/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. +typedef PostureTask EntityClassName; + +typedef Eigen::Matrix Vector2; +typedef Eigen::Matrix VectorN; +typedef Eigen::Matrix VectorN6; +/* --- DG FACTORY ---------------------------------------------------- */ +DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PostureTask, + "PostureTask"); + +/* ------------------------------------------------------------------- */ +/* --- CONSTRUCTION -------------------------------------------------- */ +/* ------------------------------------------------------------------- */ +PostureTask:: +PostureTask(const std::string& name) + : Entity(name) + + , CONSTRUCT_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(kp_posture, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(w_posture, double) + , CONSTRUCT_SIGNAL_IN(kp_constraints, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(kd_constraints, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(w_forces, double) + , CONSTRUCT_SIGNAL_IN(mu, double) + , CONSTRUCT_SIGNAL_IN(contact_points, dynamicgraph::Matrix) + , CONSTRUCT_SIGNAL_IN(contact_normal, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(f_min, double) + , CONSTRUCT_SIGNAL_IN(f_max_right_foot, double) + , CONSTRUCT_SIGNAL_IN(f_max_left_foot, double) + , CONSTRUCT_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(w_base_orientation, 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) + , 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(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) + , CONSTRUCT_SIGNAL_OUT(energy, double, INPUT_SIGNALS << m_q_desSOUT) + , CONSTRUCT_SIGNAL_OUT(energy_derivative, double, m_energySOUT) + , CONSTRUCT_SIGNAL_OUT(energy_tank, double, INPUT_SIGNALS << m_q_desSOUT) + , CONSTRUCT_SIGNAL_OUT(denergy_tank, double, INPUT_SIGNALS << m_q_desSOUT) + , CONSTRUCT_SIGNAL_OUT(energy_bound, double, INPUT_SIGNALS) + , CONSTRUCT_SIGNAL_OUT(task_energy_alpha, double, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(task_energy_beta, double, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(task_energy_gamma, double, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(task_energy_S, dg::Vector, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(task_energy_dS, dg::Vector, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(task_energy_A, double, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(base_orientation, double, m_tau_desSOUT) + , m_t(0.0) + , m_initSucceeded(false) + , m_enabled(false) + , m_firstTime(true) + , m_timeLast(0) + , m_robot_util(RefVoidRobotUtil()) + , m_ctrlMode(CONTROL_OUTPUT_VELOCITY){ + RESETDEBUG5(); + Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS ); + + m_com_offset.setZero(); + + /* Commands. */ + addCommand("init", + makeCommandVoid2(*this, &PostureTask::init, + docCommandVoid2("Initialize the entity.", + "Time period in seconds (double)", + "Robot reference (string)"))); + /* SET of control output type. */ + addCommand("setControlOutputType", + makeCommandVoid1(*this, &PostureTask::setControlOutputType, + docCommandVoid1("Set the type of control output.", + "Control type: velocity or torque (string)"))); + + addCommand("updateComOffset", + makeCommandVoid1(*this, &PostureTask::updateComOffset, + docCommandVoid1("Update the offset on the CoM based on its measurement.", + "Measured CoM"))); + +} + +void PostureTask::updateComOffset(const dg::Vector& com_measured) { + const Vector3 & com = m_robot->com(m_invDyn->data()); + m_com_offset = com_measured - com; + SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO); +} + +void PostureTask::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 PostureTask::init(const double& dt, const std::string& robotRef) { + if (dt <= 0.0) + return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR); + + /* Retrieve m_robot_util informations */ + std::string localName(robotRef); + if (isNameInRobotUtil(localName)) + m_robot_util = getRobotUtil(localName); + else { + SEND_MSG("You should have an entity controller manager initialized before", MSG_TYPE_ERROR); + return; + } + const Eigen::Matrix& contactPoints = m_contact_pointsSIN(0); + const Eigen::Vector3d& contactNormal = m_contact_normalSIN(0); + const dg::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0); + const dg::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0); + const double& w_forces = m_w_forcesSIN(0); + const double& mu = m_muSIN(0); + const double& fMin = m_f_minSIN(0); + const double& fMaxRF = m_f_max_right_footSIN(0); + const double& fMaxLF = m_f_max_left_footSIN(0); + const VectorN& kp_posture = m_kp_postureSIN(0); + const VectorN& kd_posture = m_kd_postureSIN(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); + m_w_base_orientation = m_w_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)); + + m_w_posture = m_w_postureSIN(0); + + try { + vector package_dirs; + m_robot = new robots::RobotWrapper(m_robot_util->m_urdf_filename, + package_dirs, + pinocchio::JointModelFreeFlyer()); + + assert(m_robot->nv() >= 6); + m_robot_util->m_nbJoints = m_robot->nv() - 6; + + 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); + m_q_urdf.setZero(m_robot->nq()); + m_v_urdf.setZero(m_robot->nv()); + m_dv_urdf.setZero(m_robot->nv()); + + 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); + m_contactRF->Kd(kd_contact); + m_invDyn->addRigidContact(*m_contactRF, w_forces); + + m_contactLF = new Contact6d("contact_lfoot", *m_robot, m_robot_util->m_foot_util.m_Left_Foot_Frame_Name, + contactPoints, contactNormal, mu, fMin, fMaxLF); + m_contactLF->Kp(kp_contact); + m_contactLF->Kd(kd_contact); + m_invDyn->addRigidContact(*m_contactLF, w_forces); + + // 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); + + // 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); + + // ACTUATION BOUNDS TASK + Vector tau_max = 0.8 * m_robot->model().effortLimit.tail(m_robot->nv() - 6); + m_taskActBounds = new TaskActuationBounds("task-actuation-bounds", *m_robot); + m_taskActBounds->setBounds(-tau_max, tau_max); + m_invDyn->addActuationTask(*m_taskActBounds, 1.0, 0); + + 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)); + + // ENERGY TASK + m_taskEnergy = new TaskEnergy("task-energy", *m_robot, dt); + m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0); + + // TRAJECTORY INIT + 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 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()); + + } catch (const std::exception& e) { + std::cout << e.what(); + return SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR); + } + m_dt = dt; + m_initSucceeded = true; +} + +/* ------------------------------------------------------------------- */ +/* --- SIGNALS ------------------------------------------------------- */ +/* ------------------------------------------------------------------- */ +/** Copy active_joints only if a valid transition occurs. (From all OFF) or (To all OFF)**/ +DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector) { + if (s.size() != static_cast(m_robot_util->m_nbJoints)) + s.resize(m_robot_util->m_nbJoints); + + const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter); + if (m_enabled == false) { + if (active_joints_sot.any()) { + /* from all OFF to some ON */ + m_enabled = true ; + s = active_joints_sot; + Eigen::VectorXd active_joints_urdf(m_robot_util->m_nbJoints); + m_robot_util->joints_sot_to_urdf(active_joints_sot, active_joints_urdf); + + m_taskBlockedJoints = new TaskJointPosture("task-posture-blocked", *m_robot); + Eigen::VectorXd blocked_joints(m_robot_util->m_nbJoints); + for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) + if (active_joints_urdf(i) == 0.0) + blocked_joints(i) = 1.0; + else + blocked_joints(i) = 0.0; + SEND_MSG("Blocked joints: " + toString(blocked_joints.transpose()), MSG_TYPE_INFO); + m_taskBlockedJoints->setMask(blocked_joints); + TrajectorySample ref_zero(static_cast(m_robot_util->m_nbJoints)); + m_taskBlockedJoints->setReference(ref_zero); + //m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0); + } + } else if (!active_joints_sot.any()) { + /* from some ON to all OFF */ + m_enabled = false ; + } + if (m_enabled == false) + for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) + s(i) = false; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal tau_des before initialization!"); + return s; + } + if (s.size() != static_cast(m_robot_util->m_nbJoints)) + s.resize(m_robot_util->m_nbJoints); + + m_active_joints_checkedSINNER(iter); + + 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& 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); + assert(dq_ref.size() == static_cast(m_robot_util->m_nbJoints)); + const VectorN& ddq_ref = m_posture_ref_accSIN(iter); + assert(ddq_ref.size() == static_cast(m_robot_util->m_nbJoints)); + + 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 double & w_posture = m_w_postureSIN(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 dg::sot::Vector6d& kp_base_orientation = m_kp_base_orientationSIN(iter); + const dg::sot::Vector6d& kd_base_orientation = m_kd_base_orientationSIN(iter); + const double & w_base_orientation = m_w_base_orientationSIN(iter); + + // Update tasks + Vector q_urdf, dq_urdf, ddq_urdf; + q_urdf.setZero(m_robot_util->m_nbJoints); + dq_urdf.setZero(m_robot_util->m_nbJoints); + ddq_urdf.setZero(m_robot_util->m_nbJoints); + m_robot_util->joints_sot_to_urdf(q_ref, q_urdf); + m_robot_util->joints_sot_to_urdf(dq_ref, dq_urdf); + m_robot_util->joints_sot_to_urdf(ddq_ref, ddq_urdf); + m_samplePosture.setValue(q_urdf); + m_samplePosture.setDerivative(dq_urdf); + m_samplePosture.setSecondDerivative(ddq_urdf); + m_taskPosture->setReference(m_samplePosture); + m_taskPosture->Kp(kp_posture); + m_taskPosture->Kd(kd_posture); + if (m_w_posture != w_posture) { + m_w_posture = w_posture; + m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture); + } + + m_sampleWaist.setValue(x_waist_ref); + m_sampleWaist.setDerivative(dx_waist_ref); + m_sampleWaist.setSecondDerivative(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); + } + + if (m_firstTime) { + 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); + 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); + + 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); + + } 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)) { + 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_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); + SolverHQPBase * solver = m_hqpSolver; + const HQPOutput & sol = solver->solve(hqpData); + + if (sol.status != HQP_STATUS_OPTIMAL) { + 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; + } + + m_dv_urdf = m_invDyn->getAccelerations(sol); + m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot); + m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot); + m_t += m_dt; + + s = m_tau_sot; + + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(dv_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal dv_des before initialization!"); + return s; + } + if (s.size() != m_robot->nv()) + s.resize(m_robot->nv()); + m_tau_desSOUT(iter); + s = m_dv_sot; + 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(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; +} + +DEFINE_SIGNAL_OUT_FUNCTION(energy, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy before initialization!"); + return s; + } + s = m_taskEnergy->get_H(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(energy_derivative, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy_derivative before initialization!"); + return s; + } + s = m_taskEnergy->get_dH(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(energy_tank, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy_tank before initialization!"); + return s; + } + s = m_taskEnergy->get_E_tank(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(denergy_tank, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal denergy_tank before initialization!"); + return s; + } + s = m_taskEnergy->get_dE_tank(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(energy_bound, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy_bound before initialization!"); + return s; + } + const VectorN6& v_robot = m_vSIN(iter); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + m_tau_desSOUT(iter); + s = - v_robot.tail(m_robot_util->m_nbJoints).transpose() * m_tau_sot; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_alpha, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_alpha before initialization!"); + return s; + } + s = m_taskEnergy->get_alpha(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_beta, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_beta before initialization!"); + return s; + } + s = m_taskEnergy->get_beta(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_gamma, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_gamma before initialization!"); + return s; + } + s = m_taskEnergy->get_gamma(); + return s; +} + + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_S, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_S before initialization!"); + return s; + } + s = m_taskEnergy->get_S(); + // s = S.sum(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_dS, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_dS before initialization!"); + return s; + } + s = m_taskEnergy->get_dS(); + // s = dS.sum(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_A, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_A before initialization!"); + return s; + } + Vector A = m_taskEnergy->get_A_vector(); + s = A.sum(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, double) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal base_orientation before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + m_tau_desSOUT(iter); + // pinocchio::SE3 oMi; + // int frame_id_waist = (int)m_robot->model().getFrameId("root_joint"); + // m_robot->framePosition(m_invDyn->data(), frame_id_waist, oMi); + // s.resize(12); + // tsid::math::SE3ToVector(oMi, s); + Vector error = m_taskWaist->position_error(); + s = error.norm(); + return s; +} + + +/* --- COMMANDS ---------------------------------------------------------- */ + +/* ------------------------------------------------------------------- */ +/* --- ENTITY -------------------------------------------------------- */ +/* ------------------------------------------------------------------- */ + +void PostureTask::display(std::ostream& os) const { + os << "PostureTask " << getName(); + try { + os << "QP size: nVar " << m_invDyn->nVar() << " nEq " << m_invDyn->nEq() << " nIn " << m_invDyn->nIn() << "\n"; + } catch (ExceptionSignal e) {} +} +} // namespace torquecontrol +} // namespace sot +} // namespace dynamicgraph diff --git a/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index 6321fee..87e7158 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -14,7 +14,7 @@ * with sot-torque-control. If not, see . */ -#include +#include #include #include @@ -25,9 +25,10 @@ #include #include +#include "pinocchio/algorithm/center-of-mass.hpp" +#include #include -#include #if DEBUG #define ODEBUG(x) std::cout << x << std::endl @@ -89,6 +90,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 +114,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 +147,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 +171,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 +179,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 +207,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); } @@ -289,11 +301,17 @@ void SimpleInverseDyn::init(const double& dt, const std::string& robotRef) { m_contactLF->Kd(kd_contact); m_invDyn->addRigidContact(*m_contactLF, w_forces); + // ACTUATION BOUNDS TASK + Vector tau_max = 0.8 * m_robot->model().effortLimit.tail(m_robot->nv() - 6); + m_taskActBounds = new TaskActuationBounds("task-actuation-bounds", *m_robot); + m_taskActBounds->setBounds(-tau_max, tau_max); + m_invDyn->addActuationTask(*m_taskActBounds, 1.0, 0); + // 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, 0); + m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1); // WAIST Task m_taskWaist = new TaskSE3Equality("task-waist", *m_robot, "root_joint"); @@ -319,6 +337,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()); @@ -356,7 +396,7 @@ DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector) { else blocked_joints(i) = 0.0; SEND_MSG("Blocked joints: " + toString(blocked_joints.transpose()), MSG_TYPE_INFO); - m_taskBlockedJoints->mask(blocked_joints); + m_taskBlockedJoints->setMask(blocked_joints); TrajectorySample ref_zero(static_cast(m_robot_util->m_nbJoints)); m_taskBlockedJoints->setReference(ref_zero); m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0); @@ -425,9 +465,10 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().start(PROFILE_PREPARE_INV_DYN); - m_sampleCom.pos = x_com_ref - m_com_offset; - m_sampleCom.vel = dx_com_ref; - m_sampleCom.acc = ddx_com_ref; + // Update tasks + m_sampleCom.setValue(x_com_ref - m_com_offset); + m_sampleCom.setDerivative(dx_com_ref); + m_sampleCom.setSecondDerivative(ddx_com_ref); m_taskCom->setReference(m_sampleCom); m_taskCom->Kp(kp_com); m_taskCom->Kd(kd_com); @@ -436,9 +477,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskCom->name(), w_com); } - m_sampleWaist.pos = x_waist_ref; - m_sampleWaist.vel = dx_waist_ref; - m_sampleWaist.acc = ddx_waist_ref; + m_sampleWaist.setValue(x_waist_ref); + m_sampleWaist.setDerivative(dx_waist_ref); + m_sampleWaist.setSecondDerivative(ddx_waist_ref); m_taskWaist->setReference(m_sampleWaist); m_taskWaist->Kp(kp_waist); m_taskWaist->Kd(kd_waist); @@ -447,9 +488,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskWaist->name(), w_waist); } - 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); + Vector q_urdf, dq_urdf, ddq_urdf; + q_urdf.setZero(m_robot_util->m_nbJoints); + dq_urdf.setZero(m_robot_util->m_nbJoints); + ddq_urdf.setZero(m_robot_util->m_nbJoints); + m_robot_util->joints_sot_to_urdf(q_ref, q_urdf); + m_robot_util->joints_sot_to_urdf(dq_ref, dq_urdf); + m_robot_util->joints_sot_to_urdf(ddq_ref, ddq_urdf); + m_samplePosture.setValue(q_urdf); + m_samplePosture.setDerivative(dq_urdf); + m_samplePosture.setSecondDerivative(ddq_urdf); m_taskPosture->setReference(m_samplePosture); m_taskPosture->Kp(kp_posture); m_taskPosture->Kd(kd_posture); @@ -472,21 +520,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 +542,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 +562,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 +639,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; }