From 27bac456f165462dc10b52b51f91dbab65341deb Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 6 Apr 2020 16:33:59 +0200 Subject: [PATCH 01/47] [Angular Momentum] Add AM task in inverse-dynamic-balance-controller entity Try to use it with talos-torque-control package and trajectories from multicontact_api. Works with walk on spot trajectories. --- .../inverse-dynamics-balance-controller.hh | 34 ++- src/inverse-dynamics-balance-controller.cpp | 241 ++++++++++++++++-- 2 files changed, 243 insertions(+), 32 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 0f3a68f..49a020e 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -30,6 +30,7 @@ /* Pinocchio */ #include #include +#include "pinocchio/algorithm/joint-configuration.hpp" #include #include @@ -37,6 +38,7 @@ #include #include #include +#include #include /* HELPER */ @@ -79,6 +81,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(am_ref_L, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(am_ref_dL, dynamicgraph::Vector); DECLARE_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector); @@ -106,6 +110,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_am, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kd_am, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_feet, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_feet, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_hands, dynamicgraph::Vector); @@ -116,6 +122,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(kd_pos, 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); @@ -152,6 +159,10 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); @@ -167,6 +178,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_OUT(com_vel, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(com_acc, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(com_acc_des, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(am_dL, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(am_dL_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); @@ -182,6 +195,7 @@ 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(time, 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); @@ -243,6 +257,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::contacts::Contact6d* m_contactRH; tsid::contacts::Contact6d* m_contactLH; tsid::tasks::TaskComEquality* m_taskCom; + tsid::tasks::TaskAMEquality* m_taskAM; + tsid::tasks::TaskSE3Equality* m_taskWaist; tsid::tasks::TaskSE3Equality* m_taskRF; tsid::tasks::TaskSE3Equality* m_taskLF; tsid::tasks::TaskSE3Equality* m_taskRH; @@ -251,18 +267,28 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::tasks::TaskJointPosture* m_taskBlockedJoints; tsid::trajectories::TrajectorySample m_sampleCom; + tsid::trajectories::TrajectorySample m_sampleAM; tsid::trajectories::TrajectorySample m_sampleRF; tsid::trajectories::TrajectorySample m_sampleLF; tsid::trajectories::TrajectorySample m_sampleRH; tsid::trajectories::TrajectorySample m_sampleLH; + tsid::trajectories::TrajectorySample m_sampleWaist; tsid::trajectories::TrajectorySample m_samplePosture; double m_w_com; + double m_w_am; double m_w_posture; double m_w_hands; + double m_w_base_orientation; + + tsid::math::Vector m_dv_sot; /// desired accelerations (sot order) + tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order) + tsid::math::Vector m_v_sot; /// desired velocities (sot order) + tsid::math::Vector m_v_urdf; /// desired and current velocities (urdf order) (close the TSID loop on it) + tsid::math::Vector m_q_sot; /// desired positions (sot order) + tsid::math::Vector m_q_urdf; /// desired and current positions (urdf order) (close the TSID loop on it) + tsid::math::Vector m_tau_sot; /// desired torques (sot order) - tsid::math::Vector m_dv_sot; /// desired accelerations (sot order) - tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order) tsid::math::Vector m_f; /// desired force coefficients (24d) tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot @@ -275,9 +301,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::math::Vector3 m_zmp_LF; /// 3d zmp left foot tsid::math::Vector3 m_zmp_RF; /// 3d zmp left foot tsid::math::Vector3 m_zmp; /// 3d global zmp - tsid::math::Vector m_tau_sot; - tsid::math::Vector m_q_urdf; - tsid::math::Vector m_v_urdf; + typedef pinocchio::Data::Matrix6x Matrix6x; Matrix6x m_J_RF; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 018dd15..9be6795 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -87,6 +87,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; #define INPUT_SIGNALS m_com_ref_posSIN \ << m_com_ref_velSIN \ << m_com_ref_accSIN \ + << m_am_ref_LSIN \ + << m_am_ref_dLSIN \ << m_rf_ref_posSIN \ << m_rf_ref_velSIN \ << m_rf_ref_accSIN \ @@ -113,6 +115,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_kd_constraintsSIN \ << m_kp_comSIN \ << m_kd_comSIN \ + << m_kp_amSIN \ + << m_kd_amSIN \ << m_kp_feetSIN \ << m_kd_feetSIN \ << m_kp_handsSIN \ @@ -122,6 +126,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_kp_posSIN \ << m_kd_posSIN \ << m_w_comSIN \ + << m_w_amSIN \ << m_w_feetSIN \ << m_w_handsSIN \ << m_w_postureSIN \ @@ -154,6 +159,9 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; #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 \ @@ -169,6 +177,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_com_velSOUT \ << m_com_accSOUT \ << m_com_acc_desSOUT \ + << m_am_dLSOUT \ + << m_am_dL_desSOUT \ << m_base_orientationSOUT \ << m_left_foot_posSOUT \ << m_right_foot_posSOUT \ @@ -183,7 +193,8 @@ 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_timeSOUT /// Define EntityClassName here rather than in the header file /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. @@ -203,6 +214,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(am_ref_L, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(am_ref_dL, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector), @@ -229,6 +242,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(kd_constraints, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_com, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_com, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kp_am, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kd_am, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_feet, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_feet, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_hands, dynamicgraph::Vector), @@ -238,6 +253,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_pos, 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), @@ -269,6 +285,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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), @@ -284,6 +303,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_OUT(com_vel, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(com_acc, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(com_acc_des, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(am_dL, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(am_dL_des, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT), @@ -299,6 +320,7 @@ 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(time, double, m_tau_desSOUT), CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN), m_t(0.0), m_initSucceeded(false), @@ -341,6 +363,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, @@ -362,7 +392,6 @@ void InverseDynamicsBalanceController::updateComOffset() { void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); bool res = m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -370,18 +399,18 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran MSG_TYPE_ERROR); } const double& w_feet = m_w_feetSIN.accessCopy(); - m_invDyn->addMotionTask(*m_taskRF, w_feet, 1); + m_invDyn->addMotionTask(*m_taskRF, w_feet, 0); if (transitionTime > m_dt) { m_contactState = LEFT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; - } else + } else { m_contactState = LEFT_SUPPORT; + } } } void InverseDynamicsBalanceController::removeLeftFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); bool res = m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -389,12 +418,13 @@ void InverseDynamicsBalanceController::removeLeftFootContact(const double& trans MSG_TYPE_ERROR); } const double& w_feet = m_w_feetSIN.accessCopy(); - m_invDyn->addMotionTask(*m_taskLF, w_feet, 1); + m_invDyn->addMotionTask(*m_taskLF, w_feet, 0); if (transitionTime > m_dt) { m_contactState = RIGHT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; - } else + } else { m_contactState = RIGHT_SUPPORT; + } } } @@ -432,7 +462,7 @@ void InverseDynamicsBalanceController::addTaskLeftHand(/*const double& transitio void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime) { if (m_contactState == LEFT_SUPPORT) { - SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); + SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); m_invDyn->addRigidContact(*m_contactRF, w_forces); m_invDyn->removeTask(m_taskRF->name(), transitionTime); @@ -442,7 +472,7 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit void InverseDynamicsBalanceController::addLeftFootContact(const double& transitionTime) { if (m_contactState == RIGHT_SUPPORT) { - SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); + SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); m_invDyn->addRigidContact(*m_contactLF, w_forces); m_invDyn->removeTask(m_taskLF->name(), transitionTime); @@ -485,12 +515,16 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& const dg::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0); const Eigen::Vector3d& kp_com = m_kp_comSIN(0); const Eigen::Vector3d& kd_com = m_kd_comSIN(0); + const Eigen::Vector3d& kp_am = m_kp_amSIN(0); + const Eigen::Vector3d& kd_am = m_kd_amSIN(0); const dg::sot::Vector6d& kd_hands = m_kd_handsSIN(0); const dg::sot::Vector6d& kp_hands = m_kp_handsSIN(0); const dg::sot::Vector6d& kp_feet = m_kp_feetSIN(0); const dg::sot::Vector6d& kd_feet = m_kd_feetSIN(0); const VectorN& kp_posture = m_kp_postureSIN(0); const VectorN& kd_posture = m_kd_postureSIN(0); + const dg::sot::Vector6d& kp_base_orientation = m_kp_base_orientationSIN(0); + const dg::sot::Vector6d& kd_base_orientation = m_kd_base_orientationSIN(0); const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0); const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0); @@ -501,9 +535,10 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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); @@ -523,7 +558,9 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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->nq()); + 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); @@ -534,6 +571,7 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot); + // CONTACT 6D TASKS m_contactRF = new Contact6d("contact_rfoot", *m_robot, m_robot_util->m_foot_util.m_Right_Foot_Frame_Name, contactPoints, contactNormal, mu, fMin, fMaxRF); m_contactRF->Kp(kp_contact); @@ -546,16 +584,37 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_contactLF->Kd(kd_contact); m_invDyn->addRigidContact(*m_contactLF, w_forces); - if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { - m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones()); - m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones()); - } + if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { + m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones()); + m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones()); + } + // TASK COM m_taskCom = new TaskComEquality("task-com", *m_robot); m_taskCom->Kp(kp_com); m_taskCom->Kd(kd_com); - m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1); - + m_invDyn->addMotionTask(*m_taskCom, m_w_com, 0); + + // TASK ANGULAR MOMENTUM + m_taskAM = new TaskAMEquality("task-am", *m_robot); + m_taskAM->Kp(kp_am); + m_taskAM->Kd(kd_am); + m_invDyn->addMotionTask(*m_taskAM, m_w_am, 1); + + // TASK BASE ORIENTATION + m_taskWaist = new TaskSE3Equality("task-waist", *m_robot, "root_joint"); + m_taskWaist->Kp(kp_base_orientation); + m_taskWaist->Kd(kd_base_orientation); + // Add a Mask to the task which will select the vector dimensions on which the task will act. + // In this case the waist configuration is a vector 6d (position and orientation -> SE3) + // Here we set a mask = [0 0 0 1 1 1] so the task on the waist will act on the orientation of the robot + Eigen::VectorXd mask_orientation(6); + mask_orientation << 0, 0, 0, 1, 1, 1; + m_taskWaist->setMask(mask_orientation); + // Add the task to the HQP with weight = 1.0, priority level = 1 (in the cost function) and a transition duration = 0.0 + m_invDyn->addMotionTask(*m_taskWaist, m_w_base_orientation, 1); + + // FEET TASKS (not added yet to the invDyn pb, only when contacts are removed) m_taskRF = new TaskSE3Equality("task-rf", *m_robot, m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); m_taskRF->Kp(kp_feet); m_taskRF->Kd(kd_feet); @@ -564,11 +623,13 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskLF->Kp(kp_feet); m_taskLF->Kd(kd_feet); + // POSTURE TASK m_taskPosture = new TaskJointPosture("task-posture", *m_robot); m_taskPosture->Kp(kp_posture); m_taskPosture->Kd(kd_posture); m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1); + // HANDS TASKS (not added yet to the invDyn pb, only when the command addTaskXHand is called) m_taskRH = new TaskSE3Equality("task-rh", *m_robot, m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_taskRH->Kp(kp_hands); m_taskRH->Kd(kd_hands); @@ -577,7 +638,10 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskLH->Kp(kp_hands); m_taskLH->Kd(kd_hands); + // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); + // m_sampleAM = TrajectorySample(3); + m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); m_sampleRH = TrajectorySample(3); @@ -660,7 +724,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) { @@ -691,9 +755,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 +775,26 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const Vector6& kd_contact = m_kd_constraintsSIN(iter); const Vector3& kp_com = m_kp_comSIN(iter); const Vector3& kd_com = m_kd_comSIN(iter); + const Vector3& kp_am = m_kp_amSIN(iter); + const Vector3& kd_am = m_kd_amSIN(iter); + const dg::sot::Vector6d& kp_base_orientation = m_kp_base_orientationSIN(0); + const dg::sot::Vector6d& kd_base_orientation = m_kd_base_orientationSIN(0); const VectorN& kp_posture = m_kp_postureSIN(iter); assert(kp_posture.size() == static_cast(m_robot_util->m_nbJoints)); const VectorN& kd_posture = m_kd_postureSIN(iter); assert(kd_posture.size() == static_cast(m_robot_util->m_nbJoints)); - const VectorN& kp_pos = m_kp_posSIN(iter); - assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); - const VectorN& kd_pos = m_kd_posSIN(iter); - assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); + // const VectorN& kp_pos = m_kp_posSIN(iter); + // assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); + // const VectorN& kd_pos = m_kd_posSIN(iter); + // assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); /*const double & w_hands = m_w_handsSIN(iter);*/ const double& w_com = m_w_comSIN(iter); + const double& w_am = m_w_amSIN(iter); const double& w_posture = m_w_postureSIN(iter); const double& w_forces = m_w_forcesSIN(iter); + const double & w_base_orientation = m_w_base_orientationSIN(iter); if (m_contactState == LEFT_SUPPORT || m_contactState == LEFT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); @@ -775,8 +851,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); getProfiler().start(PROFILE_PREPARE_INV_DYN); - m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); - m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); + // m_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; @@ -790,6 +866,28 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskCom->name(), w_com); } + m_sampleAM.vel = L_am_ref; + m_sampleAM.acc = dL_am_ref; + m_taskAM->setReference(m_sampleAM); + m_taskAM->Kp(kp_am); + m_taskAM->Kd(kd_am); + if (m_w_am != w_am) { + // SEND_MSG("Change w_am from "+toString(m_w_am)+" to "+toString(w_am), MSG_TYPE_INFO); + m_w_am = w_am; + m_invDyn->updateTaskWeight(m_taskAM->name(), w_am); + } + + m_sampleWaist.pos = x_waist_ref; + m_sampleWaist.vel = dx_waist_ref; + m_sampleWaist.acc = ddx_waist_ref; + m_taskWaist->setReference(m_sampleWaist); + m_taskWaist->Kp(kp_base_orientation); + m_taskWaist->Kd(kd_base_orientation); + if (m_w_base_orientation != w_base_orientation) { + m_w_base_orientation = w_base_orientation; + m_invDyn->updateTaskWeight(m_taskWaist->name(), w_base_orientation); + } + m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos); m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel); m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc); @@ -825,6 +923,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_firstTime) { m_firstTime = false; + m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); // m_robot->computeAllTerms(m_invDyn->data(), q, v); pinocchio::SE3 H_lf = m_robot->position( @@ -884,8 +984,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_invDyn->getContactForces(m_contactLF->name(), sol, tmp)) m_f_LF = m_contactLF->getForceGeneratorMatrix() * tmp; m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot); - m_tau_sot += kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) + - kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints)); + // m_tau_sot += kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) + + // kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints)); getProfiler().stop(PROFILE_TAU_DES_COMPUTATION); m_t += m_dt; @@ -917,6 +1017,62 @@ DEFINE_SIGNAL_OUT_FUNCTION(dv_des, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(v_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal v_des before initialization!"); + return s; + } + if (s.size() != m_robot->nv()) + s.resize(m_robot->nv()); + m_dv_desSOUT(iter); + tsid::math::Vector v_mean; + v_mean = m_v_urdf + 0.5 * m_dt * m_dv_urdf; + m_v_urdf = m_v_urdf + m_dt * m_dv_urdf; + m_q_urdf = pinocchio::integrate(m_robot->model(), m_q_urdf, m_dt * v_mean); + m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_v_urdf, m_v_sot); + s = m_v_sot; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(q_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal q_des before initialization!"); + return s; + } + if (s.size() != m_robot->nv()) + s.resize(m_robot->nv()); + m_v_desSOUT(iter); + m_robot_util->config_urdf_to_sot(m_q_urdf, m_q_sot); + s = m_q_sot; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal tau_pd_des before initialization!"); + return s; + } + if (s.size() != static_cast(m_robot_util->m_nbJoints)) + s.resize(m_robot_util->m_nbJoints); + + const VectorN& kp_pos = m_kp_posSIN(iter); + assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); + const VectorN& kd_pos = m_kd_posSIN(iter); + assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); + + const 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)); + + m_q_desSOUT(iter); + + s = m_tau_sot + kp_pos.cwiseProduct(m_q_sot.tail(m_robot_util->m_nbJoints) - q_robot.tail(m_robot_util->m_nbJoints)) + + kd_pos.cwiseProduct(m_v_sot.tail(m_robot_util->m_nbJoints) - v_robot.tail(m_robot_util->m_nbJoints)); + + 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 +1125,28 @@ DEFINE_SIGNAL_OUT_FUNCTION(com_acc, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(am_dL_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal am_dL_des before initialization!"); + return s; + } + if (s.size() != 3) s.resize(3); + m_tau_desSOUT(iter); + s = m_taskAM->getDesiredMomentumDerivative(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(am_dL, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal am_dL before initialization!"); + return s; + } + if (s.size() != 3) s.resize(3); + m_tau_desSOUT(iter); + s = m_taskAM->getdMomentum(m_dv_urdf); + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_right_foot_local, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal zmp_des_right_foot_local before initialization!"); @@ -1186,7 +1364,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(com_vel, dynamicgraph::Vector) { DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, dynamicgraph::Vector) { if (!m_initSucceeded) { - std::ostringstream oss("Cannot compute signal com_vel before initialization! iter:"); + std::ostringstream oss("Cannot compute signal base_orientation before initialization! iter:"); oss << iter; SEND_WARNING_STREAM_MSG(oss.str()); return s; @@ -1373,6 +1551,15 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc_des, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(time, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal time before initialization!"); + return s; + } + m_tau_desSOUT(iter); + return m_t; +} + /* --- COMMANDS ---------------------------------------------------------- */ /* ------------------------------------------------------------------- */ From 99513912e7ebf4fe3c2cd3ef846e6bdebc1ac0f9 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 6 Apr 2020 18:39:23 +0200 Subject: [PATCH 02/47] [Inv_Dyn] Update references of contacts when they are added --- src/inverse-dynamics-balance-controller.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 9be6795..566b42b 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -464,6 +464,10 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit if (m_contactState == LEFT_SUPPORT) { SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); + TrajectorySample traj_ref = m_taskRF->getReference(); + pinocchio::SE3 ref; + vectorToSE3(traj_ref.pos, ref); + m_contactRF->setReference(ref); m_invDyn->addRigidContact(*m_contactRF, w_forces); m_invDyn->removeTask(m_taskRF->name(), transitionTime); m_contactState = DOUBLE_SUPPORT; @@ -474,6 +478,10 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti if (m_contactState == RIGHT_SUPPORT) { SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); + TrajectorySample traj_ref = m_taskLF->getReference(); + pinocchio::SE3 ref; + vectorToSE3(traj_ref.pos, ref); + m_contactLF->setReference(ref); m_invDyn->addRigidContact(*m_contactLF, w_forces); m_invDyn->removeTask(m_taskLF->name(), transitionTime); m_contactState = DOUBLE_SUPPORT; From c1671d10e4018dd4eaf0aac8428ea28a6f339ede Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 12 May 2020 12:13:26 +0200 Subject: [PATCH 03/47] [Base-estim] Plug base-estimator to inv_dyn entity to have updates on q/v Remove useless sendMsg functions --- .../torque_control/admittance-controller.hh | 4 - include/sot/torque_control/base-estimator.hh | 4 - include/sot/torque_control/control-manager.hh | 4 - .../sot/torque_control/current-controller.hh | 4 - .../sot/torque_control/device-torque-ctrl.hh | 4 - .../sot/torque_control/free-flyer-locator.hh | 4 - .../torque_control/imu_offset_compensation.hh | 3 - .../inverse-dynamics-balance-controller.hh | 11 +- .../torque_control/joint-torque-controller.hh | 4 - .../joint-trajectory-generator.hh | 4 - .../torque_control/numerical-difference.hh | 5 - .../sot/torque_control/position-controller.hh | 4 - .../se3-trajectory-generator.hh | 4 - .../sot/torque_control/simple-inverse-dyn.hh | 18 ++- .../torque_control/torque-offset-estimator.hh | 4 - include/sot/torque_control/trace-player.hh | 4 - src/inverse-dynamics-balance-controller.cpp | 138 +++++++++++++----- src/simple-inverse-dyn.cpp | 129 +++++++++++++--- 18 files changed, 228 insertions(+), 124 deletions(-) diff --git a/include/sot/torque_control/admittance-controller.hh b/include/sot/torque_control/admittance-controller.hh index edbdea9..5f8f67f 100644 --- a/include/sot/torque_control/admittance-controller.hh +++ b/include/sot/torque_control/admittance-controller.hh @@ -90,10 +90,6 @@ class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController : public ::dynamicgrap /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: Eigen::VectorXd m_u; /// control (i.e. motor currents) bool m_firstIter; diff --git a/include/sot/torque_control/base-estimator.hh b/include/sot/torque_control/base-estimator.hh index d27505c..e91250a 100644 --- a/include/sot/torque_control/base-estimator.hh +++ b/include/sot/torque_control/base-estimator.hh @@ -160,10 +160,6 @@ class SOTBASEESTIMATOR_EXPORT BaseEstimator : public ::dynamicgraph::Entity { /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: bool m_initSucceeded; /// true if the entity has been successfully initialized bool m_reset_foot_pos; /// true after the command resetFootPositions is called diff --git a/include/sot/torque_control/control-manager.hh b/include/sot/torque_control/control-manager.hh index 058555d..fa0fccd 100644 --- a/include/sot/torque_control/control-manager.hh +++ b/include/sot/torque_control/control-manager.hh @@ -130,10 +130,6 @@ class SOTCONTROLMANAGER_EXPORT ControlManager : public ::dynamicgraph::Entity { /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[ControlManager-" + name + "] " + msg) << '\n'; - } - protected: RobotUtilShrPtr m_robot_util; tsid::robots::RobotWrapper* m_robot; diff --git a/include/sot/torque_control/current-controller.hh b/include/sot/torque_control/current-controller.hh index 4995e8e..23b711c 100644 --- a/include/sot/torque_control/current-controller.hh +++ b/include/sot/torque_control/current-controller.hh @@ -106,10 +106,6 @@ class SOTCURRENTCONTROLLER_EXPORT CurrentController : public ::dynamicgraph::Ent /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[CurrentController-" + name + "] " + msg) << '\n'; - } - protected: RobotUtilShrPtr m_robot_util; bool m_initSucceeded; /// true if the entity has been successfully initialized diff --git a/include/sot/torque_control/device-torque-ctrl.hh b/include/sot/torque_control/device-torque-ctrl.hh index 5b72328..3b3b40e 100644 --- a/include/sot/torque_control/device-torque-ctrl.hh +++ b/include/sot/torque_control/device-torque-ctrl.hh @@ -75,10 +75,6 @@ class DeviceTorqueCtrl : public dgsot::Device { virtual void integrate(const double& dt); void computeForwardDynamics(); - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[DeviceTorqueCtrl] " + msg) << '\n'; - } - /// \brief Current integration step. double timestep_; bool m_initSucceeded; diff --git a/include/sot/torque_control/free-flyer-locator.hh b/include/sot/torque_control/free-flyer-locator.hh index 083c7fb..987a374 100644 --- a/include/sot/torque_control/free-flyer-locator.hh +++ b/include/sot/torque_control/free-flyer-locator.hh @@ -78,10 +78,6 @@ class SOTFREEFLYERLOCATOR_EXPORT FreeFlyerLocator : public ::dynamicgraph::Entit /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[FreeFlyerLocator-" + name + "] " + msg) << '\n'; - } - protected: bool m_initSucceeded; /// true if the entity has been successfully initialized pinocchio::Model* m_model; /// Pinocchio robot model diff --git a/include/sot/torque_control/imu_offset_compensation.hh b/include/sot/torque_control/imu_offset_compensation.hh index 10a512a..86de0a4 100644 --- a/include/sot/torque_control/imu_offset_compensation.hh +++ b/include/sot/torque_control/imu_offset_compensation.hh @@ -66,9 +66,6 @@ class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation : public ::dynamicgr /* --- METHODS --- */ void update_offset_impl(int iter); - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) { - logger_.stream(t) << ("[ImuOffsetCompensation-" + name + "] " + msg, t, file, line); - } protected: bool m_initSucceeded; /// true if the entity has been successfully initialized diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 49a020e..f9a44d3 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -120,6 +120,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_tau, dynamicgraph::Vector); DECLARE_SIGNAL_IN(w_com, double); DECLARE_SIGNAL_IN(w_am, double); @@ -147,7 +148,8 @@ 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); @@ -182,7 +184,9 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_OUT(am_dL_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(right_foot_pos_quat, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(left_foot_pos_quat, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector); @@ -195,7 +199,6 @@ 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(time, 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); @@ -204,10 +207,6 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: double m_dt; /// control loop time period double m_t; diff --git a/include/sot/torque_control/joint-torque-controller.hh b/include/sot/torque_control/joint-torque-controller.hh index 0d7b086..d76c1f4 100644 --- a/include/sot/torque_control/joint-torque-controller.hh +++ b/include/sot/torque_control/joint-torque-controller.hh @@ -113,10 +113,6 @@ class SOTJOINTTORQUECONTROLLER_EXPORT JointTorqueController : public ::dynamicgr RobotUtilShrPtr m_robot_util; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/include/sot/torque_control/joint-trajectory-generator.hh b/include/sot/torque_control/joint-trajectory-generator.hh index d8a23e9..803ebdc 100644 --- a/include/sot/torque_control/joint-trajectory-generator.hh +++ b/include/sot/torque_control/joint-trajectory-generator.hh @@ -147,10 +147,6 @@ class SOTJOINTTRAJECTORYGENERATOR_EXPORT JointTrajectoryGenerator : public ::dyn /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[JointTrajectoryGenerator-" + name + "] " + msg) << '\n'; - } - protected: enum JTG_Status { JTG_STOP, JTG_SINUSOID, JTG_MIN_JERK, JTG_LIN_CHIRP, JTG_TRIANGLE, JTG_CONST_ACC, JTG_TEXT_FILE }; diff --git a/include/sot/torque_control/numerical-difference.hh b/include/sot/torque_control/numerical-difference.hh index 8bc9761..7ccd8e8 100644 --- a/include/sot/torque_control/numerical-difference.hh +++ b/include/sot/torque_control/numerical-difference.hh @@ -94,11 +94,6 @@ class SOTNUMERICALDIFFERENCE_EXPORT NumericalDifference : public ::dynamicgraph: */ void init(const double& timestep, const int& sigSize, const double& delay, const int& polyOrder); - protected: - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - public: /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; diff --git a/include/sot/torque_control/position-controller.hh b/include/sot/torque_control/position-controller.hh index 34fdafb..e80bb05 100644 --- a/include/sot/torque_control/position-controller.hh +++ b/include/sot/torque_control/position-controller.hh @@ -72,10 +72,6 @@ class SOTPOSITIONCONTROLLER_EXPORT PositionController : public ::dynamicgraph::E /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[PositionController-" + name + "] " + msg) << '\n'; - } - protected: RobotUtilShrPtr m_robot_util; /// Robot Util Eigen::VectorXd m_pwmDes; diff --git a/include/sot/torque_control/se3-trajectory-generator.hh b/include/sot/torque_control/se3-trajectory-generator.hh index acf1255..937d0b3 100644 --- a/include/sot/torque_control/se3-trajectory-generator.hh +++ b/include/sot/torque_control/se3-trajectory-generator.hh @@ -121,10 +121,6 @@ class SOTSE3TRAJECTORYGENERATOR_EXPORT SE3TrajectoryGenerator : public ::dynamic /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[SE3TrajectoryGenerator-" + name + "] " + msg) << '\n'; - } - protected: enum TG_Status { TG_STOP, diff --git a/include/sot/torque_control/simple-inverse-dyn.hh b/include/sot/torque_control/simple-inverse-dyn.hh index 8c4c0b9..6f816c7 100644 --- a/include/sot/torque_control/simple-inverse-dyn.hh +++ b/include/sot/torque_control/simple-inverse-dyn.hh @@ -41,7 +41,7 @@ /* Pinocchio */ #include #include -#include "pinocchio/algorithm/joint-configuration.hpp" +#include #include #include @@ -92,6 +92,9 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(tau_measured, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_tau, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector); @@ -117,6 +120,7 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit DECLARE_SIGNAL_IN(q, dynamicgraph::Vector); DECLARE_SIGNAL_IN(v, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_measured, dynamicgraph::Vector); DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector); @@ -125,20 +129,19 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); /* --- COMMANDS --- */ void init(const double& dt, const std::string& robotRef); virtual void setControlOutputType(const std::string& type); - void updateComOffset(); + void updateComOffset(const dynamicgraph::Vector& com_measured); /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: double m_dt; /// control loop time period double m_t; /// current time @@ -146,6 +149,9 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit bool m_enabled; /// True if controler is enabled bool m_firstTime; /// True at the first iteration of the controller + int m_frame_id_rf; /// frame id of right foot + int m_frame_id_lf; /// frame id of left foot + /// TSID /// Robot tsid::robots::RobotWrapper* m_robot; diff --git a/include/sot/torque_control/torque-offset-estimator.hh b/include/sot/torque_control/torque-offset-estimator.hh index addce40..e18fd98 100644 --- a/include/sot/torque_control/torque-offset-estimator.hh +++ b/include/sot/torque_control/torque-offset-estimator.hh @@ -90,10 +90,6 @@ class TORQUEOFFSETESTIMATOR_EXPORT TorqueOffsetEstimator : public ::dynamicgraph // stdAlignedVector stdVecJointTorqueOffsets; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - private: enum { PRECOMPUTATION, INPROGRESS, COMPUTED } sensor_offset_status; diff --git a/include/sot/torque_control/trace-player.hh b/include/sot/torque_control/trace-player.hh index 759cc2c..5b13003 100644 --- a/include/sot/torque_control/trace-player.hh +++ b/include/sot/torque_control/trace-player.hh @@ -74,10 +74,6 @@ class SOTTRACEPLAYER_EXPORT TracePlayer : public ::dynamicgraph::Entity { /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: typedef dynamicgraph::Vector DataType; typedef std::list DataHistoryType; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 566b42b..300c5d7 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -19,6 +19,7 @@ #include +#include "pinocchio/algorithm/center-of-mass.hpp" #include #include @@ -82,7 +83,7 @@ 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 \ @@ -125,6 +126,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_kd_postureSIN \ << m_kp_posSIN \ << m_kd_posSIN \ + << m_kp_tauSIN \ << m_w_comSIN \ << m_w_amSIN \ << m_w_feetSIN \ @@ -148,7 +150,8 @@ 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 \ @@ -181,7 +184,9 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_am_dL_desSOUT \ << m_base_orientationSOUT \ << m_left_foot_posSOUT \ + << m_left_foot_pos_quatSOUT \ << m_right_foot_posSOUT \ + << m_right_foot_pos_quatSOUT \ << m_left_hand_posSOUT \ << m_right_hand_posSOUT \ << m_left_foot_velSOUT \ @@ -194,7 +199,6 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_right_hand_accSOUT \ << m_right_foot_acc_desSOUT \ << m_left_foot_acc_desSOUT \ - << m_timeSOUT /// Define EntityClassName here rather than in the header file /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. @@ -252,6 +256,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kp_tau, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(w_com, double), CONSTRUCT_SIGNAL_IN(w_am, double), CONSTRUCT_SIGNAL_IN(w_feet, double), @@ -275,7 +280,8 @@ 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), @@ -307,7 +313,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_OUT(am_dL_des, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(right_foot_pos_quat, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(left_foot_pos_quat, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(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), @@ -320,7 +328,6 @@ 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(time, double, m_tau_desSOUT), CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN), m_t(0.0), m_initSucceeded(false), @@ -392,6 +399,7 @@ void InverseDynamicsBalanceController::updateComOffset() { void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { + SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); bool res = m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -411,6 +419,7 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran void InverseDynamicsBalanceController::removeLeftFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { + SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); bool res = m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -464,9 +473,10 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit if (m_contactState == LEFT_SUPPORT) { SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); - TrajectorySample traj_ref = m_taskRF->getReference(); + // TrajectorySample traj_ref = m_taskRF->getReference(); pinocchio::SE3 ref; - vectorToSE3(traj_ref.pos, ref); + // vectorToSE3(traj_ref.pos, ref); + ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); m_contactRF->setReference(ref); m_invDyn->addRigidContact(*m_contactRF, w_forces); m_invDyn->removeTask(m_taskRF->name(), transitionTime); @@ -478,9 +488,10 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti if (m_contactState == RIGHT_SUPPORT) { SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); - TrajectorySample traj_ref = m_taskLF->getReference(); + // TrajectorySample traj_ref = m_taskLF->getReference(); pinocchio::SE3 ref; - vectorToSE3(traj_ref.pos, ref); + // vectorToSE3(traj_ref.pos, ref); + ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); m_contactLF->setReference(ref); m_invDyn->addRigidContact(*m_contactLF, w_forces); m_invDyn->removeTask(m_taskLF->name(), transitionTime); @@ -533,13 +544,9 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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); - const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0); - const VectorN& gear_ratios_sot = m_gear_ratiosSIN(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); @@ -560,12 +567,18 @@ 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->nq()); m_v_sot.setZero(m_robot->nv()); @@ -659,6 +672,25 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_frame_id_rh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_frame_id_lh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); + // COM OFFSET + if (m_com_measuredSIN.isPlugged()){ + const VectorN6& q_robot = m_qSIN(0); + assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_robot = m_vSIN(0); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const dg::Vector& com_measured = m_com_measuredSIN(0); + assert(com_measured.size() == 3); + 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()); m_hqpSolver_60_36_34 = @@ -859,8 +891,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); getProfiler().start(PROFILE_PREPARE_INV_DYN); - // m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); - // m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); + m_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; @@ -931,8 +963,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_firstTime) { m_firstTime = false; - m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); - m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); // m_robot->computeAllTerms(m_invDyn->data(), q, v); pinocchio::SE3 H_lf = m_robot->position( @@ -972,10 +1002,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; } @@ -1067,17 +1097,24 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, 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; } @@ -1398,6 +1435,24 @@ DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos_quat, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal left_foot_pos before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + m_tau_desSOUT(iter); + const dg::Vector& x_lf_ref = m_lf_ref_posSIN(iter); + pinocchio::SE3 oMi; + oMi.translation(x_lf_ref.head<3>() ); + oMi.rotation( Eigen::Map >(&x_lf_ref(3), 3, 3) ); + s.resize(7); + // m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, oMi); + tsid::math::SE3ToXYZQUAT(oMi, s); + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(left_hand_pos, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal left hand_pos before initialization!"); @@ -1426,6 +1481,24 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos_quat, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal rigt_foot_pos before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + // m_tau_desSOUT(iter); + const dg::Vector& x_rf_ref = m_rf_ref_posSIN(iter); + pinocchio::SE3 oMi; + oMi.translation(x_rf_ref.head<3>() ); + oMi.rotation( Eigen::Map >(&x_rf_ref(3), 3, 3) ); + s.resize(7); + // m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, oMi); + tsid::math::SE3ToXYZQUAT(oMi, s); + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(right_hand_pos, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal right_hand_pos before initialization!"); @@ -1559,15 +1632,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc_des, dynamicgraph::Vector) { return s; } -DEFINE_SIGNAL_OUT_FUNCTION(time, double) { - if (!m_initSucceeded) { - SEND_WARNING_STREAM_MSG("Cannot compute signal time before initialization!"); - return s; - } - m_tau_desSOUT(iter); - return m_t; -} - /* --- COMMANDS ---------------------------------------------------------- */ /* ------------------------------------------------------------------- */ diff --git a/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index 6321fee..d2ab3e0 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -25,6 +25,8 @@ #include #include +#include "pinocchio/algorithm/center-of-mass.hpp" +#include #include #include @@ -89,6 +91,8 @@ using namespace dg::sot; << m_w_postureSIN \ << m_kp_posSIN \ << m_kd_posSIN \ + << m_tau_measuredSIN \ + << m_kp_tauSIN \ << m_com_ref_posSIN \ << m_com_ref_velSIN \ << m_com_ref_accSIN \ @@ -111,9 +115,11 @@ using namespace dg::sot; << m_w_waistSIN \ << m_qSIN \ << m_vSIN \ + << m_com_measuredSIN \ << m_active_jointsSIN -#define OUTPUT_SIGNALS m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT +#define OUTPUT_SIGNALS m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT << m_comSOUT \ + << m_right_foot_posSOUT << m_left_foot_posSOUT /// Define EntityClassName here rather than in the header file @@ -142,6 +148,8 @@ SimpleInverseDyn(const std::string& name) , CONSTRUCT_SIGNAL_IN(w_posture, double) , CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(tau_measured, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(kp_tau, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector) @@ -164,6 +172,7 @@ SimpleInverseDyn(const std::string& name) , CONSTRUCT_SIGNAL_IN(w_waist, double) , CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(com_measured, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN) , CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS) @@ -171,6 +180,9 @@ SimpleInverseDyn(const std::string& name) , CONSTRUCT_SIGNAL_OUT(v_des, dg::Vector, m_dv_desSOUT) , CONSTRUCT_SIGNAL_OUT(q_des, dg::Vector, m_v_desSOUT) , CONSTRUCT_SIGNAL_OUT(u, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT << m_v_desSOUT << m_q_desSOUT) + , CONSTRUCT_SIGNAL_OUT(com, dg::Vector, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT) , m_t(0.0) , m_initSucceeded(false) , m_enabled(false) @@ -196,14 +208,15 @@ SimpleInverseDyn(const std::string& name) "Control type: velocity or torque (string)"))); addCommand("updateComOffset", - makeCommandVoid0(*this, &SimpleInverseDyn::updateComOffset, - docCommandVoid0("Update the offset on the CoM based on the CoP measurement."))); + makeCommandVoid1(*this, &SimpleInverseDyn::updateComOffset, + docCommandVoid1("Update the offset on the CoM based on its measurement.", + "Measured CoM"))); } -void SimpleInverseDyn::updateComOffset() { +void SimpleInverseDyn::updateComOffset(const dg::Vector& com_measured) { const Vector3 & com = m_robot->com(m_invDyn->data()); - m_com_offset = com; + m_com_offset = com_measured - com; SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO); } @@ -319,6 +332,28 @@ void SimpleInverseDyn::init(const double& dt, const std::string& robotRef) { m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); + m_frame_id_rf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); + m_frame_id_lf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name); + + // COM OFFSET + if (m_com_measuredSIN.isPlugged()){ + const VectorN6& q_robot = m_qSIN(0); + assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_robot = m_vSIN(0); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const dg::Vector& com_measured = m_com_measuredSIN(0); + assert(com_measured.size() == 3); + SEND_MSG("COM_measured: " + toString(com_measured), MSG_TYPE_ERROR); + + m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf); + + pinocchio::Data data = pinocchio::Data(m_robot->model()); + pinocchio::centerOfMass(m_robot->model(), data, m_q_urdf, m_v_urdf); + m_com_offset = com_measured - data.com[0]; + SEND_MSG("Update COM: " + toString(m_com_offset), MSG_TYPE_ERROR); + } + m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog-fast"); m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn()); @@ -425,6 +460,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().start(PROFILE_PREPARE_INV_DYN); + // Update tasks m_sampleCom.pos = x_com_ref - m_com_offset; m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; @@ -472,21 +508,22 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces); if (m_firstTime) { - m_firstTime = false; + SEND_ERROR_STREAM_MSG("FIRST TIME iter " + toString(iter)); + 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_ERROR); 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_ERROR); } 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 +531,13 @@ 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. + SEND_ERROR_STREAM_MSG("CONTROL_OUTPUT_TORQUE iter " + toString(iter)); + 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 +552,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 +629,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; } From e23325058a7fb0a8ffca9e6918f0beea6062fe4d Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 25 May 2020 11:52:43 +0200 Subject: [PATCH 04/47] =?UTF-8?q?[Doc]=C2=A0Add=20documentation=20for=20wa?= =?UTF-8?q?lk=20and=20bellStep=20simulation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- doc/Overview.md | 6 ++++- doc/bellStepRun.md | 40 +++++++++++++++++++++++++++++++ doc/running.md | 2 +- doc/walkRun.md | 59 ++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 105 insertions(+), 2 deletions(-) create mode 100644 doc/bellStepRun.md create mode 100644 doc/walkRun.md diff --git a/doc/Overview.md b/doc/Overview.md index efffacb..a6d3652 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 efefab8..3109273 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). From c05584c77f38f97b8f65f8604e88a03fc37da9f0 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 25 May 2020 14:26:18 +0200 Subject: [PATCH 05/47] [Inv_Dyn] Clean useless sendMsg --- src/inverse-dynamics-balance-controller.cpp | 2 +- src/simple-inverse-dyn.cpp | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 300c5d7..030b27e 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -1113,7 +1113,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { 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); + s += kp_tau.cwiseProduct(tau_measured - s); return s; } diff --git a/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index d2ab3e0..2f1e73d 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -508,7 +508,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces); if (m_firstTime) { - SEND_ERROR_STREAM_MSG("FIRST TIME iter " + toString(iter)); 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); @@ -518,12 +517,12 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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_ERROR); + 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_ERROR); + 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)) { @@ -533,7 +532,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { } 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. - SEND_ERROR_STREAM_MSG("CONTROL_OUTPUT_TORQUE iter " + toString(iter)); 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); } From 15c85a6b967a1ad6b674e569cfd8af19418dedff Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 25 May 2020 14:26:47 +0200 Subject: [PATCH 06/47] [DDP] Add ddp files to plugin list --- src/CMakeLists.txt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 46ce277..09214c4 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,13 +18,10 @@ SET(plugins se3-trajectory-generator torque-offset-estimator trace-player + ddp-actuator-solver + ddp_pyrene_actuator_solver ) -IF(DDP_ACTUATOR_SOLVER_FOUND) - SET(plugins ${plugins} ddp-actuator-solver) - SET(plugins ${plugins} ddp_pyrene_actuator_solver) -ENDIF(DDP_ACTUATOR_SOLVER_FOUND) - FOREACH(plugin ${plugins}) GET_FILENAME_COMPONENT(LIBRARY_NAME ${plugin} NAME) ADD_LIBRARY(${LIBRARY_NAME} SHARED ${plugin}) From 1382c0a7268a6786ddc66c11cc2c81a3f87aca1c Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 16 Jun 2020 10:57:38 +0200 Subject: [PATCH 07/47] [Inv_Dyn] Add control mode (vel/torque) and change signal phase (int) In velocity control use the base estimator to estimate feet positions and CoM position. Use them in the feet and CoM tasks. --- .../inverse-dynamics-balance-controller.hh | 17 +- src/inverse-dynamics-balance-controller.cpp | 197 ++++++++++++++++-- 2 files changed, 199 insertions(+), 15 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index f9a44d3..ccb8f2b 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -39,6 +39,7 @@ #include #include #include +// #include "tsid/tasks/task-angular-momentum-integral-equality.hpp" #include /* HELPER */ @@ -52,6 +53,10 @@ namespace dynamicgraph { namespace sot { namespace torque_control { +enum ControlOutput { CONTROL_OUTPUT_VELOCITY = 0, CONTROL_OUTPUT_TORQUE = 1, CONTROL_OUTPUT_SIZE = 2 }; + +const std::string ControlOutput_s[] = {"velocity", "torque"}; + /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ @@ -68,6 +73,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle void init(const double& dt, const std::string& robotRef); void updateComOffset(); + virtual void setControlOutputType(const std::string& type); void removeRightFootContact(const double& transitionTime); void removeLeftFootContact(const double& transitionTime); void addRightFootContact(const double& transitionTime); @@ -155,6 +161,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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 @@ -177,6 +184,7 @@ 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); @@ -187,6 +195,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_OUT(right_foot_pos_quat, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_foot_pos_quat, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(lf_est, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(rf_est, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector); @@ -292,6 +302,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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 @@ -303,6 +314,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle typedef pinocchio::Data::Matrix6x Matrix6x; + pinocchio::Data m_estim_data; Matrix6x m_J_RF; Matrix6x m_J_LF; Eigen::ColPivHouseholderQR m_J_RF_QR; @@ -310,8 +322,9 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::math::Vector6 m_v_RF_int; tsid::math::Vector6 m_v_LF_int; - unsigned int m_timeLast; - RobotUtilShrPtr m_robot_util; + unsigned int m_timeLast; /// Final time of the control loop + RobotUtilShrPtr m_robot_util; /// Share pointer to the robot utils methods + ControlOutput m_ctrlMode; /// ctrl mode desired for the output (velocity or torque) }; // class InverseDynamicsBalanceController } // namespace torque_control diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 030b27e..a4a0a08 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -7,6 +7,10 @@ #include +#include +#include +#include + #include #include #include @@ -19,7 +23,6 @@ #include -#include "pinocchio/algorithm/center-of-mass.hpp" #include #include @@ -157,6 +160,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_wrench_baseSIN \ << m_wrench_left_footSIN \ << m_wrench_right_footSIN \ + << m_ref_phaseSIN \ << m_active_jointsSIN #define OUTPUT_SIGNALS m_tau_desSOUT \ @@ -177,6 +181,7 @@ 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 \ @@ -187,6 +192,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_left_foot_pos_quatSOUT \ << m_right_foot_posSOUT \ << m_right_foot_pos_quatSOUT \ + << m_lf_estSOUT \ + << m_rf_estSOUT \ << m_left_hand_posSOUT \ << m_right_hand_posSOUT \ << m_left_foot_velSOUT \ @@ -287,6 +294,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(wrench_base, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(ref_phase, int), CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector), CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS), CONSTRUCT_SIGNAL_OUT(M, dg::Matrix, m_tau_desSOUT), @@ -306,6 +314,7 @@ 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), @@ -315,7 +324,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_pos_quat, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT), - CONSTRUCT_SIGNAL_OUT(left_foot_pos_quat, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(left_foot_pos_quat, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(lf_est, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(rf_est, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_hand_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_hand_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_vel, dg::Vector, m_tau_desSOUT), @@ -337,7 +348,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st m_rightHandState(TASK_RIGHT_HAND_OFF), m_leftHandState(TASK_LEFT_HAND_OFF), m_timeLast(0), - m_robot_util(RefVoidRobotUtil()) { + m_robot_util(RefVoidRobotUtil()), + m_ctrlMode(CONTROL_OUTPUT_TORQUE) { RESETDEBUG5(); Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); @@ -350,6 +362,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st m_zmp_LF.setZero(); m_zmp.setZero(); m_com_offset.setZero(); + m_dcom_offset.setZero(); m_v_RF_int.setZero(); m_v_LF_int.setZero(); @@ -362,6 +375,11 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st makeCommandVoid0(*this, &InverseDynamicsBalanceController::updateComOffset, docCommandVoid0("Update the offset on the CoM based on the CoP measurement."))); + /* SET of control output type. */ + addCommand("setControlOutputType", + makeCommandVoid1(*this, &InverseDynamicsBalanceController::setControlOutputType, + docCommandVoid1("Set the type of control output.", + "Control type: velocity or torque (string)"))); addCommand("removeRightFootContact", makeCommandVoid1( *this, &InverseDynamicsBalanceController::removeRightFootContact, @@ -397,6 +415,16 @@ void InverseDynamicsBalanceController::updateComOffset() { SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO); } +void InverseDynamicsBalanceController::setControlOutputType(const std::string& type) { + for (int i = 0; i < CONTROL_OUTPUT_SIZE; i++) + if (type == ControlOutput_s[i]) { + m_ctrlMode = (ControlOutput)i; + sotDEBUG(25) << "Control output type: " << ControlOutput_s[i] << endl; + return; + } + sotDEBUG(25) << "Unrecognized control output type: " << type << endl; +} + void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); @@ -478,8 +506,13 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit // vectorToSE3(traj_ref.pos, ref); ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); m_contactRF->setReference(ref); - m_invDyn->addRigidContact(*m_contactRF, w_forces); m_invDyn->removeTask(m_taskRF->name(), transitionTime); + bool res = m_invDyn->addRigidContact(*m_contactRF, w_forces); + if (!res) { + const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); + SEND_MSG("Error while adding right foot contact." + tsid::solvers::HQPDataToString(hqpData, false), + MSG_TYPE_ERROR); + } m_contactState = DOUBLE_SUPPORT; } } @@ -493,8 +526,13 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti // vectorToSE3(traj_ref.pos, ref); ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); m_contactLF->setReference(ref); - m_invDyn->addRigidContact(*m_contactLF, w_forces); m_invDyn->removeTask(m_taskLF->name(), transitionTime); + bool res = m_invDyn->addRigidContact(*m_contactLF, w_forces); + if (!res) { + const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); + SEND_MSG("Error while adding left foot contact." + tsid::solvers::HQPDataToString(hqpData, false), + MSG_TYPE_ERROR); + } m_contactState = DOUBLE_SUPPORT; } } @@ -590,6 +628,8 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_J_RF.setZero(6, m_robot->nv()); m_J_LF.setZero(6, m_robot->nv()); + m_estim_data = pinocchio::Data(m_robot->model()); + m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot); // CONTACT 6D TASKS @@ -773,6 +813,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; @@ -836,31 +892,73 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const double& w_forces = m_w_forcesSIN(iter); const double & w_base_orientation = m_w_base_orientationSIN(iter); + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // BASE ESTIMATOR computation to have com and feet estimations + // Used to regulate on estimation (desired value of tasks are the estimations) + // -> only in velocity because close the TSID loop on itself (v_des, q_des) + // In torque close loop on the (q,v) of the base estimator (with freeflyer) -> not needed + Vector q_base_estimator, v_base_estimator; + q_base_estimator.setZero(m_robot->nq()); + v_base_estimator.setZero(m_robot->nv()); + m_robot_util->config_sot_to_urdf(q_sot, q_base_estimator); + m_robot_util->velocity_sot_to_urdf(q_base_estimator, v_sot, v_base_estimator); + pinocchio::computeAllTerms(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); + pinocchio::centerOfMass(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); + } + if (m_contactState == LEFT_SUPPORT || m_contactState == LEFT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); const Vector6& kp_feet = m_kp_feetSIN(iter); const Vector6& kd_feet = m_kd_feetSIN(iter); - m_sampleRF.pos = x_rf_ref; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // Right foot estimation + Eigen::Matrix rf_estim, rf_data; + pinocchio::SE3 rf_estim_se3, rf_data_se3; + m_robot->framePosition(m_estim_data, m_frame_id_rf, rf_estim_se3); + tsid::math::SE3ToVector(rf_estim_se3, rf_estim); + m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, rf_data_se3); + tsid::math::SE3ToVector(rf_data_se3, rf_data); + m_sampleRF.pos = x_rf_ref - rf_estim + rf_data; + } else { + m_sampleRF.pos = x_rf_ref; + } + m_sampleRF.vel = dx_rf_ref; m_sampleRF.acc = ddx_rf_ref; m_taskRF->setReference(m_sampleRF); m_taskRF->Kp(kp_feet); m_taskRF->Kd(kd_feet); + } else if (m_contactState == RIGHT_SUPPORT || m_contactState == RIGHT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_lf_ref = m_lf_ref_posSIN(iter); const Vector6& dx_lf_ref = m_lf_ref_velSIN(iter); const Vector6& ddx_lf_ref = m_lf_ref_accSIN(iter); const Vector6& kp_feet = m_kp_feetSIN(iter); const Vector6& kd_feet = m_kd_feetSIN(iter); - m_sampleLF.pos = x_lf_ref; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // Left foot estimation + Eigen::Matrix lf_estim, lf_data; + pinocchio::SE3 lf_estim_se3, lf_data_se3; + m_robot->framePosition(m_estim_data, m_frame_id_lf, lf_estim_se3); + tsid::math::SE3ToVector(lf_estim_se3, lf_estim); + m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, lf_data_se3); + tsid::math::SE3ToVector(lf_data_se3, lf_data); + m_sampleLF.pos = x_lf_ref - lf_estim + lf_data; + } else { + m_sampleLF.pos = x_lf_ref; + } + m_sampleLF.vel = dx_lf_ref; m_sampleLF.acc = ddx_lf_ref; m_taskLF->setReference(m_sampleLF); m_taskLF->Kp(kp_feet); m_taskLF->Kd(kd_feet); } + if (m_rightHandState == TASK_RIGHT_HAND_ON /*|| m_rightHandState == TASK_RIGHT_HAND_TRANSITION*/) { const Eigen::Matrix& x_rh_ref = m_rh_ref_posSIN(iter); const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter); @@ -891,10 +989,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); getProfiler().start(PROFILE_PREPARE_INV_DYN); - m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); - m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); - - m_sampleCom.pos = x_com_ref - m_com_offset; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // COM estimation + const Vector3& com = m_robot->com(m_invDyn->data()); + // const Vector3& dcom = m_robot->com_vel(m_invDyn->data()); + // m_dcom_offset = dcom - data.vcom[0]; + m_sampleCom.pos = x_com_ref - m_estim_data.com[0] + com; //- m_com_offset + } else { + m_sampleCom.pos = x_com_ref - m_com_offset; + } m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; m_taskCom->setReference(m_sampleCom); @@ -963,6 +1067,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_firstTime) { m_firstTime = false; + m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); // m_robot->computeAllTerms(m_invDyn->data(), q, v); pinocchio::SE3 H_lf = m_robot->position( @@ -980,7 +1086,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); @@ -1104,7 +1215,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { 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); + const VectorN& tau_measured = m_tau_measuredSIN(iter); assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); m_q_desSOUT(iter); @@ -1394,6 +1505,32 @@ DEFINE_SIGNAL_OUT_FUNCTION(com, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(com_est, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal com_est before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + if (s.size() != 3) s.resize(3); + + const VectorN6& q_sot = m_qSIN(iter); + assert(q_sot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_sot = m_vSIN(iter); + assert(v_sot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + + Vector q_base_estimator, v_base_estimator; + q_base_estimator.setZero(m_robot->nq()); + v_base_estimator.setZero(m_robot->nv()); + m_robot_util->config_sot_to_urdf(q_sot, q_base_estimator); + m_robot_util->velocity_sot_to_urdf(q_base_estimator, v_sot, v_base_estimator); + + pinocchio::Data data = pinocchio::Data(m_robot->model()); + pinocchio::centerOfMass(m_robot->model(), data, q_base_estimator, v_base_estimator); + s = data.com[0]; + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(com_vel, dynamicgraph::Vector) { if (!m_initSucceeded) { std::ostringstream oss("Cannot compute signal com_vel before initialization! iter:"); @@ -1442,7 +1579,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos_quat, dynamicgraph::Vector) { SEND_WARNING_STREAM_MSG(oss.str()); return s; } - m_tau_desSOUT(iter); + // m_tau_desSOUT(iter); const dg::Vector& x_lf_ref = m_lf_ref_posSIN(iter); pinocchio::SE3 oMi; oMi.translation(x_lf_ref.head<3>() ); @@ -1453,6 +1590,23 @@ DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos_quat, dynamicgraph::Vector) { 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!"); @@ -1499,6 +1653,23 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos_quat, dynamicgraph::Vector) { 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!"); From 30ff3c6ee493ffe5da0e90a2db8e7c8aa60bee09 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 16 Jun 2020 15:51:42 +0200 Subject: [PATCH 08/47] [Inv_Dyn] Wrong init size for m_q_sot (39 -> 38) --- src/inverse-dynamics-balance-controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index a4a0a08..6d7e4b5 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -618,7 +618,7 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_robot->gear_ratios(gear_ratios_urdf); } - m_q_sot.setZero(m_robot->nq()); + 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); From da0fd7d6186d26796af3d90962641a58d9abead4 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 6 Apr 2020 16:33:59 +0200 Subject: [PATCH 09/47] [Angular Momentum] Add AM task in inverse-dynamic-balance-controller entity Try to use it with talos-torque-control package and trajectories from multicontact_api. Works with walk on spot trajectories. --- .../inverse-dynamics-balance-controller.hh | 34 ++- src/inverse-dynamics-balance-controller.cpp | 241 ++++++++++++++++-- 2 files changed, 243 insertions(+), 32 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 0f3a68f..49a020e 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -30,6 +30,7 @@ /* Pinocchio */ #include #include +#include "pinocchio/algorithm/joint-configuration.hpp" #include #include @@ -37,6 +38,7 @@ #include #include #include +#include #include /* HELPER */ @@ -79,6 +81,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(am_ref_L, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(am_ref_dL, dynamicgraph::Vector); DECLARE_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector); @@ -106,6 +110,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_am, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kd_am, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_feet, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_feet, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_hands, dynamicgraph::Vector); @@ -116,6 +122,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(kd_pos, 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); @@ -152,6 +159,10 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); @@ -167,6 +178,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_OUT(com_vel, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(com_acc, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(com_acc_des, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(am_dL, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(am_dL_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); @@ -182,6 +195,7 @@ 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(time, 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); @@ -243,6 +257,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::contacts::Contact6d* m_contactRH; tsid::contacts::Contact6d* m_contactLH; tsid::tasks::TaskComEquality* m_taskCom; + tsid::tasks::TaskAMEquality* m_taskAM; + tsid::tasks::TaskSE3Equality* m_taskWaist; tsid::tasks::TaskSE3Equality* m_taskRF; tsid::tasks::TaskSE3Equality* m_taskLF; tsid::tasks::TaskSE3Equality* m_taskRH; @@ -251,18 +267,28 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::tasks::TaskJointPosture* m_taskBlockedJoints; tsid::trajectories::TrajectorySample m_sampleCom; + tsid::trajectories::TrajectorySample m_sampleAM; tsid::trajectories::TrajectorySample m_sampleRF; tsid::trajectories::TrajectorySample m_sampleLF; tsid::trajectories::TrajectorySample m_sampleRH; tsid::trajectories::TrajectorySample m_sampleLH; + tsid::trajectories::TrajectorySample m_sampleWaist; tsid::trajectories::TrajectorySample m_samplePosture; double m_w_com; + double m_w_am; double m_w_posture; double m_w_hands; + double m_w_base_orientation; + + tsid::math::Vector m_dv_sot; /// desired accelerations (sot order) + tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order) + tsid::math::Vector m_v_sot; /// desired velocities (sot order) + tsid::math::Vector m_v_urdf; /// desired and current velocities (urdf order) (close the TSID loop on it) + tsid::math::Vector m_q_sot; /// desired positions (sot order) + tsid::math::Vector m_q_urdf; /// desired and current positions (urdf order) (close the TSID loop on it) + tsid::math::Vector m_tau_sot; /// desired torques (sot order) - tsid::math::Vector m_dv_sot; /// desired accelerations (sot order) - tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order) tsid::math::Vector m_f; /// desired force coefficients (24d) tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot @@ -275,9 +301,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::math::Vector3 m_zmp_LF; /// 3d zmp left foot tsid::math::Vector3 m_zmp_RF; /// 3d zmp left foot tsid::math::Vector3 m_zmp; /// 3d global zmp - tsid::math::Vector m_tau_sot; - tsid::math::Vector m_q_urdf; - tsid::math::Vector m_v_urdf; + typedef pinocchio::Data::Matrix6x Matrix6x; Matrix6x m_J_RF; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 018dd15..9be6795 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -87,6 +87,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; #define INPUT_SIGNALS m_com_ref_posSIN \ << m_com_ref_velSIN \ << m_com_ref_accSIN \ + << m_am_ref_LSIN \ + << m_am_ref_dLSIN \ << m_rf_ref_posSIN \ << m_rf_ref_velSIN \ << m_rf_ref_accSIN \ @@ -113,6 +115,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_kd_constraintsSIN \ << m_kp_comSIN \ << m_kd_comSIN \ + << m_kp_amSIN \ + << m_kd_amSIN \ << m_kp_feetSIN \ << m_kd_feetSIN \ << m_kp_handsSIN \ @@ -122,6 +126,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_kp_posSIN \ << m_kd_posSIN \ << m_w_comSIN \ + << m_w_amSIN \ << m_w_feetSIN \ << m_w_handsSIN \ << m_w_postureSIN \ @@ -154,6 +159,9 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; #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 \ @@ -169,6 +177,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_com_velSOUT \ << m_com_accSOUT \ << m_com_acc_desSOUT \ + << m_am_dLSOUT \ + << m_am_dL_desSOUT \ << m_base_orientationSOUT \ << m_left_foot_posSOUT \ << m_right_foot_posSOUT \ @@ -183,7 +193,8 @@ 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_timeSOUT /// Define EntityClassName here rather than in the header file /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. @@ -203,6 +214,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(am_ref_L, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(am_ref_dL, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(rf_ref_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(rf_ref_vel, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(rf_ref_acc, dynamicgraph::Vector), @@ -229,6 +242,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(kd_constraints, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_com, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_com, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kp_am, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kd_am, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_feet, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_feet, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_hands, dynamicgraph::Vector), @@ -238,6 +253,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_pos, 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), @@ -269,6 +285,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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), @@ -284,6 +303,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_OUT(com_vel, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(com_acc, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(com_acc_des, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(am_dL, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(am_dL_des, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT), @@ -299,6 +320,7 @@ 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(time, double, m_tau_desSOUT), CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN), m_t(0.0), m_initSucceeded(false), @@ -341,6 +363,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, @@ -362,7 +392,6 @@ void InverseDynamicsBalanceController::updateComOffset() { void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); bool res = m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -370,18 +399,18 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran MSG_TYPE_ERROR); } const double& w_feet = m_w_feetSIN.accessCopy(); - m_invDyn->addMotionTask(*m_taskRF, w_feet, 1); + m_invDyn->addMotionTask(*m_taskRF, w_feet, 0); if (transitionTime > m_dt) { m_contactState = LEFT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; - } else + } else { m_contactState = LEFT_SUPPORT; + } } } void InverseDynamicsBalanceController::removeLeftFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); bool res = m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -389,12 +418,13 @@ void InverseDynamicsBalanceController::removeLeftFootContact(const double& trans MSG_TYPE_ERROR); } const double& w_feet = m_w_feetSIN.accessCopy(); - m_invDyn->addMotionTask(*m_taskLF, w_feet, 1); + m_invDyn->addMotionTask(*m_taskLF, w_feet, 0); if (transitionTime > m_dt) { m_contactState = RIGHT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; - } else + } else { m_contactState = RIGHT_SUPPORT; + } } } @@ -432,7 +462,7 @@ void InverseDynamicsBalanceController::addTaskLeftHand(/*const double& transitio void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime) { if (m_contactState == LEFT_SUPPORT) { - SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); + SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); m_invDyn->addRigidContact(*m_contactRF, w_forces); m_invDyn->removeTask(m_taskRF->name(), transitionTime); @@ -442,7 +472,7 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit void InverseDynamicsBalanceController::addLeftFootContact(const double& transitionTime) { if (m_contactState == RIGHT_SUPPORT) { - SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); + SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); m_invDyn->addRigidContact(*m_contactLF, w_forces); m_invDyn->removeTask(m_taskLF->name(), transitionTime); @@ -485,12 +515,16 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& const dg::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0); const Eigen::Vector3d& kp_com = m_kp_comSIN(0); const Eigen::Vector3d& kd_com = m_kd_comSIN(0); + const Eigen::Vector3d& kp_am = m_kp_amSIN(0); + const Eigen::Vector3d& kd_am = m_kd_amSIN(0); const dg::sot::Vector6d& kd_hands = m_kd_handsSIN(0); const dg::sot::Vector6d& kp_hands = m_kp_handsSIN(0); const dg::sot::Vector6d& kp_feet = m_kp_feetSIN(0); const dg::sot::Vector6d& kd_feet = m_kd_feetSIN(0); const VectorN& kp_posture = m_kp_postureSIN(0); const VectorN& kd_posture = m_kd_postureSIN(0); + const dg::sot::Vector6d& kp_base_orientation = m_kp_base_orientationSIN(0); + const dg::sot::Vector6d& kd_base_orientation = m_kd_base_orientationSIN(0); const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0); const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0); @@ -501,9 +535,10 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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); @@ -523,7 +558,9 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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->nq()); + 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); @@ -534,6 +571,7 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot); + // CONTACT 6D TASKS m_contactRF = new Contact6d("contact_rfoot", *m_robot, m_robot_util->m_foot_util.m_Right_Foot_Frame_Name, contactPoints, contactNormal, mu, fMin, fMaxRF); m_contactRF->Kp(kp_contact); @@ -546,16 +584,37 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_contactLF->Kd(kd_contact); m_invDyn->addRigidContact(*m_contactLF, w_forces); - if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { - m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones()); - m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones()); - } + if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { + m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones()); + m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones()); + } + // TASK COM m_taskCom = new TaskComEquality("task-com", *m_robot); m_taskCom->Kp(kp_com); m_taskCom->Kd(kd_com); - m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1); - + m_invDyn->addMotionTask(*m_taskCom, m_w_com, 0); + + // TASK ANGULAR MOMENTUM + m_taskAM = new TaskAMEquality("task-am", *m_robot); + m_taskAM->Kp(kp_am); + m_taskAM->Kd(kd_am); + m_invDyn->addMotionTask(*m_taskAM, m_w_am, 1); + + // TASK BASE ORIENTATION + m_taskWaist = new TaskSE3Equality("task-waist", *m_robot, "root_joint"); + m_taskWaist->Kp(kp_base_orientation); + m_taskWaist->Kd(kd_base_orientation); + // Add a Mask to the task which will select the vector dimensions on which the task will act. + // In this case the waist configuration is a vector 6d (position and orientation -> SE3) + // Here we set a mask = [0 0 0 1 1 1] so the task on the waist will act on the orientation of the robot + Eigen::VectorXd mask_orientation(6); + mask_orientation << 0, 0, 0, 1, 1, 1; + m_taskWaist->setMask(mask_orientation); + // Add the task to the HQP with weight = 1.0, priority level = 1 (in the cost function) and a transition duration = 0.0 + m_invDyn->addMotionTask(*m_taskWaist, m_w_base_orientation, 1); + + // FEET TASKS (not added yet to the invDyn pb, only when contacts are removed) m_taskRF = new TaskSE3Equality("task-rf", *m_robot, m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); m_taskRF->Kp(kp_feet); m_taskRF->Kd(kd_feet); @@ -564,11 +623,13 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskLF->Kp(kp_feet); m_taskLF->Kd(kd_feet); + // POSTURE TASK m_taskPosture = new TaskJointPosture("task-posture", *m_robot); m_taskPosture->Kp(kp_posture); m_taskPosture->Kd(kd_posture); m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1); + // HANDS TASKS (not added yet to the invDyn pb, only when the command addTaskXHand is called) m_taskRH = new TaskSE3Equality("task-rh", *m_robot, m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_taskRH->Kp(kp_hands); m_taskRH->Kd(kd_hands); @@ -577,7 +638,10 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskLH->Kp(kp_hands); m_taskLH->Kd(kd_hands); + // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); + // m_sampleAM = TrajectorySample(3); + m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); m_sampleRH = TrajectorySample(3); @@ -660,7 +724,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) { @@ -691,9 +755,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 +775,26 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const Vector6& kd_contact = m_kd_constraintsSIN(iter); const Vector3& kp_com = m_kp_comSIN(iter); const Vector3& kd_com = m_kd_comSIN(iter); + const Vector3& kp_am = m_kp_amSIN(iter); + const Vector3& kd_am = m_kd_amSIN(iter); + const dg::sot::Vector6d& kp_base_orientation = m_kp_base_orientationSIN(0); + const dg::sot::Vector6d& kd_base_orientation = m_kd_base_orientationSIN(0); const VectorN& kp_posture = m_kp_postureSIN(iter); assert(kp_posture.size() == static_cast(m_robot_util->m_nbJoints)); const VectorN& kd_posture = m_kd_postureSIN(iter); assert(kd_posture.size() == static_cast(m_robot_util->m_nbJoints)); - const VectorN& kp_pos = m_kp_posSIN(iter); - assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); - const VectorN& kd_pos = m_kd_posSIN(iter); - assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); + // const VectorN& kp_pos = m_kp_posSIN(iter); + // assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); + // const VectorN& kd_pos = m_kd_posSIN(iter); + // assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); /*const double & w_hands = m_w_handsSIN(iter);*/ const double& w_com = m_w_comSIN(iter); + const double& w_am = m_w_amSIN(iter); const double& w_posture = m_w_postureSIN(iter); const double& w_forces = m_w_forcesSIN(iter); + const double & w_base_orientation = m_w_base_orientationSIN(iter); if (m_contactState == LEFT_SUPPORT || m_contactState == LEFT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); @@ -775,8 +851,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); getProfiler().start(PROFILE_PREPARE_INV_DYN); - m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); - m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); + // m_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; @@ -790,6 +866,28 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskCom->name(), w_com); } + m_sampleAM.vel = L_am_ref; + m_sampleAM.acc = dL_am_ref; + m_taskAM->setReference(m_sampleAM); + m_taskAM->Kp(kp_am); + m_taskAM->Kd(kd_am); + if (m_w_am != w_am) { + // SEND_MSG("Change w_am from "+toString(m_w_am)+" to "+toString(w_am), MSG_TYPE_INFO); + m_w_am = w_am; + m_invDyn->updateTaskWeight(m_taskAM->name(), w_am); + } + + m_sampleWaist.pos = x_waist_ref; + m_sampleWaist.vel = dx_waist_ref; + m_sampleWaist.acc = ddx_waist_ref; + m_taskWaist->setReference(m_sampleWaist); + m_taskWaist->Kp(kp_base_orientation); + m_taskWaist->Kd(kd_base_orientation); + if (m_w_base_orientation != w_base_orientation) { + m_w_base_orientation = w_base_orientation; + m_invDyn->updateTaskWeight(m_taskWaist->name(), w_base_orientation); + } + m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos); m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel); m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc); @@ -825,6 +923,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_firstTime) { m_firstTime = false; + m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); // m_robot->computeAllTerms(m_invDyn->data(), q, v); pinocchio::SE3 H_lf = m_robot->position( @@ -884,8 +984,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_invDyn->getContactForces(m_contactLF->name(), sol, tmp)) m_f_LF = m_contactLF->getForceGeneratorMatrix() * tmp; m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot); - m_tau_sot += kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) + - kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints)); + // m_tau_sot += kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) + + // kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints)); getProfiler().stop(PROFILE_TAU_DES_COMPUTATION); m_t += m_dt; @@ -917,6 +1017,62 @@ DEFINE_SIGNAL_OUT_FUNCTION(dv_des, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(v_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal v_des before initialization!"); + return s; + } + if (s.size() != m_robot->nv()) + s.resize(m_robot->nv()); + m_dv_desSOUT(iter); + tsid::math::Vector v_mean; + v_mean = m_v_urdf + 0.5 * m_dt * m_dv_urdf; + m_v_urdf = m_v_urdf + m_dt * m_dv_urdf; + m_q_urdf = pinocchio::integrate(m_robot->model(), m_q_urdf, m_dt * v_mean); + m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_v_urdf, m_v_sot); + s = m_v_sot; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(q_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal q_des before initialization!"); + return s; + } + if (s.size() != m_robot->nv()) + s.resize(m_robot->nv()); + m_v_desSOUT(iter); + m_robot_util->config_urdf_to_sot(m_q_urdf, m_q_sot); + s = m_q_sot; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal tau_pd_des before initialization!"); + return s; + } + if (s.size() != static_cast(m_robot_util->m_nbJoints)) + s.resize(m_robot_util->m_nbJoints); + + const VectorN& kp_pos = m_kp_posSIN(iter); + assert(kp_pos.size() == static_cast(m_robot_util->m_nbJoints)); + const VectorN& kd_pos = m_kd_posSIN(iter); + assert(kd_pos.size() == static_cast(m_robot_util->m_nbJoints)); + + const 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)); + + m_q_desSOUT(iter); + + s = m_tau_sot + kp_pos.cwiseProduct(m_q_sot.tail(m_robot_util->m_nbJoints) - q_robot.tail(m_robot_util->m_nbJoints)) + + kd_pos.cwiseProduct(m_v_sot.tail(m_robot_util->m_nbJoints) - v_robot.tail(m_robot_util->m_nbJoints)); + + 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 +1125,28 @@ DEFINE_SIGNAL_OUT_FUNCTION(com_acc, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(am_dL_des, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal am_dL_des before initialization!"); + return s; + } + if (s.size() != 3) s.resize(3); + m_tau_desSOUT(iter); + s = m_taskAM->getDesiredMomentumDerivative(); + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(am_dL, dynamicgraph::Vector) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal am_dL before initialization!"); + return s; + } + if (s.size() != 3) s.resize(3); + m_tau_desSOUT(iter); + s = m_taskAM->getdMomentum(m_dv_urdf); + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(zmp_des_right_foot_local, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal zmp_des_right_foot_local before initialization!"); @@ -1186,7 +1364,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(com_vel, dynamicgraph::Vector) { DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, dynamicgraph::Vector) { if (!m_initSucceeded) { - std::ostringstream oss("Cannot compute signal com_vel before initialization! iter:"); + std::ostringstream oss("Cannot compute signal base_orientation before initialization! iter:"); oss << iter; SEND_WARNING_STREAM_MSG(oss.str()); return s; @@ -1373,6 +1551,15 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc_des, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(time, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal time before initialization!"); + return s; + } + m_tau_desSOUT(iter); + return m_t; +} + /* --- COMMANDS ---------------------------------------------------------- */ /* ------------------------------------------------------------------- */ From 8766480d474426769c930fa32cb51eb3e20a9656 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 6 Apr 2020 18:39:23 +0200 Subject: [PATCH 10/47] [Inv_Dyn] Update references of contacts when they are added --- src/inverse-dynamics-balance-controller.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 9be6795..566b42b 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -464,6 +464,10 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit if (m_contactState == LEFT_SUPPORT) { SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); + TrajectorySample traj_ref = m_taskRF->getReference(); + pinocchio::SE3 ref; + vectorToSE3(traj_ref.pos, ref); + m_contactRF->setReference(ref); m_invDyn->addRigidContact(*m_contactRF, w_forces); m_invDyn->removeTask(m_taskRF->name(), transitionTime); m_contactState = DOUBLE_SUPPORT; @@ -474,6 +478,10 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti if (m_contactState == RIGHT_SUPPORT) { SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); + TrajectorySample traj_ref = m_taskLF->getReference(); + pinocchio::SE3 ref; + vectorToSE3(traj_ref.pos, ref); + m_contactLF->setReference(ref); m_invDyn->addRigidContact(*m_contactLF, w_forces); m_invDyn->removeTask(m_taskLF->name(), transitionTime); m_contactState = DOUBLE_SUPPORT; From 6629e64697344d65e27c0a171b895d381a87337d Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 12 May 2020 12:13:26 +0200 Subject: [PATCH 11/47] [Base-estim] Plug base-estimator to inv_dyn entity to have updates on q/v Remove useless sendMsg functions --- .../torque_control/admittance-controller.hh | 4 - include/sot/torque_control/base-estimator.hh | 4 - include/sot/torque_control/control-manager.hh | 4 - .../sot/torque_control/current-controller.hh | 4 - .../sot/torque_control/device-torque-ctrl.hh | 4 - .../sot/torque_control/free-flyer-locator.hh | 4 - .../torque_control/imu_offset_compensation.hh | 3 - .../inverse-dynamics-balance-controller.hh | 11 +- .../torque_control/joint-torque-controller.hh | 4 - .../joint-trajectory-generator.hh | 4 - .../torque_control/numerical-difference.hh | 5 - .../sot/torque_control/position-controller.hh | 4 - .../se3-trajectory-generator.hh | 4 - .../sot/torque_control/simple-inverse-dyn.hh | 18 ++- .../torque_control/torque-offset-estimator.hh | 4 - include/sot/torque_control/trace-player.hh | 4 - src/inverse-dynamics-balance-controller.cpp | 138 +++++++++++++----- src/simple-inverse-dyn.cpp | 129 +++++++++++++--- 18 files changed, 228 insertions(+), 124 deletions(-) diff --git a/include/sot/torque_control/admittance-controller.hh b/include/sot/torque_control/admittance-controller.hh index edbdea9..5f8f67f 100644 --- a/include/sot/torque_control/admittance-controller.hh +++ b/include/sot/torque_control/admittance-controller.hh @@ -90,10 +90,6 @@ class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController : public ::dynamicgrap /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: Eigen::VectorXd m_u; /// control (i.e. motor currents) bool m_firstIter; diff --git a/include/sot/torque_control/base-estimator.hh b/include/sot/torque_control/base-estimator.hh index d27505c..e91250a 100644 --- a/include/sot/torque_control/base-estimator.hh +++ b/include/sot/torque_control/base-estimator.hh @@ -160,10 +160,6 @@ class SOTBASEESTIMATOR_EXPORT BaseEstimator : public ::dynamicgraph::Entity { /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: bool m_initSucceeded; /// true if the entity has been successfully initialized bool m_reset_foot_pos; /// true after the command resetFootPositions is called diff --git a/include/sot/torque_control/control-manager.hh b/include/sot/torque_control/control-manager.hh index 058555d..fa0fccd 100644 --- a/include/sot/torque_control/control-manager.hh +++ b/include/sot/torque_control/control-manager.hh @@ -130,10 +130,6 @@ class SOTCONTROLMANAGER_EXPORT ControlManager : public ::dynamicgraph::Entity { /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[ControlManager-" + name + "] " + msg) << '\n'; - } - protected: RobotUtilShrPtr m_robot_util; tsid::robots::RobotWrapper* m_robot; diff --git a/include/sot/torque_control/current-controller.hh b/include/sot/torque_control/current-controller.hh index 4995e8e..23b711c 100644 --- a/include/sot/torque_control/current-controller.hh +++ b/include/sot/torque_control/current-controller.hh @@ -106,10 +106,6 @@ class SOTCURRENTCONTROLLER_EXPORT CurrentController : public ::dynamicgraph::Ent /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[CurrentController-" + name + "] " + msg) << '\n'; - } - protected: RobotUtilShrPtr m_robot_util; bool m_initSucceeded; /// true if the entity has been successfully initialized diff --git a/include/sot/torque_control/device-torque-ctrl.hh b/include/sot/torque_control/device-torque-ctrl.hh index 5b72328..3b3b40e 100644 --- a/include/sot/torque_control/device-torque-ctrl.hh +++ b/include/sot/torque_control/device-torque-ctrl.hh @@ -75,10 +75,6 @@ class DeviceTorqueCtrl : public dgsot::Device { virtual void integrate(const double& dt); void computeForwardDynamics(); - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[DeviceTorqueCtrl] " + msg) << '\n'; - } - /// \brief Current integration step. double timestep_; bool m_initSucceeded; diff --git a/include/sot/torque_control/free-flyer-locator.hh b/include/sot/torque_control/free-flyer-locator.hh index 083c7fb..987a374 100644 --- a/include/sot/torque_control/free-flyer-locator.hh +++ b/include/sot/torque_control/free-flyer-locator.hh @@ -78,10 +78,6 @@ class SOTFREEFLYERLOCATOR_EXPORT FreeFlyerLocator : public ::dynamicgraph::Entit /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[FreeFlyerLocator-" + name + "] " + msg) << '\n'; - } - protected: bool m_initSucceeded; /// true if the entity has been successfully initialized pinocchio::Model* m_model; /// Pinocchio robot model diff --git a/include/sot/torque_control/imu_offset_compensation.hh b/include/sot/torque_control/imu_offset_compensation.hh index 10a512a..86de0a4 100644 --- a/include/sot/torque_control/imu_offset_compensation.hh +++ b/include/sot/torque_control/imu_offset_compensation.hh @@ -66,9 +66,6 @@ class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation : public ::dynamicgr /* --- METHODS --- */ void update_offset_impl(int iter); - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) { - logger_.stream(t) << ("[ImuOffsetCompensation-" + name + "] " + msg, t, file, line); - } protected: bool m_initSucceeded; /// true if the entity has been successfully initialized diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 49a020e..f9a44d3 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -120,6 +120,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_tau, dynamicgraph::Vector); DECLARE_SIGNAL_IN(w_com, double); DECLARE_SIGNAL_IN(w_am, double); @@ -147,7 +148,8 @@ 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); @@ -182,7 +184,9 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_OUT(am_dL_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(right_foot_pos_quat, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(left_foot_pos_quat, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector); @@ -195,7 +199,6 @@ 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(time, 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); @@ -204,10 +207,6 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: double m_dt; /// control loop time period double m_t; diff --git a/include/sot/torque_control/joint-torque-controller.hh b/include/sot/torque_control/joint-torque-controller.hh index 0d7b086..d76c1f4 100644 --- a/include/sot/torque_control/joint-torque-controller.hh +++ b/include/sot/torque_control/joint-torque-controller.hh @@ -113,10 +113,6 @@ class SOTJOINTTORQUECONTROLLER_EXPORT JointTorqueController : public ::dynamicgr RobotUtilShrPtr m_robot_util; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/include/sot/torque_control/joint-trajectory-generator.hh b/include/sot/torque_control/joint-trajectory-generator.hh index d8a23e9..803ebdc 100644 --- a/include/sot/torque_control/joint-trajectory-generator.hh +++ b/include/sot/torque_control/joint-trajectory-generator.hh @@ -147,10 +147,6 @@ class SOTJOINTTRAJECTORYGENERATOR_EXPORT JointTrajectoryGenerator : public ::dyn /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[JointTrajectoryGenerator-" + name + "] " + msg) << '\n'; - } - protected: enum JTG_Status { JTG_STOP, JTG_SINUSOID, JTG_MIN_JERK, JTG_LIN_CHIRP, JTG_TRIANGLE, JTG_CONST_ACC, JTG_TEXT_FILE }; diff --git a/include/sot/torque_control/numerical-difference.hh b/include/sot/torque_control/numerical-difference.hh index 8bc9761..7ccd8e8 100644 --- a/include/sot/torque_control/numerical-difference.hh +++ b/include/sot/torque_control/numerical-difference.hh @@ -94,11 +94,6 @@ class SOTNUMERICALDIFFERENCE_EXPORT NumericalDifference : public ::dynamicgraph: */ void init(const double& timestep, const int& sigSize, const double& delay, const int& polyOrder); - protected: - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - public: /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; diff --git a/include/sot/torque_control/position-controller.hh b/include/sot/torque_control/position-controller.hh index 34fdafb..e80bb05 100644 --- a/include/sot/torque_control/position-controller.hh +++ b/include/sot/torque_control/position-controller.hh @@ -72,10 +72,6 @@ class SOTPOSITIONCONTROLLER_EXPORT PositionController : public ::dynamicgraph::E /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[PositionController-" + name + "] " + msg) << '\n'; - } - protected: RobotUtilShrPtr m_robot_util; /// Robot Util Eigen::VectorXd m_pwmDes; diff --git a/include/sot/torque_control/se3-trajectory-generator.hh b/include/sot/torque_control/se3-trajectory-generator.hh index acf1255..937d0b3 100644 --- a/include/sot/torque_control/se3-trajectory-generator.hh +++ b/include/sot/torque_control/se3-trajectory-generator.hh @@ -121,10 +121,6 @@ class SOTSE3TRAJECTORYGENERATOR_EXPORT SE3TrajectoryGenerator : public ::dynamic /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[SE3TrajectoryGenerator-" + name + "] " + msg) << '\n'; - } - protected: enum TG_Status { TG_STOP, diff --git a/include/sot/torque_control/simple-inverse-dyn.hh b/include/sot/torque_control/simple-inverse-dyn.hh index 8c4c0b9..6f816c7 100644 --- a/include/sot/torque_control/simple-inverse-dyn.hh +++ b/include/sot/torque_control/simple-inverse-dyn.hh @@ -41,7 +41,7 @@ /* Pinocchio */ #include #include -#include "pinocchio/algorithm/joint-configuration.hpp" +#include #include #include @@ -92,6 +92,9 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(tau_measured, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(kp_tau, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector); @@ -117,6 +120,7 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit DECLARE_SIGNAL_IN(q, dynamicgraph::Vector); DECLARE_SIGNAL_IN(v, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_measured, dynamicgraph::Vector); DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector); @@ -125,20 +129,19 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); /* --- COMMANDS --- */ void init(const double& dt, const std::string& robotRef); virtual void setControlOutputType(const std::string& type); - void updateComOffset(); + void updateComOffset(const dynamicgraph::Vector& com_measured); /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: double m_dt; /// control loop time period double m_t; /// current time @@ -146,6 +149,9 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entit bool m_enabled; /// True if controler is enabled bool m_firstTime; /// True at the first iteration of the controller + int m_frame_id_rf; /// frame id of right foot + int m_frame_id_lf; /// frame id of left foot + /// TSID /// Robot tsid::robots::RobotWrapper* m_robot; diff --git a/include/sot/torque_control/torque-offset-estimator.hh b/include/sot/torque_control/torque-offset-estimator.hh index addce40..e18fd98 100644 --- a/include/sot/torque_control/torque-offset-estimator.hh +++ b/include/sot/torque_control/torque-offset-estimator.hh @@ -90,10 +90,6 @@ class TORQUEOFFSETESTIMATOR_EXPORT TorqueOffsetEstimator : public ::dynamicgraph // stdAlignedVector stdVecJointTorqueOffsets; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - private: enum { PRECOMPUTATION, INPROGRESS, COMPUTED } sensor_offset_status; diff --git a/include/sot/torque_control/trace-player.hh b/include/sot/torque_control/trace-player.hh index 759cc2c..5b13003 100644 --- a/include/sot/torque_control/trace-player.hh +++ b/include/sot/torque_control/trace-player.hh @@ -74,10 +74,6 @@ class SOTTRACEPLAYER_EXPORT TracePlayer : public ::dynamicgraph::Entity { /* --- ENTITY INHERITANCE --- */ virtual void display(std::ostream& os) const; - void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) { - logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; - } - protected: typedef dynamicgraph::Vector DataType; typedef std::list DataHistoryType; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 566b42b..300c5d7 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -19,6 +19,7 @@ #include +#include "pinocchio/algorithm/center-of-mass.hpp" #include #include @@ -82,7 +83,7 @@ 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 \ @@ -125,6 +126,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_kd_postureSIN \ << m_kp_posSIN \ << m_kd_posSIN \ + << m_kp_tauSIN \ << m_w_comSIN \ << m_w_amSIN \ << m_w_feetSIN \ @@ -148,7 +150,8 @@ 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 \ @@ -181,7 +184,9 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_am_dL_desSOUT \ << m_base_orientationSOUT \ << m_left_foot_posSOUT \ + << m_left_foot_pos_quatSOUT \ << m_right_foot_posSOUT \ + << m_right_foot_pos_quatSOUT \ << m_left_hand_posSOUT \ << m_right_hand_posSOUT \ << m_left_foot_velSOUT \ @@ -194,7 +199,6 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_right_hand_accSOUT \ << m_right_foot_acc_desSOUT \ << m_left_foot_acc_desSOUT \ - << m_timeSOUT /// Define EntityClassName here rather than in the header file /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. @@ -252,6 +256,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(kp_tau, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(w_com, double), CONSTRUCT_SIGNAL_IN(w_am, double), CONSTRUCT_SIGNAL_IN(w_feet, double), @@ -275,7 +280,8 @@ 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), @@ -307,7 +313,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_OUT(am_dL_des, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(right_foot_pos_quat, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(left_foot_pos_quat, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(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), @@ -320,7 +328,6 @@ 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(time, double, m_tau_desSOUT), CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN), m_t(0.0), m_initSucceeded(false), @@ -392,6 +399,7 @@ void InverseDynamicsBalanceController::updateComOffset() { void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { + SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); bool res = m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -411,6 +419,7 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran void InverseDynamicsBalanceController::removeLeftFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { + SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); bool res = m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -464,9 +473,10 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit if (m_contactState == LEFT_SUPPORT) { SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); - TrajectorySample traj_ref = m_taskRF->getReference(); + // TrajectorySample traj_ref = m_taskRF->getReference(); pinocchio::SE3 ref; - vectorToSE3(traj_ref.pos, ref); + // vectorToSE3(traj_ref.pos, ref); + ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); m_contactRF->setReference(ref); m_invDyn->addRigidContact(*m_contactRF, w_forces); m_invDyn->removeTask(m_taskRF->name(), transitionTime); @@ -478,9 +488,10 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti if (m_contactState == RIGHT_SUPPORT) { SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); - TrajectorySample traj_ref = m_taskLF->getReference(); + // TrajectorySample traj_ref = m_taskLF->getReference(); pinocchio::SE3 ref; - vectorToSE3(traj_ref.pos, ref); + // vectorToSE3(traj_ref.pos, ref); + ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); m_contactLF->setReference(ref); m_invDyn->addRigidContact(*m_contactLF, w_forces); m_invDyn->removeTask(m_taskLF->name(), transitionTime); @@ -533,13 +544,9 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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); - const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0); - const VectorN& gear_ratios_sot = m_gear_ratiosSIN(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); @@ -560,12 +567,18 @@ 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->nq()); m_v_sot.setZero(m_robot->nv()); @@ -659,6 +672,25 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_frame_id_rh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_frame_id_lh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); + // COM OFFSET + if (m_com_measuredSIN.isPlugged()){ + const VectorN6& q_robot = m_qSIN(0); + assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_robot = m_vSIN(0); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const dg::Vector& com_measured = m_com_measuredSIN(0); + assert(com_measured.size() == 3); + 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()); m_hqpSolver_60_36_34 = @@ -859,8 +891,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); getProfiler().start(PROFILE_PREPARE_INV_DYN); - // m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); - // m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); + m_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; @@ -931,8 +963,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_firstTime) { m_firstTime = false; - m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); - m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); // m_robot->computeAllTerms(m_invDyn->data(), q, v); pinocchio::SE3 H_lf = m_robot->position( @@ -972,10 +1002,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; } @@ -1067,17 +1097,24 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, 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; } @@ -1398,6 +1435,24 @@ DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos_quat, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal left_foot_pos before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + m_tau_desSOUT(iter); + const dg::Vector& x_lf_ref = m_lf_ref_posSIN(iter); + pinocchio::SE3 oMi; + oMi.translation(x_lf_ref.head<3>() ); + oMi.rotation( Eigen::Map >(&x_lf_ref(3), 3, 3) ); + s.resize(7); + // m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, oMi); + tsid::math::SE3ToXYZQUAT(oMi, s); + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(left_hand_pos, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal left hand_pos before initialization!"); @@ -1426,6 +1481,24 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos_quat, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal rigt_foot_pos before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + // m_tau_desSOUT(iter); + const dg::Vector& x_rf_ref = m_rf_ref_posSIN(iter); + pinocchio::SE3 oMi; + oMi.translation(x_rf_ref.head<3>() ); + oMi.rotation( Eigen::Map >(&x_rf_ref(3), 3, 3) ); + s.resize(7); + // m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, oMi); + tsid::math::SE3ToXYZQUAT(oMi, s); + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(right_hand_pos, dynamicgraph::Vector) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot compute signal right_hand_pos before initialization!"); @@ -1559,15 +1632,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc_des, dynamicgraph::Vector) { return s; } -DEFINE_SIGNAL_OUT_FUNCTION(time, double) { - if (!m_initSucceeded) { - SEND_WARNING_STREAM_MSG("Cannot compute signal time before initialization!"); - return s; - } - m_tau_desSOUT(iter); - return m_t; -} - /* --- COMMANDS ---------------------------------------------------------- */ /* ------------------------------------------------------------------- */ diff --git a/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index 6321fee..d2ab3e0 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -25,6 +25,8 @@ #include #include +#include "pinocchio/algorithm/center-of-mass.hpp" +#include #include #include @@ -89,6 +91,8 @@ using namespace dg::sot; << m_w_postureSIN \ << m_kp_posSIN \ << m_kd_posSIN \ + << m_tau_measuredSIN \ + << m_kp_tauSIN \ << m_com_ref_posSIN \ << m_com_ref_velSIN \ << m_com_ref_accSIN \ @@ -111,9 +115,11 @@ using namespace dg::sot; << m_w_waistSIN \ << m_qSIN \ << m_vSIN \ + << m_com_measuredSIN \ << m_active_jointsSIN -#define OUTPUT_SIGNALS m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT +#define OUTPUT_SIGNALS m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT << m_comSOUT \ + << m_right_foot_posSOUT << m_left_foot_posSOUT /// Define EntityClassName here rather than in the header file @@ -142,6 +148,8 @@ SimpleInverseDyn(const std::string& name) , CONSTRUCT_SIGNAL_IN(w_posture, double) , CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(tau_measured, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(kp_tau, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector) @@ -164,6 +172,7 @@ SimpleInverseDyn(const std::string& name) , CONSTRUCT_SIGNAL_IN(w_waist, double) , CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector) + , CONSTRUCT_SIGNAL_IN(com_measured, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN) , CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS) @@ -171,6 +180,9 @@ SimpleInverseDyn(const std::string& name) , CONSTRUCT_SIGNAL_OUT(v_des, dg::Vector, m_dv_desSOUT) , CONSTRUCT_SIGNAL_OUT(q_des, dg::Vector, m_v_desSOUT) , CONSTRUCT_SIGNAL_OUT(u, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT << m_v_desSOUT << m_q_desSOUT) + , CONSTRUCT_SIGNAL_OUT(com, dg::Vector, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT) + , CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT) , m_t(0.0) , m_initSucceeded(false) , m_enabled(false) @@ -196,14 +208,15 @@ SimpleInverseDyn(const std::string& name) "Control type: velocity or torque (string)"))); addCommand("updateComOffset", - makeCommandVoid0(*this, &SimpleInverseDyn::updateComOffset, - docCommandVoid0("Update the offset on the CoM based on the CoP measurement."))); + makeCommandVoid1(*this, &SimpleInverseDyn::updateComOffset, + docCommandVoid1("Update the offset on the CoM based on its measurement.", + "Measured CoM"))); } -void SimpleInverseDyn::updateComOffset() { +void SimpleInverseDyn::updateComOffset(const dg::Vector& com_measured) { const Vector3 & com = m_robot->com(m_invDyn->data()); - m_com_offset = com; + m_com_offset = com_measured - com; SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO); } @@ -319,6 +332,28 @@ void SimpleInverseDyn::init(const double& dt, const std::string& robotRef) { m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); + m_frame_id_rf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); + m_frame_id_lf = (int)m_robot->model().getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name); + + // COM OFFSET + if (m_com_measuredSIN.isPlugged()){ + const VectorN6& q_robot = m_qSIN(0); + assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_robot = m_vSIN(0); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const dg::Vector& com_measured = m_com_measuredSIN(0); + assert(com_measured.size() == 3); + SEND_MSG("COM_measured: " + toString(com_measured), MSG_TYPE_ERROR); + + m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf); + + pinocchio::Data data = pinocchio::Data(m_robot->model()); + pinocchio::centerOfMass(m_robot->model(), data, m_q_urdf, m_v_urdf); + m_com_offset = com_measured - data.com[0]; + SEND_MSG("Update COM: " + toString(m_com_offset), MSG_TYPE_ERROR); + } + m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog-fast"); m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn()); @@ -425,6 +460,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().start(PROFILE_PREPARE_INV_DYN); + // Update tasks m_sampleCom.pos = x_com_ref - m_com_offset; m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; @@ -472,21 +508,22 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces); if (m_firstTime) { - m_firstTime = false; + SEND_ERROR_STREAM_MSG("FIRST TIME iter " + toString(iter)); + 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_ERROR); 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_ERROR); } 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 +531,13 @@ 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. + SEND_ERROR_STREAM_MSG("CONTROL_OUTPUT_TORQUE iter " + toString(iter)); + 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 +552,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 +629,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; } From 2e8ba0fa04316cd4a974c5d5b19f2c8764aa8932 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 25 May 2020 11:52:43 +0200 Subject: [PATCH 12/47] =?UTF-8?q?[Doc]=C2=A0Add=20documentation=20for=20wa?= =?UTF-8?q?lk=20and=20bellStep=20simulation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- doc/Overview.md | 6 ++++- doc/bellStepRun.md | 40 +++++++++++++++++++++++++++++++ doc/running.md | 2 +- doc/walkRun.md | 59 ++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 105 insertions(+), 2 deletions(-) create mode 100644 doc/bellStepRun.md create mode 100644 doc/walkRun.md 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). From d5ff70108180fb2e3d7ae65282034726dfee9a90 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 25 May 2020 14:26:18 +0200 Subject: [PATCH 13/47] [Inv_Dyn] Clean useless sendMsg --- src/inverse-dynamics-balance-controller.cpp | 2 +- src/simple-inverse-dyn.cpp | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 300c5d7..030b27e 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -1113,7 +1113,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { 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); + s += kp_tau.cwiseProduct(tau_measured - s); return s; } diff --git a/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index d2ab3e0..2f1e73d 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -508,7 +508,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces); if (m_firstTime) { - SEND_ERROR_STREAM_MSG("FIRST TIME iter " + toString(iter)); 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); @@ -518,12 +517,12 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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_ERROR); + 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_ERROR); + 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)) { @@ -533,7 +532,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { } 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. - SEND_ERROR_STREAM_MSG("CONTROL_OUTPUT_TORQUE iter " + toString(iter)); 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); } From 446850805c17b76514ab253d0c3e2878a74ec031 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 25 May 2020 14:26:47 +0200 Subject: [PATCH 14/47] [DDP] Add ddp files to plugin list --- src/CMakeLists.txt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 46ce277..09214c4 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,13 +18,10 @@ SET(plugins se3-trajectory-generator torque-offset-estimator trace-player + ddp-actuator-solver + ddp_pyrene_actuator_solver ) -IF(DDP_ACTUATOR_SOLVER_FOUND) - SET(plugins ${plugins} ddp-actuator-solver) - SET(plugins ${plugins} ddp_pyrene_actuator_solver) -ENDIF(DDP_ACTUATOR_SOLVER_FOUND) - FOREACH(plugin ${plugins}) GET_FILENAME_COMPONENT(LIBRARY_NAME ${plugin} NAME) ADD_LIBRARY(${LIBRARY_NAME} SHARED ${plugin}) From 501baecb290f9ea53614acbbe0082380292bcafe Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 16 Jun 2020 10:57:38 +0200 Subject: [PATCH 15/47] [Inv_Dyn] Add control mode (vel/torque) and change signal phase (int) In velocity control use the base estimator to estimate feet positions and CoM position. Use them in the feet and CoM tasks. --- .../inverse-dynamics-balance-controller.hh | 17 +- src/inverse-dynamics-balance-controller.cpp | 197 ++++++++++++++++-- 2 files changed, 199 insertions(+), 15 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index f9a44d3..ccb8f2b 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -39,6 +39,7 @@ #include #include #include +// #include "tsid/tasks/task-angular-momentum-integral-equality.hpp" #include /* HELPER */ @@ -52,6 +53,10 @@ namespace dynamicgraph { namespace sot { namespace torque_control { +enum ControlOutput { CONTROL_OUTPUT_VELOCITY = 0, CONTROL_OUTPUT_TORQUE = 1, CONTROL_OUTPUT_SIZE = 2 }; + +const std::string ControlOutput_s[] = {"velocity", "torque"}; + /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ @@ -68,6 +73,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle void init(const double& dt, const std::string& robotRef); void updateComOffset(); + virtual void setControlOutputType(const std::string& type); void removeRightFootContact(const double& transitionTime); void removeLeftFootContact(const double& transitionTime); void addRightFootContact(const double& transitionTime); @@ -155,6 +161,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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 @@ -177,6 +184,7 @@ 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); @@ -187,6 +195,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_OUT(right_foot_pos_quat, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_foot_pos_quat, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(lf_est, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(rf_est, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector); @@ -292,6 +302,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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 @@ -303,6 +314,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle typedef pinocchio::Data::Matrix6x Matrix6x; + pinocchio::Data m_estim_data; Matrix6x m_J_RF; Matrix6x m_J_LF; Eigen::ColPivHouseholderQR m_J_RF_QR; @@ -310,8 +322,9 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::math::Vector6 m_v_RF_int; tsid::math::Vector6 m_v_LF_int; - unsigned int m_timeLast; - RobotUtilShrPtr m_robot_util; + unsigned int m_timeLast; /// Final time of the control loop + RobotUtilShrPtr m_robot_util; /// Share pointer to the robot utils methods + ControlOutput m_ctrlMode; /// ctrl mode desired for the output (velocity or torque) }; // class InverseDynamicsBalanceController } // namespace torque_control diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 030b27e..a4a0a08 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -7,6 +7,10 @@ #include +#include +#include +#include + #include #include #include @@ -19,7 +23,6 @@ #include -#include "pinocchio/algorithm/center-of-mass.hpp" #include #include @@ -157,6 +160,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_wrench_baseSIN \ << m_wrench_left_footSIN \ << m_wrench_right_footSIN \ + << m_ref_phaseSIN \ << m_active_jointsSIN #define OUTPUT_SIGNALS m_tau_desSOUT \ @@ -177,6 +181,7 @@ 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 \ @@ -187,6 +192,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_left_foot_pos_quatSOUT \ << m_right_foot_posSOUT \ << m_right_foot_pos_quatSOUT \ + << m_lf_estSOUT \ + << m_rf_estSOUT \ << m_left_hand_posSOUT \ << m_right_hand_posSOUT \ << m_left_foot_velSOUT \ @@ -287,6 +294,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(wrench_base, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(ref_phase, int), CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector), CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS), CONSTRUCT_SIGNAL_OUT(M, dg::Matrix, m_tau_desSOUT), @@ -306,6 +314,7 @@ 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), @@ -315,7 +324,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_pos_quat, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT), - CONSTRUCT_SIGNAL_OUT(left_foot_pos_quat, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(left_foot_pos_quat, dg::Vector, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(lf_est, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(rf_est, dg::Vector, INPUT_SIGNALS << m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_hand_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(left_hand_pos, dg::Vector, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(right_foot_vel, dg::Vector, m_tau_desSOUT), @@ -337,7 +348,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st m_rightHandState(TASK_RIGHT_HAND_OFF), m_leftHandState(TASK_LEFT_HAND_OFF), m_timeLast(0), - m_robot_util(RefVoidRobotUtil()) { + m_robot_util(RefVoidRobotUtil()), + m_ctrlMode(CONTROL_OUTPUT_TORQUE) { RESETDEBUG5(); Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); @@ -350,6 +362,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st m_zmp_LF.setZero(); m_zmp.setZero(); m_com_offset.setZero(); + m_dcom_offset.setZero(); m_v_RF_int.setZero(); m_v_LF_int.setZero(); @@ -362,6 +375,11 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st makeCommandVoid0(*this, &InverseDynamicsBalanceController::updateComOffset, docCommandVoid0("Update the offset on the CoM based on the CoP measurement."))); + /* SET of control output type. */ + addCommand("setControlOutputType", + makeCommandVoid1(*this, &InverseDynamicsBalanceController::setControlOutputType, + docCommandVoid1("Set the type of control output.", + "Control type: velocity or torque (string)"))); addCommand("removeRightFootContact", makeCommandVoid1( *this, &InverseDynamicsBalanceController::removeRightFootContact, @@ -397,6 +415,16 @@ void InverseDynamicsBalanceController::updateComOffset() { SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO); } +void InverseDynamicsBalanceController::setControlOutputType(const std::string& type) { + for (int i = 0; i < CONTROL_OUTPUT_SIZE; i++) + if (type == ControlOutput_s[i]) { + m_ctrlMode = (ControlOutput)i; + sotDEBUG(25) << "Control output type: " << ControlOutput_s[i] << endl; + return; + } + sotDEBUG(25) << "Unrecognized control output type: " << type << endl; +} + void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); @@ -478,8 +506,13 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit // vectorToSE3(traj_ref.pos, ref); ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); m_contactRF->setReference(ref); - m_invDyn->addRigidContact(*m_contactRF, w_forces); m_invDyn->removeTask(m_taskRF->name(), transitionTime); + bool res = m_invDyn->addRigidContact(*m_contactRF, w_forces); + if (!res) { + const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); + SEND_MSG("Error while adding right foot contact." + tsid::solvers::HQPDataToString(hqpData, false), + MSG_TYPE_ERROR); + } m_contactState = DOUBLE_SUPPORT; } } @@ -493,8 +526,13 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti // vectorToSE3(traj_ref.pos, ref); ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); m_contactLF->setReference(ref); - m_invDyn->addRigidContact(*m_contactLF, w_forces); m_invDyn->removeTask(m_taskLF->name(), transitionTime); + bool res = m_invDyn->addRigidContact(*m_contactLF, w_forces); + if (!res) { + const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); + SEND_MSG("Error while adding left foot contact." + tsid::solvers::HQPDataToString(hqpData, false), + MSG_TYPE_ERROR); + } m_contactState = DOUBLE_SUPPORT; } } @@ -590,6 +628,8 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_J_RF.setZero(6, m_robot->nv()); m_J_LF.setZero(6, m_robot->nv()); + m_estim_data = pinocchio::Data(m_robot->model()); + m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot); // CONTACT 6D TASKS @@ -773,6 +813,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; @@ -836,31 +892,73 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const double& w_forces = m_w_forcesSIN(iter); const double & w_base_orientation = m_w_base_orientationSIN(iter); + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // BASE ESTIMATOR computation to have com and feet estimations + // Used to regulate on estimation (desired value of tasks are the estimations) + // -> only in velocity because close the TSID loop on itself (v_des, q_des) + // In torque close loop on the (q,v) of the base estimator (with freeflyer) -> not needed + Vector q_base_estimator, v_base_estimator; + q_base_estimator.setZero(m_robot->nq()); + v_base_estimator.setZero(m_robot->nv()); + m_robot_util->config_sot_to_urdf(q_sot, q_base_estimator); + m_robot_util->velocity_sot_to_urdf(q_base_estimator, v_sot, v_base_estimator); + pinocchio::computeAllTerms(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); + pinocchio::centerOfMass(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); + } + if (m_contactState == LEFT_SUPPORT || m_contactState == LEFT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); const Vector6& kp_feet = m_kp_feetSIN(iter); const Vector6& kd_feet = m_kd_feetSIN(iter); - m_sampleRF.pos = x_rf_ref; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // Right foot estimation + Eigen::Matrix rf_estim, rf_data; + pinocchio::SE3 rf_estim_se3, rf_data_se3; + m_robot->framePosition(m_estim_data, m_frame_id_rf, rf_estim_se3); + tsid::math::SE3ToVector(rf_estim_se3, rf_estim); + m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, rf_data_se3); + tsid::math::SE3ToVector(rf_data_se3, rf_data); + m_sampleRF.pos = x_rf_ref - rf_estim + rf_data; + } else { + m_sampleRF.pos = x_rf_ref; + } + m_sampleRF.vel = dx_rf_ref; m_sampleRF.acc = ddx_rf_ref; m_taskRF->setReference(m_sampleRF); m_taskRF->Kp(kp_feet); m_taskRF->Kd(kd_feet); + } else if (m_contactState == RIGHT_SUPPORT || m_contactState == RIGHT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_lf_ref = m_lf_ref_posSIN(iter); const Vector6& dx_lf_ref = m_lf_ref_velSIN(iter); const Vector6& ddx_lf_ref = m_lf_ref_accSIN(iter); const Vector6& kp_feet = m_kp_feetSIN(iter); const Vector6& kd_feet = m_kd_feetSIN(iter); - m_sampleLF.pos = x_lf_ref; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // Left foot estimation + Eigen::Matrix lf_estim, lf_data; + pinocchio::SE3 lf_estim_se3, lf_data_se3; + m_robot->framePosition(m_estim_data, m_frame_id_lf, lf_estim_se3); + tsid::math::SE3ToVector(lf_estim_se3, lf_estim); + m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, lf_data_se3); + tsid::math::SE3ToVector(lf_data_se3, lf_data); + m_sampleLF.pos = x_lf_ref - lf_estim + lf_data; + } else { + m_sampleLF.pos = x_lf_ref; + } + m_sampleLF.vel = dx_lf_ref; m_sampleLF.acc = ddx_lf_ref; m_taskLF->setReference(m_sampleLF); m_taskLF->Kp(kp_feet); m_taskLF->Kd(kd_feet); } + if (m_rightHandState == TASK_RIGHT_HAND_ON /*|| m_rightHandState == TASK_RIGHT_HAND_TRANSITION*/) { const Eigen::Matrix& x_rh_ref = m_rh_ref_posSIN(iter); const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter); @@ -891,10 +989,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); getProfiler().start(PROFILE_PREPARE_INV_DYN); - m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); - m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); - - m_sampleCom.pos = x_com_ref - m_com_offset; + + if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // COM estimation + const Vector3& com = m_robot->com(m_invDyn->data()); + // const Vector3& dcom = m_robot->com_vel(m_invDyn->data()); + // m_dcom_offset = dcom - data.vcom[0]; + m_sampleCom.pos = x_com_ref - m_estim_data.com[0] + com; //- m_com_offset + } else { + m_sampleCom.pos = x_com_ref - m_com_offset; + } m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; m_taskCom->setReference(m_sampleCom); @@ -963,6 +1067,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_firstTime) { m_firstTime = false; + m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf); + m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf); m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); // m_robot->computeAllTerms(m_invDyn->data(), q, v); pinocchio::SE3 H_lf = m_robot->position( @@ -980,7 +1086,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); @@ -1104,7 +1215,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { 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); + const VectorN& tau_measured = m_tau_measuredSIN(iter); assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); m_q_desSOUT(iter); @@ -1394,6 +1505,32 @@ DEFINE_SIGNAL_OUT_FUNCTION(com, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(com_est, dynamicgraph::Vector) { + if (!m_initSucceeded) { + std::ostringstream oss("Cannot compute signal com_est before initialization! iter:"); + oss << iter; + SEND_WARNING_STREAM_MSG(oss.str()); + return s; + } + if (s.size() != 3) s.resize(3); + + const VectorN6& q_sot = m_qSIN(iter); + assert(q_sot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN6& v_sot = m_vSIN(iter); + assert(v_sot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + + Vector q_base_estimator, v_base_estimator; + q_base_estimator.setZero(m_robot->nq()); + v_base_estimator.setZero(m_robot->nv()); + m_robot_util->config_sot_to_urdf(q_sot, q_base_estimator); + m_robot_util->velocity_sot_to_urdf(q_base_estimator, v_sot, v_base_estimator); + + pinocchio::Data data = pinocchio::Data(m_robot->model()); + pinocchio::centerOfMass(m_robot->model(), data, q_base_estimator, v_base_estimator); + s = data.com[0]; + return s; +} + DEFINE_SIGNAL_OUT_FUNCTION(com_vel, dynamicgraph::Vector) { if (!m_initSucceeded) { std::ostringstream oss("Cannot compute signal com_vel before initialization! iter:"); @@ -1442,7 +1579,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos_quat, dynamicgraph::Vector) { SEND_WARNING_STREAM_MSG(oss.str()); return s; } - m_tau_desSOUT(iter); + // m_tau_desSOUT(iter); const dg::Vector& x_lf_ref = m_lf_ref_posSIN(iter); pinocchio::SE3 oMi; oMi.translation(x_lf_ref.head<3>() ); @@ -1453,6 +1590,23 @@ DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos_quat, dynamicgraph::Vector) { 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!"); @@ -1499,6 +1653,23 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos_quat, dynamicgraph::Vector) { 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!"); From 479e4aaf33e22b864db9db7aa3b695dd7883488c Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 16 Jun 2020 15:51:42 +0200 Subject: [PATCH 16/47] [Inv_Dyn] Wrong init size for m_q_sot (39 -> 38) --- src/inverse-dynamics-balance-controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index a4a0a08..6d7e4b5 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -618,7 +618,7 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_robot->gear_ratios(gear_ratios_urdf); } - m_q_sot.setZero(m_robot->nq()); + 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); From ccda8eb929f8dc2162121c6453e6ecec309bbf9d Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Fri, 31 Jul 2020 12:04:13 +0200 Subject: [PATCH 17/47] [Inv_Dyn] Add com_admittance references for tasks on CoM in velocity mode Allow to control the DCM (com_adm_ref computed before TSID with a DCM controller and ZMP estimator). --- .../inverse-dynamics-balance-controller.hh | 4 + src/inverse-dynamics-balance-controller.cpp | 123 +++++++++++------- 2 files changed, 82 insertions(+), 45 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index ccb8f2b..4ef856f 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -87,6 +87,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector); DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_adm_ref_pos, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(com_adm_ref_vel, 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); @@ -266,6 +268,7 @@ 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; @@ -276,6 +279,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::tasks::TaskJointPosture* m_taskBlockedJoints; 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; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 6d7e4b5..101970c 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -91,6 +91,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; #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_am_ref_LSIN \ << m_am_ref_dLSIN \ << m_rf_ref_posSIN \ @@ -225,6 +227,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(com_adm_ref_pos, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_IN(com_adm_ref_vel, 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), @@ -654,7 +658,22 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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); + 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); + } else { + m_invDyn->addMotionTask(*m_taskCom, m_w_com, 0); + } // TASK ANGULAR MOMENTUM m_taskAM = new TaskAMEquality("task-am", *m_robot); @@ -701,7 +720,10 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); - // m_sampleAM = 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); @@ -892,19 +914,19 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const double& w_forces = m_w_forcesSIN(iter); const double & w_base_orientation = m_w_base_orientationSIN(iter); - if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ // BASE ESTIMATOR computation to have com and feet estimations // Used to regulate on estimation (desired value of tasks are the estimations) // -> only in velocity because close the TSID loop on itself (v_des, q_des) // In torque close loop on the (q,v) of the base estimator (with freeflyer) -> not needed - Vector q_base_estimator, v_base_estimator; - q_base_estimator.setZero(m_robot->nq()); - v_base_estimator.setZero(m_robot->nv()); - m_robot_util->config_sot_to_urdf(q_sot, q_base_estimator); - m_robot_util->velocity_sot_to_urdf(q_base_estimator, v_sot, v_base_estimator); - pinocchio::computeAllTerms(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); - pinocchio::centerOfMass(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); - } + // Vector q_base_estimator, v_base_estimator; + // q_base_estimator.setZero(m_robot->nq()); + // v_base_estimator.setZero(m_robot->nv()); + // m_robot_util->config_sot_to_urdf(q_sot, q_base_estimator); + // m_robot_util->velocity_sot_to_urdf(q_base_estimator, v_sot, v_base_estimator); + // pinocchio::computeAllTerms(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); + // pinocchio::centerOfMass(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); + // } if (m_contactState == LEFT_SUPPORT || m_contactState == LEFT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); @@ -913,18 +935,18 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const Vector6& kp_feet = m_kp_feetSIN(iter); const Vector6& kd_feet = m_kd_feetSIN(iter); - if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ - // Right foot estimation - Eigen::Matrix rf_estim, rf_data; - pinocchio::SE3 rf_estim_se3, rf_data_se3; - m_robot->framePosition(m_estim_data, m_frame_id_rf, rf_estim_se3); - tsid::math::SE3ToVector(rf_estim_se3, rf_estim); - m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, rf_data_se3); - tsid::math::SE3ToVector(rf_data_se3, rf_data); - m_sampleRF.pos = x_rf_ref - rf_estim + rf_data; - } else { - m_sampleRF.pos = x_rf_ref; - } + // if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // // Right foot estimation + // Eigen::Matrix rf_estim, rf_data; + // pinocchio::SE3 rf_estim_se3, rf_data_se3; + // m_robot->framePosition(m_estim_data, m_frame_id_rf, rf_estim_se3); + // tsid::math::SE3ToVector(rf_estim_se3, rf_estim); + // m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, rf_data_se3); + // tsid::math::SE3ToVector(rf_data_se3, rf_data); + // m_sampleRF.pos = x_rf_ref - rf_estim + rf_data; + // } else { + m_sampleRF.pos = x_rf_ref; + // } m_sampleRF.vel = dx_rf_ref; m_sampleRF.acc = ddx_rf_ref; @@ -939,18 +961,18 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const Vector6& kp_feet = m_kp_feetSIN(iter); const Vector6& kd_feet = m_kd_feetSIN(iter); - if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ - // Left foot estimation - Eigen::Matrix lf_estim, lf_data; - pinocchio::SE3 lf_estim_se3, lf_data_se3; - m_robot->framePosition(m_estim_data, m_frame_id_lf, lf_estim_se3); - tsid::math::SE3ToVector(lf_estim_se3, lf_estim); - m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, lf_data_se3); - tsid::math::SE3ToVector(lf_data_se3, lf_data); - m_sampleLF.pos = x_lf_ref - lf_estim + lf_data; - } else { - m_sampleLF.pos = x_lf_ref; - } + // if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ + // // Left foot estimation + // Eigen::Matrix lf_estim, lf_data; + // pinocchio::SE3 lf_estim_se3, lf_data_se3; + // m_robot->framePosition(m_estim_data, m_frame_id_lf, lf_estim_se3); + // tsid::math::SE3ToVector(lf_estim_se3, lf_estim); + // m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, lf_data_se3); + // tsid::math::SE3ToVector(lf_data_se3, lf_data); + // m_sampleLF.pos = x_lf_ref - lf_estim + lf_data; + // } else { + m_sampleLF.pos = x_lf_ref; + // } m_sampleLF.vel = dx_lf_ref; m_sampleLF.acc = ddx_lf_ref; @@ -992,23 +1014,34 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ // COM estimation - const Vector3& com = m_robot->com(m_invDyn->data()); + // const Vector3& com = m_robot->com(m_invDyn->data()); // const Vector3& dcom = m_robot->com_vel(m_invDyn->data()); // m_dcom_offset = dcom - data.vcom[0]; - m_sampleCom.pos = x_com_ref - m_estim_data.com[0] + com; //- m_com_offset - } else { - m_sampleCom.pos = x_com_ref - m_com_offset; - } + // m_sampleCom.pos = x_com_ref - m_estim_data.com[0] + com; //- m_com_offset + const Vector3& x_com_adm_ref = m_com_adm_ref_posSIN(iter); + const Vector3& dx_com_adm_ref = m_com_adm_ref_velSIN(iter); + m_sampleComAdm.pos = x_com_adm_ref; + m_sampleComAdm.vel = dx_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.pos = x_com_ref - m_com_offset; m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; m_taskCom->setReference(m_sampleCom); m_taskCom->Kp(kp_com); m_taskCom->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_taskCom->name(), w_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_taskCom->name(), w_com); + // } m_sampleAM.vel = L_am_ref; m_sampleAM.acc = dL_am_ref; From 0b0aa5209511a32ee1c0b96360cf5261b15e19c0 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 10 Aug 2020 11:57:53 +0200 Subject: [PATCH 18/47] [param-server] Fix includes for boost in robot-utils of sot-core --- include/sot/torque_control/admittance-controller.hh | 8 ++++---- include/sot/torque_control/base-estimator.hh | 4 ++-- include/sot/torque_control/control-manager.hh | 7 +++++-- include/sot/torque_control/current-controller.hh | 9 +++++---- include/sot/torque_control/ddp-actuator-solver.hh | 2 +- .../sot/torque_control/ddp_pyrene_actuator_solver.hh | 2 +- include/sot/torque_control/device-torque-ctrl.hh | 3 ++- include/sot/torque_control/free-flyer-locator.hh | 5 +++-- include/sot/torque_control/imu_offset_compensation.hh | 2 +- .../inverse-dynamics-balance-controller.hh | 10 ++++++---- include/sot/torque_control/joint-torque-controller.hh | 2 +- .../sot/torque_control/joint-trajectory-generator.hh | 3 ++- include/sot/torque_control/position-controller.hh | 2 +- include/sot/torque_control/simple-inverse-dyn.hh | 10 ++++++---- include/sot/torque_control/torque-offset-estimator.hh | 4 ++-- include/sot/torque_control/trace-player.hh | 2 +- src/base-estimator.cpp | 2 +- src/control-manager.cpp | 3 +-- src/device-torque-ctrl.cpp | 3 +-- src/free-flyer-locator.cpp | 2 +- src/inverse-dynamics-balance-controller.cpp | 2 +- src/simple-inverse-dyn.cpp | 2 +- 22 files changed, 49 insertions(+), 40 deletions(-) diff --git a/include/sot/torque_control/admittance-controller.hh b/include/sot/torque_control/admittance-controller.hh index 5f8f67f..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 { diff --git a/include/sot/torque_control/base-estimator.hh b/include/sot/torque_control/base-estimator.hh index e91250a..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 { diff --git a/include/sot/torque_control/control-manager.hh b/include/sot/torque_control/control-manager.hh index fa0fccd..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 { diff --git a/include/sot/torque_control/current-controller.hh b/include/sot/torque_control/current-controller.hh index 23b711c..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 { 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 3b3b40e..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; diff --git a/include/sot/torque_control/free-flyer-locator.hh b/include/sot/torque_control/free-flyer-locator.hh index 987a374..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 { diff --git a/include/sot/torque_control/imu_offset_compensation.hh b/include/sot/torque_control/imu_offset_compensation.hh index 86de0a4..9cee93a 100644 --- a/include/sot/torque_control/imu_offset_compensation.hh +++ b/include/sot/torque_control/imu_offset_compensation.hh @@ -23,12 +23,12 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include #include #include "boost/assign.hpp" #include #include -#include #include namespace dynamicgraph { diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 4ef856f..f9061e4 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -24,15 +24,18 @@ /* --- 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 @@ -45,7 +48,6 @@ /* HELPER */ #include #include -#include #include diff --git a/include/sot/torque_control/joint-torque-controller.hh b/include/sot/torque_control/joint-torque-controller.hh index d76c1f4..d71bccc 100644 --- a/include/sot/torque_control/joint-torque-controller.hh +++ b/include/sot/torque_control/joint-torque-controller.hh @@ -26,11 +26,11 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include #include /* HELPER */ #include #include -#include #include /*Motor model*/ diff --git a/include/sot/torque_control/joint-trajectory-generator.hh b/include/sot/torque_control/joint-trajectory-generator.hh index 803ebdc..5ea7988 100644 --- a/include/sot/torque_control/joint-trajectory-generator.hh +++ b/include/sot/torque_control/joint-trajectory-generator.hh @@ -23,13 +23,14 @@ /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ + +#include #include #include "boost/assign.hpp" /* HELPER */ #include #include -#include #include #include diff --git a/include/sot/torque_control/position-controller.hh b/include/sot/torque_control/position-controller.hh index e80bb05..604f05b 100644 --- a/include/sot/torque_control/position-controller.hh +++ b/include/sot/torque_control/position-controller.hh @@ -24,12 +24,12 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include #include #include "boost/assign.hpp" #include #include -#include #include diff --git a/include/sot/torque_control/simple-inverse-dyn.hh b/include/sot/torque_control/simple-inverse-dyn.hh index 6f816c7..ae3a665 100644 --- a/include/sot/torque_control/simple-inverse-dyn.hh +++ b/include/sot/torque_control/simple-inverse-dyn.hh @@ -35,15 +35,18 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -#include -#include "boost/assign.hpp" +#include +#include +#include /* Pinocchio */ #include #include #include -#include +#include +#include "boost/assign.hpp" + #include #include #include @@ -54,7 +57,6 @@ /* HELPER */ #include #include -#include #include diff --git a/include/sot/torque_control/torque-offset-estimator.hh b/include/sot/torque_control/torque-offset-estimator.hh index e18fd98..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*/ diff --git a/include/sot/torque_control/trace-player.hh b/include/sot/torque_control/trace-player.hh index 5b13003..71f7a7d 100644 --- a/include/sot/torque_control/trace-player.hh +++ b/include/sot/torque_control/trace-player.hh @@ -24,13 +24,13 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include #include #include #include "boost/assign.hpp" /* HELPER */ #include #include -#include #include namespace dynamicgraph { 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..4b7bd8d 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; 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 101970c..b5394fd 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -5,6 +5,7 @@ #define EIGEN_RUNTIME_NO_MALLOC +#include #include #include @@ -24,7 +25,6 @@ #include #include -#include #if DEBUG #define ODEBUG(x) std::cout << x << std::endl diff --git a/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index 2f1e73d..6d3bee2 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -14,6 +14,7 @@ * with sot-torque-control. If not, see . */ +#include #include #include @@ -29,7 +30,6 @@ #include #include -#include #if DEBUG #define ODEBUG(x) std::cout << x << std::endl From 1043c80e720da84b7797b73118ff556a914703d8 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Fri, 31 Jul 2020 12:07:03 +0200 Subject: [PATCH 19/47] =?UTF-8?q?[Inv=5FDyn]=C2=A0Fix=20wrong=20sendMsg=20?= =?UTF-8?q?type=20(ERROR=20to=20INFO)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/inverse-dynamics-balance-controller.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index b5394fd..52d3828 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -431,7 +431,7 @@ void InverseDynamicsBalanceController::setControlOutputType(const std::string& t void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); + SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); bool res = m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -451,7 +451,7 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran void InverseDynamicsBalanceController::removeLeftFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); + SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); bool res = m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -503,7 +503,7 @@ void InverseDynamicsBalanceController::addTaskLeftHand(/*const double& transitio void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime) { if (m_contactState == LEFT_SUPPORT) { - SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); + SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); const double& w_forces = m_w_forcesSIN.accessCopy(); // TrajectorySample traj_ref = m_taskRF->getReference(); pinocchio::SE3 ref; @@ -523,7 +523,7 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit void InverseDynamicsBalanceController::addLeftFootContact(const double& transitionTime) { if (m_contactState == RIGHT_SUPPORT) { - SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); + SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); const double& w_forces = m_w_forcesSIN.accessCopy(); // TrajectorySample traj_ref = m_taskLF->getReference(); pinocchio::SE3 ref; @@ -742,7 +742,6 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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); @@ -750,7 +749,6 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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"); From d8952448d7bdee6aa9e2f724dd6c034b4a460a34 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Wed, 4 Nov 2020 10:32:20 +0100 Subject: [PATCH 20/47] [Inv_Dyn] Add DCM & AM signal outputs + acceleration ref to taskComAdm Clean some useless comments and change zmp output signal size (2->3). --- .../inverse-dynamics-balance-controller.hh | 4 ++ src/inverse-dynamics-balance-controller.cpp | 55 +++++++++++++------ 2 files changed, 41 insertions(+), 18 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index f9061e4..861d39c 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -91,6 +91,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); @@ -192,6 +193,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); @@ -295,6 +298,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle double m_w_posture; double m_w_hands; double m_w_base_orientation; + double m_omega; /// sqrt(g/h) tsid::math::Vector m_dv_sot; /// desired accelerations (sot order) tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 52d3828..3967d05 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -93,6 +93,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << 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 \ @@ -187,6 +188,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << 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 \ @@ -229,6 +232,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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), @@ -322,6 +326,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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), @@ -912,20 +918,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const double& w_forces = m_w_forcesSIN(iter); const double & w_base_orientation = m_w_base_orientationSIN(iter); - // if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ - // BASE ESTIMATOR computation to have com and feet estimations - // Used to regulate on estimation (desired value of tasks are the estimations) - // -> only in velocity because close the TSID loop on itself (v_des, q_des) - // In torque close loop on the (q,v) of the base estimator (with freeflyer) -> not needed - // Vector q_base_estimator, v_base_estimator; - // q_base_estimator.setZero(m_robot->nq()); - // v_base_estimator.setZero(m_robot->nv()); - // m_robot_util->config_sot_to_urdf(q_sot, q_base_estimator); - // m_robot_util->velocity_sot_to_urdf(q_base_estimator, v_sot, v_base_estimator); - // pinocchio::computeAllTerms(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); - // pinocchio::centerOfMass(m_robot->model(), m_estim_data, q_base_estimator, v_base_estimator); - // } - if (m_contactState == LEFT_SUPPORT || m_contactState == LEFT_SUPPORT_TRANSITION) { const Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); @@ -1018,8 +1010,10 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { // m_sampleCom.pos = x_com_ref - m_estim_data.com[0] + com; //- m_com_offset 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.pos = x_com_adm_ref; m_sampleComAdm.vel = dx_com_adm_ref; + m_sampleCom.acc = ddx_com_adm_ref; m_taskComAdm->setReference(m_sampleComAdm); m_taskComAdm->Kp(kp_com); m_taskComAdm->Kd(kd_com); @@ -1101,6 +1095,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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); + const Vector3& com = m_robot->com(m_invDyn->data()); + m_omega = std::sqrt(9.81 / com[2]); // 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)); @@ -1164,9 +1160,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { if (m_invDyn->getContactForces(m_contactLF->name(), sol, tmp)) m_f_LF = m_contactLF->getForceGeneratorMatrix() * tmp; m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot); - // m_tau_sot += kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) + - // kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints)); - getProfiler().stop(PROFILE_TAU_DES_COMPUTATION); m_t += m_dt; @@ -1334,6 +1327,17 @@ DEFINE_SIGNAL_OUT_FUNCTION(am_dL, dynamicgraph::Vector) { 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!"); @@ -1512,7 +1516,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); @@ -1520,6 +1524,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; } @@ -1575,6 +1580,20 @@ 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 base_orientation before initialization! iter:"); From a1f38cdbc07bbed4807f83916fd4422fcac0a18b Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Wed, 4 Nov 2020 10:37:36 +0100 Subject: [PATCH 21/47] =?UTF-8?q?[Inv=5FDyn]=C2=A0Change=20formulation=20o?= =?UTF-8?q?f=20the=20PD+?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit To have feed-forward terms in torque and acceleration. Not useful for now but perhaps on real robot. By default the feed-forward term in acceleration should be 0 and 1 in torque. --- .../inverse-dynamics-balance-controller.hh | 2 ++ src/inverse-dynamics-balance-controller.cpp | 18 ++++++++++++++---- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 861d39c..899c98f 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -132,6 +132,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 3967d05..860e6e0 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -133,6 +133,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_kp_posSIN \ << m_kd_posSIN \ << m_kp_tauSIN \ + << m_kff_tauSIN \ + << m_kff_dqSIN \ << m_w_comSIN \ << m_w_amSIN \ << m_w_feetSIN \ @@ -272,6 +274,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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), @@ -1232,8 +1236,12 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, 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); + 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)); @@ -1243,13 +1251,15 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); m_q_desSOUT(iter); + if (tau_measured.sum() < 100) { + kp_tau.setZero(); + } - s = m_tau_sot + + 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)); - s += kp_tau.cwiseProduct(tau_measured - s); - return s; } From 484eea6c797005ea8c9617cd949cd6dca9ea0cfd Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Wed, 4 Nov 2020 11:08:24 +0100 Subject: [PATCH 22/47] [Inv_Dyn] Add function actFrame to transform the CoM and Feet frames This transformation is necessary to have the tasks acting in the same frames as the trajectory reference ones. If they are expressed in the same frame (i.e. the world) the transformation is equal to the identity matrix. I need this to use the trajectories of the multicontact-locomotion-planning framework, for the walks on stairs and platforms which have a reference frame different from the world. --- .../inverse-dynamics-balance-controller.hh | 3 + src/inverse-dynamics-balance-controller.cpp | 139 ++++++++++-------- 2 files changed, 82 insertions(+), 60 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 899c98f..747b7bd 100644 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -74,6 +74,7 @@ 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); @@ -301,6 +302,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 860e6e0..88a3e2d 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -422,6 +422,35 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st docCommandVoid1("lowers the left hand.", "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() { const Vector3& com = m_robot->com(m_invDyn->data()); m_com_offset = m_zmp - com; @@ -680,10 +709,8 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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); - } else { - m_invDyn->addMotionTask(*m_taskCom, m_w_com, 0); } + m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1); // TASK ANGULAR MOMENTUM m_taskAM = new TaskAMEquality("task-am", *m_robot); @@ -743,22 +770,43 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_frame_id_rh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_frame_id_lh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); + + 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)); + + 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_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 VectorN6& q_robot = m_qSIN(0); - assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); - const VectorN6& v_robot = m_vSIN(0); - assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); const dg::Vector& com_measured = m_com_measuredSIN(0); assert(com_measured.size() == 3); - - m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); - m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf); - - pinocchio::Data data = pinocchio::Data(m_robot->model()); - pinocchio::centerOfMass(m_robot->model(), data, m_q_urdf, m_v_urdf); - m_com_offset = com_measured - data.com[0]; + 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"); @@ -928,20 +976,7 @@ 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); - - // if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ - // // Right foot estimation - // Eigen::Matrix rf_estim, rf_data; - // pinocchio::SE3 rf_estim_se3, rf_data_se3; - // m_robot->framePosition(m_estim_data, m_frame_id_rf, rf_estim_se3); - // tsid::math::SE3ToVector(rf_estim_se3, rf_estim); - // m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, rf_data_se3); - // tsid::math::SE3ToVector(rf_data_se3, rf_data); - // m_sampleRF.pos = x_rf_ref - rf_estim + rf_data; - // } else { - m_sampleRF.pos = x_rf_ref; - // } - + m_sampleRF.pos = actFrame(m_transformFrameFeet, x_rf_ref); m_sampleRF.vel = dx_rf_ref; m_sampleRF.acc = ddx_rf_ref; m_taskRF->setReference(m_sampleRF); @@ -954,20 +989,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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); - - // if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ - // // Left foot estimation - // Eigen::Matrix lf_estim, lf_data; - // pinocchio::SE3 lf_estim_se3, lf_data_se3; - // m_robot->framePosition(m_estim_data, m_frame_id_lf, lf_estim_se3); - // tsid::math::SE3ToVector(lf_estim_se3, lf_estim); - // m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, lf_data_se3); - // tsid::math::SE3ToVector(lf_data_se3, lf_data); - // m_sampleLF.pos = x_lf_ref - lf_estim + lf_data; - // } else { - m_sampleLF.pos = x_lf_ref; - // } - + m_sampleLF.pos = actFrame(m_transformFrameFeet, x_lf_ref); m_sampleLF.vel = dx_lf_ref; m_sampleLF.acc = ddx_lf_ref; m_taskLF->setReference(m_sampleLF); @@ -1007,11 +1029,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().start(PROFILE_PREPARE_INV_DYN); if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ - // COM estimation - // const Vector3& com = m_robot->com(m_invDyn->data()); - // const Vector3& dcom = m_robot->com_vel(m_invDyn->data()); - // m_dcom_offset = dcom - data.vcom[0]; - // m_sampleCom.pos = x_com_ref - m_estim_data.com[0] + com; //- m_com_offset 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); @@ -1021,23 +1038,23 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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); - // } + 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.pos = x_com_ref - m_com_offset; + m_sampleCom.pos = actFrame(m_transformFrameCom, x_com_ref) - m_com_offset; m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; m_taskCom->setReference(m_sampleCom); m_taskCom->Kp(kp_com); m_taskCom->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_taskCom->name(), w_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_taskCom->name(), w_com); + } m_sampleAM.vel = L_am_ref; m_sampleAM.acc = dL_am_ref; @@ -1640,7 +1657,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos_quat, dynamicgraph::Vector) { return s; } // m_tau_desSOUT(iter); - const dg::Vector& x_lf_ref = m_lf_ref_posSIN(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) ); @@ -1703,7 +1721,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos_quat, dynamicgraph::Vector) { return s; } // m_tau_desSOUT(iter); - const dg::Vector& x_rf_ref = m_rf_ref_posSIN(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) ); From 91057f4d4704e1654b245858750be7a7e7987185 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Thu, 5 Nov 2020 16:37:22 +0100 Subject: [PATCH 23/47] Missing include of robot-utils.hh --- include/sot/torque_control/joint-torque-controller.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/include/sot/torque_control/joint-torque-controller.hh b/include/sot/torque_control/joint-torque-controller.hh index aab867c..d039a8d 100644 --- a/include/sot/torque_control/joint-torque-controller.hh +++ b/include/sot/torque_control/joint-torque-controller.hh @@ -33,6 +33,7 @@ #include #include #include +#include /*Motor model*/ #include From 99b0df4c16f7fcacbd895bd72a2b094607035833 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Mon, 8 Mar 2021 11:54:57 +0100 Subject: [PATCH 24/47] [Inv_Dyn] Add torque bounds constraint + add feet quaternion signals Put the feet motions task in the cost (no more constraints) --- .../inverse-dynamics-balance-controller.hh | 4 ++ src/inverse-dynamics-balance-controller.cpp | 57 ++++++++++++++++--- 2 files changed, 54 insertions(+), 7 deletions(-) mode change 100644 => 100755 include/sot/torque_control/inverse-dynamics-balance-controller.hh 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 747b7bd..e9cba5b --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -42,6 +42,7 @@ #include #include #include +#include // #include "tsid/tasks/task-angular-momentum-integral-equality.hpp" #include @@ -203,8 +204,10 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); @@ -285,6 +288,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::tasks::TaskSE3Equality* m_taskLH; tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; + tsid::tasks::TaskActuationBounds* m_taskActBounds; tsid::trajectories::TrajectorySample m_sampleCom; tsid::trajectories::TrajectorySample m_sampleComAdm; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 88a3e2d..e48976b 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -197,8 +197,10 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << 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 \ @@ -337,8 +339,10 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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_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), @@ -434,7 +438,7 @@ Vector InverseDynamicsBalanceController::actFrame(pinocchio::SE3 frame, Vector v 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()) * + R = (Eigen::AngleAxisd(-euler(2), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(-euler(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX())) .toRotationMatrix(); @@ -478,7 +482,7 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran MSG_TYPE_ERROR); } const double& w_feet = m_w_feetSIN.accessCopy(); - m_invDyn->addMotionTask(*m_taskRF, w_feet, 0); + m_invDyn->addMotionTask(*m_taskRF, w_feet, 1); if (transitionTime > m_dt) { m_contactState = LEFT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; @@ -498,7 +502,7 @@ void InverseDynamicsBalanceController::removeLeftFootContact(const double& trans MSG_TYPE_ERROR); } const double& w_feet = m_w_feetSIN.accessCopy(); - m_invDyn->addMotionTask(*m_taskLF, w_feet, 0); + m_invDyn->addMotionTask(*m_taskLF, w_feet, 1); if (transitionTime > m_dt) { m_contactState = RIGHT_SUPPORT_TRANSITION; m_contactTransitionTime = m_t + transitionTime; @@ -746,6 +750,12 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskPosture->Kd(kd_posture); m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 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); + // 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); @@ -1628,9 +1638,12 @@ DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, dynamicgraph::Vector) { 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; } @@ -1650,6 +1663,21 @@ DEFINE_SIGNAL_OUT_FUNCTION(left_foot_pos, dynamicgraph::Vector) { } 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; @@ -1714,6 +1742,21 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos, dynamicgraph::Vector) { } 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; From 4780a8963b2fb42a2feea9105a723f2421d9aa1f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Wed, 10 Mar 2021 14:28:29 +0100 Subject: [PATCH 25/47] =?UTF-8?q?[Inv=5FDyn]=C2=A0Remove=20transformFrame?= =?UTF-8?q?=20on=20CoM=20and=20feet=20for=20Isabelle=20trajectory?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/inverse-dynamics-balance-controller.cpp | 43 ++++++++++++--------- 1 file changed, 25 insertions(+), 18 deletions(-) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index e48976b..39e5bc0 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -793,22 +793,22 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& Vector3 com_estim = data.com[0]; 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); - } + // 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); - } + // 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()){ @@ -1054,7 +1054,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskComAdm->name(), w_com); } } - m_sampleCom.pos = actFrame(m_transformFrameCom, x_com_ref) - m_com_offset; + m_sampleCom.pos = x_com_ref - m_com_offset; //actFrame(m_transformFrameCom, x_com_ref) - m_com_offset; // m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; m_taskCom->setReference(m_sampleCom); @@ -1077,9 +1077,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskAM->name(), w_am); } - m_sampleWaist.pos = x_waist_ref; - m_sampleWaist.vel = dx_waist_ref; - m_sampleWaist.acc = ddx_waist_ref; + 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 Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); + const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); + const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); + Eigen::Matrix sum_x = x_lf_ref + x_rf_ref; + m_sampleWaist.pos = 0.5 * actFrame(m_transformFrameFeet, sum_x); //x_waist_ref; + m_sampleWaist.vel = 0.5 * (dx_lf_ref + dx_rf_ref); //dx_waist_ref; + m_sampleWaist.acc = 0.5 * (ddx_lf_ref + ddx_rf_ref); //ddx_waist_ref; m_taskWaist->setReference(m_sampleWaist); m_taskWaist->Kp(kp_base_orientation); m_taskWaist->Kd(kd_base_orientation); From 2121603eae71032b7b469f0dddeb7d37d20dcf58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Wed, 24 Mar 2021 10:48:13 +0100 Subject: [PATCH 26/47] [Test] remove useless headers --- CMakeLists.txt | 1 + src/inverse-dynamics-balance-controller.cpp | 1 - src/simple-inverse-dyn.cpp | 1 - 3 files changed, 1 insertion(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 853f19e..b3d6c14 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,7 @@ 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}/position-controller.hh include/${CUSTOM_HEADER_DIR}/control-manager.hh include/${CUSTOM_HEADER_DIR}/current-controller.hh diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 39e5bc0..f8d6e19 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -6,7 +6,6 @@ #define EIGEN_RUNTIME_NO_MALLOC #include -#include #include #include diff --git a/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index 6d3bee2..bc602df 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -15,7 +15,6 @@ */ #include -#include #include #include From 823f7d7d3c454b65020fe3c777fa70218ea85d6a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Thu, 25 Mar 2021 14:07:09 +0100 Subject: [PATCH 27/47] [Posture_task] Add a simple entity with only a posture task --- CMakeLists.txt | 1 + include/sot/torque_control/posture-task.hh | 163 +++++++ src/CMakeLists.txt | 1 + src/posture-task-python.hh | 3 + src/posture-task.cpp | 486 +++++++++++++++++++++ 5 files changed, 654 insertions(+) create mode 100644 include/sot/torque_control/posture-task.hh create mode 100644 src/posture-task-python.hh create mode 100644 src/posture-task.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b3d6c14..5f698d5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -79,6 +79,7 @@ SET(${PROJECT_NAME}_HEADERS 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/include/sot/torque_control/posture-task.hh b/include/sot/torque_control/posture-task.hh new file mode 100644 index 0000000..54c6c87 --- /dev/null +++ b/include/sot/torque_control/posture-task.hh @@ -0,0 +1,163 @@ +/* + * 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 + +/* 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(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); + + /* --- 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; + + /// TASKS + tsid::tasks::TaskJointPosture* m_taskPosture; + tsid::tasks::TaskJointPosture* m_taskBlockedJoints; + + /// Trajectories of the tasks + tsid::trajectories::TrajectorySample m_samplePosture; + + /// Weights of the Tasks (which can be changed) + double m_w_posture; + + /// 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/src/CMakeLists.txt b/src/CMakeLists.txt index 4e9a284..339cfec 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 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..ed53250 --- /dev/null +++ b/src/posture-task.cpp @@ -0,0 +1,486 @@ +/* + * 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_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_right_foot_posSOUT << m_left_foot_posSOUT + + +/// 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(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) + , 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 VectorN& kp_posture = m_kp_postureSIN(0); + const VectorN& kd_posture = m_kd_postureSIN(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); + + // 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, 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 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()); + + } 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->mask(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); + + + // Update tasks + 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_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); + } + + + 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); + + } 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; +} + +/* --- 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 From 2d09c1369f142243bda59aea6637f22b59070529 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Fri, 26 Mar 2021 15:53:16 +0100 Subject: [PATCH 28/47] [Posture_Task] Add constraint torque limits --- include/sot/torque_control/posture-task.hh | 2 ++ src/posture-task.cpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/include/sot/torque_control/posture-task.hh b/include/sot/torque_control/posture-task.hh index 54c6c87..32eed29 100644 --- a/include/sot/torque_control/posture-task.hh +++ b/include/sot/torque_control/posture-task.hh @@ -51,6 +51,7 @@ #include #include #include +#include /* HELPER */ #include @@ -133,6 +134,7 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { /// TASKS tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; + tsid::tasks::TaskActuationBounds* m_taskActBounds; /// Trajectories of the tasks tsid::trajectories::TrajectorySample m_samplePosture; diff --git a/src/posture-task.cpp b/src/posture-task.cpp index ed53250..37becb4 100644 --- a/src/posture-task.cpp +++ b/src/posture-task.cpp @@ -217,6 +217,12 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { m_taskPosture->Kd(kd_posture); m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 0); + // 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); + // TRAJECTORY INIT m_samplePosture = TrajectorySample(m_robot->nv() - 6); From a5d661f3f59d4fe720cf99176af018ed50f0e3a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Mon, 29 Mar 2021 18:13:08 +0200 Subject: [PATCH 29/47] Fix Gains in contact6D because of changes in TSID --- src/inverse-dynamics-balance-controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index f8d6e19..0f2f5e4 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -683,13 +683,13 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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, 10, 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, 10, 1); if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones()); From 0a4b8cf792b5622a9ab56e8a44c1825b850d6e8a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Mon, 29 Mar 2021 18:59:10 +0200 Subject: [PATCH 30/47] [Posture_Task] Add contact6D --- include/sot/torque_control/posture-task.hh | 13 ++++++ src/posture-task.cpp | 53 +++++++++++++++++++++- 2 files changed, 64 insertions(+), 2 deletions(-) diff --git a/include/sot/torque_control/posture-task.hh b/include/sot/torque_control/posture-task.hh index 32eed29..1443cfb 100644 --- a/include/sot/torque_control/posture-task.hh +++ b/include/sot/torque_control/posture-task.hh @@ -52,6 +52,7 @@ #include #include #include +#include /* HELPER */ #include @@ -89,6 +90,16 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { 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(q, dynamicgraph::Vector); DECLARE_SIGNAL_IN(v, dynamicgraph::Vector); @@ -130,6 +141,8 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { /// 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; diff --git a/src/posture-task.cpp b/src/posture-task.cpp index 37becb4..9041ffd 100644 --- a/src/posture-task.cpp +++ b/src/posture-task.cpp @@ -79,6 +79,15 @@ using namespace dg::sot; << 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_qSIN \ << m_vSIN \ << m_com_measuredSIN \ @@ -112,6 +121,15 @@ PostureTask(const std::string& name) , 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(q, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector) , CONSTRUCT_SIGNAL_IN(com_measured, dynamicgraph::Vector) @@ -183,6 +201,15 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { 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); @@ -211,11 +238,24 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { 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, 0); + m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1); // ACTUATION BOUNDS TASK Vector tau_max = 0.8 * m_robot->model().effortLimit.tail(m_robot->nv() - 6); @@ -285,7 +325,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); @@ -349,6 +389,15 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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); From 3d2f536fc9666690737c5f73b6d94796b656055d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Tue, 30 Mar 2021 14:34:00 +0200 Subject: [PATCH 31/47] Correction on contact6D gains Forgot to set previous modifications when removing/adding contacts --- src/inverse-dynamics-balance-controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 0f2f5e4..8fde0a2 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -553,7 +553,7 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit 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); + bool res = m_invDyn->addRigidContact(*m_contactRF, w_forces, 10, 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), @@ -573,7 +573,7 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti 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); + bool res = m_invDyn->addRigidContact(*m_contactLF, w_forces, 10, 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), From c53282a5fca3c9d8e5c39bd835983672ffecf1f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Tue, 30 Mar 2021 17:50:46 +0200 Subject: [PATCH 32/47] [SimpleInvDyn] Add actuation bounds constraint --- include/sot/torque_control/simple-inverse-dyn.hh | 2 ++ src/simple-inverse-dyn.cpp | 10 ++++++++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/include/sot/torque_control/simple-inverse-dyn.hh b/include/sot/torque_control/simple-inverse-dyn.hh index ae3a665..e7ffb7a 100644 --- a/include/sot/torque_control/simple-inverse-dyn.hh +++ b/include/sot/torque_control/simple-inverse-dyn.hh @@ -52,6 +52,7 @@ #include #include #include +#include #include /* HELPER */ @@ -169,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/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index bc602df..0878f67 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -301,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"); @@ -390,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); From 5ff9478263af799388f25e9d7e8a8d7b89bcf1aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Tue, 30 Mar 2021 18:07:14 +0200 Subject: [PATCH 33/47] [Energy] Cherrypick commit branch topic/energy --- .../inverse-dynamics-balance-controller.hh | 16 ++ src/inverse-dynamics-balance-controller.cpp | 253 ++++++++++++++++-- 2 files changed, 242 insertions(+), 27 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index e9cba5b..d4eb402 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -86,6 +87,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle void removeTaskRightHand(const double& transitionTime); void addTaskLeftHand(/*const double& transitionTime*/); void removeTaskLeftHand(const double& transitionTime); + void addTaskEnergy(const double& transitionTime); + void removeTaskEnergy(const double& transitionTime); /* --- SIGNALS --- */ DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector); @@ -173,6 +176,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); @@ -222,6 +226,12 @@ 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(energy, double); + DECLARE_SIGNAL_OUT(energy_derivative, double); + DECLARE_SIGNAL_OUT(energy_derivative_time_range, double); + DECLARE_SIGNAL_OUT(energy_bound, double); + DECLARE_SIGNAL_OUT(task_energy_const, double); + DECLARE_SIGNAL_OUT(task_energy_bound, 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); @@ -289,6 +299,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; tsid::tasks::TaskActuationBounds* m_taskActBounds; + tsid::tasks::TaskEnergy* m_taskEnergy; tsid::trajectories::TrajectorySample m_sampleCom; tsid::trajectories::TrajectorySample m_sampleComAdm; @@ -299,6 +310,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::trajectories::TrajectorySample m_sampleLH; tsid::trajectories::TrajectorySample m_sampleWaist; tsid::trajectories::TrajectorySample m_samplePosture; + tsid::trajectories::TrajectorySample m_sampleEnergy; double m_w_com; double m_w_am; @@ -318,6 +330,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::math::Vector m_tau_sot; /// desired torques (sot 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 @@ -330,6 +343,9 @@ 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 + double m_previous_energy; + tsid::math::Vector m_previous_vel; + tsid::math::Vector m_previous_q; typedef pinocchio::Data::Matrix6x Matrix6x; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 8fde0a2..fb69b09 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -165,7 +165,8 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_wrench_left_footSIN \ << m_wrench_right_footSIN \ << m_ref_phaseSIN \ - << m_active_jointsSIN + << m_active_jointsSIN \ + << m_ref_pos_finalSIN #define OUTPUT_SIGNALS m_tau_desSOUT \ << m_MSOUT \ @@ -214,6 +215,12 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_right_hand_accSOUT \ << m_right_foot_acc_desSOUT \ << m_left_foot_acc_desSOUT \ + << m_energySOUT \ + << m_energy_derivativeSOUT \ + << m_energy_derivative_time_rangeSOUT \ + << m_energy_boundSOUT \ + << m_task_energy_constSOUT \ + << m_task_energy_boundSOUT /// Define EntityClassName here rather than in the header file /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. @@ -308,7 +315,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector), CONSTRUCT_SIGNAL_IN(ref_phase, int), - CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector), + CONSTRUCT_SIGNAL_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), @@ -356,6 +364,12 @@ 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(energy, double, INPUT_SIGNALS << m_q_desSOUT), + CONSTRUCT_SIGNAL_OUT(energy_derivative, double, m_energySOUT), + CONSTRUCT_SIGNAL_OUT(energy_derivative_time_range, double, INPUT_SIGNALS << m_q_desSOUT), + CONSTRUCT_SIGNAL_OUT(energy_bound, double, INPUT_SIGNALS), + CONSTRUCT_SIGNAL_OUT(task_energy_const, double, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(task_energy_bound, double, m_tau_desSOUT), CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN), m_t(0.0), m_initSucceeded(false), @@ -423,6 +437,13 @@ 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)"))); + } Vector InverseDynamicsBalanceController::actFrame(pinocchio::SE3 frame, Vector vec) { @@ -599,6 +620,14 @@ void InverseDynamicsBalanceController::removeTaskLeftHand(const double& transiti } } +void InverseDynamicsBalanceController::addTaskEnergy(const double& transitionTime) { + m_invDyn->addEnergyTask(*m_taskEnergy, 20, 1, transitionTime); +} + +void InverseDynamicsBalanceController::removeTaskEnergy(const double& transitionTime) { + m_invDyn->removeTask(m_taskEnergy->name(), transitionTime); +} + 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); @@ -669,11 +698,16 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot); @@ -764,6 +798,20 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskLH->Kp(kp_hands); m_taskLH->Kd(kd_hands); + 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, q_robot, v_robot, dt, 0.003); + Vector K_energy(m_robot_util->m_nbJoints + 6); + //K_energy = 20.0 * Vector::Ones(m_robot_util->m_nbJoints + 6); + K_energy.head<6>() = 0.0 * Vector::Ones(6); + K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; + m_taskEnergy->K(K_energy); + //m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0); + // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ @@ -773,6 +821,7 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); m_sampleRH = TrajectorySample(3); + m_sampleEnergy = TrajectorySample(m_robot->nv()); 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); @@ -780,11 +829,6 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_frame_id_rh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_frame_id_lh = (int)m_robot->model().getFrameId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); - 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)); - 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()); @@ -792,30 +836,30 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& Vector3 com_estim = data.com[0]; 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); - // } + 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_ERROR); + } 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); - // } + 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_ERROR); + } // 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); + SEND_MSG("COM_measured: " + toString(com_measured), MSG_TYPE_ERROR); m_com_offset = com_measured - com_estim; - SEND_MSG("Update COM: " + toString(m_com_offset), MSG_TYPE_INFO); + SEND_MSG("Update COM: " + toString(m_com_offset), MSG_TYPE_ERROR); } m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog-fast"); @@ -1106,6 +1150,23 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture); } + const VectorN& tau_measured = m_tau_measuredSIN(iter); + assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); + const Vector& q_ref_final = m_ref_pos_finalSIN(iter); + assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); + // Vector q_ref_final_urdf; + // q_ref_final_urdf.setZero(39); + // m_robot_util->config_sot_to_urdf(q_ref_final, q_ref_final_urdf); + + m_sampleEnergy.pos = q_ref_final; + // m_sampleEnergy.vel = m_v_sot; + m_sampleEnergy.acc = m_JF; + // m_sampleEnergy.acc = Vector::Ones(38); + // m_sampleEnergy.acc.head<6>() = 0.0 * Vector::Ones(6); + // m_sampleEnergy.acc.tail(m_robot_util->m_nbJoints) = tau_measured - m_tau_sot; + // m_robot_util->joints_sot_to_urdf(ddq_ref, m_sampleEnergy.acc); + m_taskEnergy->setReference(m_sampleEnergy); + /*m_sampleRH.pos = x_rh_ref; m_sampleRH.vel = dx_rh_ref; m_sampleRH.acc = ddx_rh_ref; @@ -1132,6 +1193,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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; const Vector3& com = m_robot->com(m_invDyn->data()); m_omega = std::sqrt(9.81 / com[2]); // m_robot->computeAllTerms(m_invDyn->data(), q, v); @@ -1191,6 +1253,7 @@ 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; @@ -1288,10 +1351,11 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { 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)); + 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; } @@ -1931,6 +1995,141 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc_des, dynamicgraph::Vector) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(energy, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy before initialization!"); + return s; + } + 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& kp_posture = m_kp_postureSIN(iter); + // assert(K_energy.size() == static_cast(m_robot_util->m_nbJoints)); + const Vector& q_ref_final = m_ref_pos_finalSIN(iter); + assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); + // std::cout << "##################### INV DYN ENERGY ################################" << std::endl; + // std::cout << "q: " << q_robot << std::endl; + // std::cout << "v: " << v_robot << std::endl; + Vector K_energy(m_robot_util->m_nbJoints + 6); + //K_energy = 20.0 * Vector::Ones(m_robot_util->m_nbJoints + 6); + K_energy.head<6>() = 0.0 * Vector::Ones(6); + K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; + m_q_desSOUT(iter); + Vector error_pos = q_robot - q_ref_final; + // std::cout << "############## mass matrix: " << m_invDyn->data().M.bottomRows(m_robot_util->m_nbJoints) << " ############" << std::endl; + s = 0.5 * v_robot.transpose() * m_robot->mass(m_invDyn->data()) * v_robot; + s += 0.5 * error_pos.transpose() * K_energy.cwiseProduct(error_pos); + 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; + } + double energy = m_energySOUT(iter); + // std::cout << "############## previous_energy: " << previous_energy << " ############" << std::endl; + // std::cout << "############## energy: " << energy << " ############" << std::endl; + s = (energy - m_previous_energy)/m_dt; + // m_q_desSOUT(iter); + // double E_c = m_taskEnergy->get_E_c(); + // double E_p = m_taskEnergy->get_E_p(); + // s = E_c + E_p; + m_previous_energy = energy; + // Vector BK = m_taskEnergy->get_BK_vector(); + // double BKv = BK.transpose() * m_previous_vel; + // s = BKv/m_dt; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(energy_derivative_time_range, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal energy_derivative_time_range before initialization!"); + return s; + } + double m_time_preview = 0.003; + 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& previous_vel = m_vSIN(iter-1); + // std::cout << "############## previous_vel: " << previous_vel << " ############" << std::endl; + // assert(previous_vel.size() == static_cast(m_robot_util->m_nbJoints + 6)); + const VectorN& kp_posture = m_kp_postureSIN(iter); + // assert(K_energy.size() == static_cast(m_robot_util->m_nbJoints)); + const Vector& q_ref_final = m_ref_pos_finalSIN(iter); + assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); + + Vector K_energy(m_robot_util->m_nbJoints + 6); + K_energy = 20.0 * Vector::Ones(m_robot_util->m_nbJoints + 6); + // K_energy.head<6>() = 0.0 * Vector::Ones(6); + // K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; + m_q_desSOUT(iter); + + Vector error_pos = q_robot - q_ref_final; + Vector error_pos_prev = q_ref_final - m_previous_q; + double time_ratio = (m_time_preview * m_time_preview)/(2*m_dt); + Vector v_des_t = error_pos_prev * time_ratio; + Vector a_des_t = ((error_pos_prev/m_dt) - m_previous_vel) * time_ratio; + + m_previous_q = q_robot; + m_previous_vel = v_robot; + + // E_c + // s = v_robot.transpose() * m_robot->mass(m_invDyn->data()) * v_robot; + // dE_p + //s = (error_pos*m_time_preview + v_des_t).transpose() * K_energy.cwiseProduct(m_previous_vel/m_dt + m_dv_sot); + s = v_robot.transpose() * K_energy.cwiseProduct(error_pos); + // dE_c + s += ((1/m_dt) * (v_robot*m_time_preview) + a_des_t).transpose() * m_robot->mass(m_invDyn->data()) * m_dv_sot; + 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)); + // const VectorN& tau_measured = m_tau_measuredSIN(iter); + // assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); + 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_const, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_const before initialization!"); + return s; + } + m_q_desSOUT(iter); + const VectorN6& v_robot = m_vSIN(iter); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + Vector A = m_taskEnergy->get_A(); + Vector BK = m_taskEnergy->get_BK_vector(); + double vJF = v_robot.transpose() * m_JF; + double AMdv = (A.transpose() * m_robot->mass(m_invDyn->data()))* m_dv_sot; // + BK.transpose() + s = AMdv + vJF; + return s; +} + +DEFINE_SIGNAL_OUT_FUNCTION(task_energy_bound, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_bound before initialization!"); + return s; + } + m_q_desSOUT(iter); + const VectorN6& v_robot = m_vSIN(iter); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + // double vJF = v_robot.transpose() * m_JF; + double upB = m_taskEnergy->get_upperBound() + v_robot.transpose() * m_robot->nonLinearEffects(m_invDyn->data()); + s = upB; + return s; +} + /* --- COMMANDS ---------------------------------------------------------- */ /* ------------------------------------------------------------------- */ From 2034bc8bfff0a830ec225df7ed69504c009c31c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Fri, 16 Jul 2021 18:55:44 +0200 Subject: [PATCH 34/47] =?UTF-8?q?[Energy]=C2=A0Add=20smoothness=20coeffici?= =?UTF-8?q?ents?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../inverse-dynamics-balance-controller.hh | 13 +- src/inverse-dynamics-balance-controller.cpp | 313 +++++++++++++----- 2 files changed, 236 insertions(+), 90 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index d4eb402..f0d67f2 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -44,7 +44,8 @@ #include #include #include -// #include "tsid/tasks/task-angular-momentum-integral-equality.hpp" +#include +#include #include /* HELPER */ @@ -89,6 +90,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle void removeTaskLeftHand(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); @@ -228,10 +230,15 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(energy, double); DECLARE_SIGNAL_OUT(energy_derivative, double); - DECLARE_SIGNAL_OUT(energy_derivative_time_range, double); + DECLARE_SIGNAL_OUT(energy_tank, double); DECLARE_SIGNAL_OUT(energy_bound, double); DECLARE_SIGNAL_OUT(task_energy_const, double); DECLARE_SIGNAL_OUT(task_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_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); @@ -299,6 +306,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; tsid::tasks::TaskActuationBounds* m_taskActBounds; + tsid::tasks::TaskJointBounds* m_taskJointBounds; + tsid::tasks::TaskCopEquality* m_taskCoP; tsid::tasks::TaskEnergy* m_taskEnergy; tsid::trajectories::TrajectorySample m_sampleCom; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index fb69b09..ae79dce 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -217,10 +217,15 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_left_foot_acc_desSOUT \ << m_energySOUT \ << m_energy_derivativeSOUT \ - << m_energy_derivative_time_rangeSOUT \ + << m_energy_tankSOUT \ << m_energy_boundSOUT \ << m_task_energy_constSOUT \ - << m_task_energy_boundSOUT + << m_task_energy_boundSOUT \ + << m_task_energy_alphaSOUT \ + << m_task_energy_betaSOUT \ + << m_task_energy_gammaSOUT \ + << m_task_energy_SSOUT \ + << 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. @@ -366,10 +371,15 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_OUT(left_foot_acc_des, 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_derivative_time_range, double, INPUT_SIGNALS << m_q_desSOUT), + CONSTRUCT_SIGNAL_OUT(energy_tank, double, INPUT_SIGNALS << m_q_desSOUT), CONSTRUCT_SIGNAL_OUT(energy_bound, double, INPUT_SIGNALS), CONSTRUCT_SIGNAL_OUT(task_energy_const, double, m_tau_desSOUT), - CONSTRUCT_SIGNAL_OUT(task_energy_bound, double, m_tau_desSOUT), + CONSTRUCT_SIGNAL_OUT(task_energy_bound, double, m_tau_desSOUT), + 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_A, double, m_tau_desSOUT), CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN), m_t(0.0), m_initSucceeded(false), @@ -443,6 +453,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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)"))); } @@ -494,7 +507,7 @@ void InverseDynamicsBalanceController::setControlOutputType(const std::string& t void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); + SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); bool res = m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -514,7 +527,7 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran void InverseDynamicsBalanceController::removeLeftFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); + SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); bool res = m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -566,15 +579,18 @@ void InverseDynamicsBalanceController::addTaskLeftHand(/*const double& transitio void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime) { if (m_contactState == LEFT_SUPPORT) { - SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); + SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); // TrajectorySample traj_ref = m_taskRF->getReference(); pinocchio::SE3 ref; // vectorToSE3(traj_ref.pos, ref); ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); m_contactRF->setReference(ref); + Vector forceRef = Vector::Zero(6); + forceRef[2] = 450.0; + m_contactRF->setForceReference(forceRef); m_invDyn->removeTask(m_taskRF->name(), transitionTime); - bool res = m_invDyn->addRigidContact(*m_contactRF, w_forces, 10, 1); + bool res = m_invDyn->addRigidContact(*m_contactRF, w_forces, 1e3, 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), @@ -586,7 +602,7 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit void InverseDynamicsBalanceController::addLeftFootContact(const double& transitionTime) { if (m_contactState == RIGHT_SUPPORT) { - SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); + SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); // TrajectorySample traj_ref = m_taskLF->getReference(); pinocchio::SE3 ref; @@ -594,7 +610,10 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti 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, 10, 1); + Vector forceRef = Vector::Zero(6); + forceRef[2] = 450.0; + m_contactLF->setForceReference(forceRef); + bool res = m_invDyn->addRigidContact(*m_contactLF, w_forces, 1e3, 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), @@ -628,6 +647,10 @@ void InverseDynamicsBalanceController::removeTaskEnergy(const double& transition m_invDyn->removeTask(m_taskEnergy->name(), transitionTime); } +void InverseDynamicsBalanceController::setEnergyTank(const double& tankValue) { + m_taskEnergy->set_E_tank(tankValue); +} + 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); @@ -717,13 +740,17 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& contactPoints, contactNormal, mu, fMin, fMaxRF); m_contactRF->Kp(kp_contact); m_contactRF->Kd(kd_contact); - m_invDyn->addRigidContact(*m_contactRF, w_forces, 10, 1); + Vector forceRef = Vector::Zero(6); + forceRef[2] = 450.0; + m_contactRF->setForceReference(forceRef); + m_invDyn->addRigidContact(*m_contactRF, w_forces, 1e3, 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, 10, 1); + m_contactLF->setForceReference(forceRef); + m_invDyn->addRigidContact(*m_contactLF, w_forces, 1e3, 1); if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) { m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones()); @@ -747,6 +774,9 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskComAdm->setMask(mask_com_adm); m_invDyn->addMotionTask(*m_taskComAdm, m_w_com, 1); } + // Eigen::VectorXd mask_com(3); + // mask_com << 1, 1, 0; + // m_taskCom->setMask(mask_com); m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1); // TASK ANGULAR MOMENTUM @@ -781,22 +811,44 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskPosture = new TaskJointPosture("task-posture", *m_robot); m_taskPosture->Kp(kp_posture); m_taskPosture->Kd(kd_posture); + //Eigen::VectorXd mask_armR(m_robot->nv() - 6); + //mask_armR = Vector::Ones(m_robot->nv() - 6); + //mask_armR[25] = 0.0; + //mask_armR.segment<8>(22) = Vector::Zero(8); + + //m_taskPosture->setMask(mask_armR); + m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1); // ACTUATION BOUNDS TASK Vector tau_max = 0.8 * m_robot->model().effortLimit.tail(m_robot->nv() - 6); + std::cout << "################# tau_max : " << tau_max << " ############" << std::endl; 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); + //m_invDyn->addMotionTask(*m_taskJointBounds, 1.0, 0); + + // TASK CoP + // m_taskCoP = new TaskCopEquality("task-cop", *m_robot); + // m_invDyn->addForceTask(*m_taskCoP, 0.0, 1, 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 = new TaskSE3Equality("task-rh", *m_robot, "gripper_right_base_link"); //m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_taskRH->Kp(kp_hands); m_taskRH->Kd(kd_hands); + Eigen::VectorXd mask_hands = Vector::Ones(6); + mask_hands.tail(3) = Vector::Zero(3); + m_taskRH->setMask(mask_hands); m_taskLH = new TaskSE3Equality("task-lh", *m_robot, m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); m_taskLH->Kp(kp_hands); m_taskLH->Kd(kd_hands); + m_taskLH->setMask(mask_hands); const VectorN6& q_robot = m_qSIN(0); assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); @@ -810,7 +862,7 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& K_energy.head<6>() = 0.0 * Vector::Ones(6); K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; m_taskEnergy->K(K_energy); - //m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0); + m_invDyn->addEnergyTask(*m_taskEnergy, 10, 1); // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); @@ -820,13 +872,14 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_sampleAM = TrajectorySample(3); m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); - m_sampleRH = TrajectorySample(3); + m_sampleRH = TrajectorySample(6); + m_sampleLH = TrajectorySample(6); m_sampleEnergy = TrajectorySample(m_robot->nv()); 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_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_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); @@ -834,7 +887,12 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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]); + // int waistFrameId = (int)m_robot->model().getFrameId("root_joint"); + // pinocchio::SE3 se3Waist; + // m_robot->framePosition(data, waistFrameId, se3Waist); + // SEND_MSG("se3Waist: " + toString(se3Waist), MSG_TYPE_ERROR); 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){ @@ -902,10 +960,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 */ @@ -1023,6 +1081,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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); const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); @@ -1050,15 +1110,29 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_taskLF->Kd(kd_feet); } + // if (m_contactState != DOUBLE_SUPPORT) { + // Vector3 zmpDes = (x_com_ref - m_com_offset) - ddx_com_ref/(m_omega*m_omega); + // zmpDes[2] = 0.0; + // m_taskCoP->setReference(zmpDes); + // m_invDyn->updateTaskWeight(m_taskCoP->name(), 1e-2); + // //m_invDyn->addForceTask(*m_taskCoP, 1e-3, 1, 0); + // } else { + // m_invDyn->updateTaskWeight(m_taskCoP->name(), 0.0); + // // m_invDyn->removeTask(m_taskCoP->name(), 0.0); + // } + 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); + Vector6 const_vel = 0.01 * Vector::Ones(6); m_sampleRH.pos = x_rh_ref; + // m_sampleRH.vel = dx_rh_ref; + // m_sampleRH.acc = ddx_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); @@ -1077,8 +1151,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_taskLH->Kd(kd_hands); } - getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); - getProfiler().start(PROFILE_PREPARE_INV_DYN); if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ @@ -1097,7 +1169,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskComAdm->name(), w_com); } } - m_sampleCom.pos = x_com_ref - m_com_offset; //actFrame(m_transformFrameCom, x_com_ref) - m_com_offset; // + m_sampleCom.pos = actFrame(m_transformFrameCom, x_com_ref) - m_com_offset; // x_com_ref - m_com_offset; // m_sampleCom.vel = dx_com_ref; m_sampleCom.acc = ddx_com_ref; m_taskCom->setReference(m_sampleCom); @@ -1120,16 +1192,30 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskAM->name(), w_am); } - 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 Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); - const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); - const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); - Eigen::Matrix sum_x = x_lf_ref + x_rf_ref; - m_sampleWaist.pos = 0.5 * actFrame(m_transformFrameFeet, sum_x); //x_waist_ref; - m_sampleWaist.vel = 0.5 * (dx_lf_ref + dx_rf_ref); //dx_waist_ref; - m_sampleWaist.acc = 0.5 * (ddx_lf_ref + ddx_rf_ref); //ddx_waist_ref; + // 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 Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); + // const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); + // const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); + // Eigen::Matrix sum_x = x_lf_ref + x_rf_ref; + // m_sampleWaist.pos = 0.5 * actFrame(m_transformFrameFeet, sum_x); //x_waist_ref; + // m_sampleWaist.vel = 0.5 * (dx_lf_ref + dx_rf_ref); //dx_waist_ref; + // m_sampleWaist.acc = 0.5 * (ddx_lf_ref + ddx_rf_ref); //ddx_waist_ref; + // VectorN waist_ref = x_waist_ref; + // Vector6 dwaist_ref = dx_waist_ref; + // Vector6 ddwaist_ref = ddx_waist_ref; + // waist_ref.head<3>() = x_com_ref.head<3>() - m_transformFrameCom.translation() - m_com_offset; + // dwaist_ref.head<3>() = dx_com_ref.head<3>(); + // ddwaist_ref.head<3>() = ddx_com_ref.head<3>(); + // m_sampleWaist.pos = waist_ref; + // m_sampleWaist.vel = dwaist_ref; + // m_sampleWaist.acc = ddx_waist_ref; + // VectorN waist_ref = x_waist_ref; + // waist_ref.head<3>() = x_com_ref.head<3>() - m_transformFrameCom.translation() - 2*m_com_offset; + m_sampleWaist.pos = x_waist_ref; + m_sampleWaist.vel = dx_waist_ref; + m_sampleWaist.acc = ddx_waist_ref; m_taskWaist->setReference(m_sampleWaist); m_taskWaist->Kp(kp_base_orientation); m_taskWaist->Kd(kd_base_orientation); @@ -1194,8 +1280,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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; - const Vector3& com = m_robot->com(m_invDyn->data()); - m_omega = std::sqrt(9.81 / com[2]); // 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)); @@ -1534,7 +1618,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; } @@ -1645,6 +1729,9 @@ 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; } @@ -2000,26 +2087,27 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy, double) { SEND_WARNING_STREAM_MSG("Cannot compute signal energy before initialization!"); return s; } - 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& kp_posture = m_kp_postureSIN(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& kp_posture = m_kp_postureSIN(iter); // assert(K_energy.size() == static_cast(m_robot_util->m_nbJoints)); - const Vector& q_ref_final = m_ref_pos_finalSIN(iter); - assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); + //const Vector& q_ref_final = m_ref_pos_finalSIN(iter); + //assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); // std::cout << "##################### INV DYN ENERGY ################################" << std::endl; // std::cout << "q: " << q_robot << std::endl; // std::cout << "v: " << v_robot << std::endl; - Vector K_energy(m_robot_util->m_nbJoints + 6); + //Vector K_energy(m_robot_util->m_nbJoints + 6); //K_energy = 20.0 * Vector::Ones(m_robot_util->m_nbJoints + 6); - K_energy.head<6>() = 0.0 * Vector::Ones(6); - K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; - m_q_desSOUT(iter); - Vector error_pos = q_robot - q_ref_final; + //K_energy.head<6>() = 0.0 * Vector::Ones(6); + //K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; + //m_q_desSOUT(iter); + //Vector error_pos = q_robot - q_ref_final; // std::cout << "############## mass matrix: " << m_invDyn->data().M.bottomRows(m_robot_util->m_nbJoints) << " ############" << std::endl; - s = 0.5 * v_robot.transpose() * m_robot->mass(m_invDyn->data()) * v_robot; - s += 0.5 * error_pos.transpose() * K_energy.cwiseProduct(error_pos); + //s = 0.5 * v_robot.transpose() * m_robot->mass(m_invDyn->data()) * v_robot; + //s += 0.5 * error_pos.transpose() * K_energy.cwiseProduct(error_pos); + s = m_taskEnergy->get_H(); return s; } @@ -2028,61 +2116,63 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy_derivative, double) { SEND_WARNING_STREAM_MSG("Cannot compute signal energy_derivative before initialization!"); return s; } - double energy = m_energySOUT(iter); + // double energy = m_energySOUT(iter); // std::cout << "############## previous_energy: " << previous_energy << " ############" << std::endl; // std::cout << "############## energy: " << energy << " ############" << std::endl; - s = (energy - m_previous_energy)/m_dt; + // s = (energy - m_previous_energy)/m_dt; // m_q_desSOUT(iter); // double E_c = m_taskEnergy->get_E_c(); // double E_p = m_taskEnergy->get_E_p(); // s = E_c + E_p; - m_previous_energy = energy; + // m_previous_energy = energy; // Vector BK = m_taskEnergy->get_BK_vector(); // double BKv = BK.transpose() * m_previous_vel; // s = BKv/m_dt; + s = m_taskEnergy->get_dH(); return s; } -DEFINE_SIGNAL_OUT_FUNCTION(energy_derivative_time_range, double) { +DEFINE_SIGNAL_OUT_FUNCTION(energy_tank, double) { if (!m_initSucceeded) { - SEND_WARNING_STREAM_MSG("Cannot compute signal energy_derivative_time_range before initialization!"); + SEND_WARNING_STREAM_MSG("Cannot compute signal energy_tank before initialization!"); return s; } - double m_time_preview = 0.003; - 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& previous_vel = m_vSIN(iter-1); - // std::cout << "############## previous_vel: " << previous_vel << " ############" << std::endl; - // assert(previous_vel.size() == static_cast(m_robot_util->m_nbJoints + 6)); - const VectorN& kp_posture = m_kp_postureSIN(iter); - // assert(K_energy.size() == static_cast(m_robot_util->m_nbJoints)); - const Vector& q_ref_final = m_ref_pos_finalSIN(iter); - assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); - - Vector K_energy(m_robot_util->m_nbJoints + 6); - K_energy = 20.0 * Vector::Ones(m_robot_util->m_nbJoints + 6); - // K_energy.head<6>() = 0.0 * Vector::Ones(6); - // K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; - m_q_desSOUT(iter); + //double m_time_preview = 0.003; + //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& previous_vel = m_vSIN(iter-1); + // //std::cout << "############## previous_vel: " << previous_vel << " ############" << std::endl; + // //assert(previous_vel.size() == static_cast(m_robot_util->m_nbJoints + 6)); + //const VectorN& kp_posture = m_kp_postureSIN(iter); + // //assert(K_energy.size() == static_cast(m_robot_util->m_nbJoints)); + //const Vector& q_ref_final = m_ref_pos_finalSIN(iter); + //assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); + + //Vector K_energy(m_robot_util->m_nbJoints + 6); + //K_energy = 20.0 * Vector::Ones(m_robot_util->m_nbJoints + 6); + // //K_energy.head<6>() = 0.0 * Vector::Ones(6); + // //K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; + //m_q_desSOUT(iter); - Vector error_pos = q_robot - q_ref_final; - Vector error_pos_prev = q_ref_final - m_previous_q; - double time_ratio = (m_time_preview * m_time_preview)/(2*m_dt); - Vector v_des_t = error_pos_prev * time_ratio; - Vector a_des_t = ((error_pos_prev/m_dt) - m_previous_vel) * time_ratio; + //Vector error_pos = q_robot - q_ref_final; + //Vector error_pos_prev = q_ref_final - m_previous_q; + //double time_ratio = (m_time_preview * m_time_preview)/(2*m_dt); + //Vector v_des_t = error_pos_prev * time_ratio; + //Vector a_des_t = ((error_pos_prev/m_dt) - m_previous_vel) * time_ratio; - m_previous_q = q_robot; - m_previous_vel = v_robot; + //m_previous_q = q_robot; + //m_previous_vel = v_robot; // E_c // s = v_robot.transpose() * m_robot->mass(m_invDyn->data()) * v_robot; // dE_p //s = (error_pos*m_time_preview + v_des_t).transpose() * K_energy.cwiseProduct(m_previous_vel/m_dt + m_dv_sot); - s = v_robot.transpose() * K_energy.cwiseProduct(error_pos); + // s = v_robot.transpose() * K_energy.cwiseProduct(error_pos); // dE_c - s += ((1/m_dt) * (v_robot*m_time_preview) + a_des_t).transpose() * m_robot->mass(m_invDyn->data()) * m_dv_sot; + //s += ((1/m_dt) * (v_robot*m_time_preview) + a_des_t).transpose() * m_robot->mass(m_invDyn->data()) * m_dv_sot; + s = m_taskEnergy->get_E_tank(); return s; } @@ -2093,10 +2183,10 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy_bound, double) { } 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); + const VectorN& tau_measured = m_tau_measuredSIN(iter); // assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); m_tau_desSOUT(iter); - s = v_robot.tail(m_robot_util->m_nbJoints).transpose() * m_tau_sot; + s = - v_robot.tail(m_robot_util->m_nbJoints).transpose() * tau_measured; return s; } @@ -2108,10 +2198,10 @@ DEFINE_SIGNAL_OUT_FUNCTION(task_energy_const, double) { m_q_desSOUT(iter); const VectorN6& v_robot = m_vSIN(iter); assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); - Vector A = m_taskEnergy->get_A(); - Vector BK = m_taskEnergy->get_BK_vector(); + // Vector A = m_taskEnergy->get_A(); + // Vector BK = m_taskEnergy->get_BK_vector(); double vJF = v_robot.transpose() * m_JF; - double AMdv = (A.transpose() * m_robot->mass(m_invDyn->data()))* m_dv_sot; // + BK.transpose() + double AMdv = (- v_robot.transpose() * m_robot->mass(m_invDyn->data()))* m_dv_sot; // + BK.transpose() s = AMdv + vJF; return s; } @@ -2125,8 +2215,55 @@ DEFINE_SIGNAL_OUT_FUNCTION(task_energy_bound, double) { const VectorN6& v_robot = m_vSIN(iter); assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); // double vJF = v_robot.transpose() * m_JF; - double upB = m_taskEnergy->get_upperBound() + v_robot.transpose() * m_robot->nonLinearEffects(m_invDyn->data()); - s = upB; + double lowB = m_taskEnergy->get_lowerBound() + v_robot.transpose() * m_robot->nonLinearEffects(m_invDyn->data()); + s = lowB; + 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(); + 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; } From e7a8ce946d0c1152e7f1d76708e55ffe80abc0d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Wed, 21 Jul 2021 13:10:34 +0200 Subject: [PATCH 35/47] [ForceTask] First draft --- .../inverse-dynamics-balance-controller.hh | 12 ++- src/inverse-dynamics-balance-controller.cpp | 74 +++++++++++++++++-- 2 files changed, 78 insertions(+), 8 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index e9cba5b..584d19b 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -38,12 +38,15 @@ #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 /* HELPER */ @@ -116,6 +119,8 @@ 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(f_ext_left_arm, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector); DECLARE_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector); @@ -236,6 +241,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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, @@ -277,7 +283,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::contacts::Contact6d* m_contactRF; tsid::contacts::Contact6d* m_contactLF; tsid::contacts::Contact6d* m_contactRH; - tsid::contacts::Contact6d* m_contactLH; + // tsid::contacts::Contact6d* m_contactLH; tsid::tasks::TaskComEquality* m_taskCom; tsid::tasks::TaskComEquality* m_taskComAdm; tsid::tasks::TaskAMEquality* m_taskAM; @@ -289,6 +295,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; tsid::tasks::TaskActuationBounds* m_taskActBounds; + tsid::contacts::ContactPoint* m_contactLH; + tsid::tasks::TaskContactForceEquality* m_taskForceLH; tsid::trajectories::TrajectorySample m_sampleCom; tsid::trajectories::TrajectorySample m_sampleComAdm; @@ -299,6 +307,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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; double m_w_com; double m_w_am; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 8fde0a2..29399cf 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -115,6 +115,8 @@ 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_f_ext_left_armSIN \ << m_kp_base_orientationSIN \ << m_kd_base_orientationSIN \ << m_kp_constraintsSIN \ @@ -258,6 +260,8 @@ 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(f_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), @@ -361,6 +365,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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), @@ -676,6 +681,12 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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 @@ -764,6 +775,16 @@ 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 force ext in hands sensors detected) + m_contactLH = new ContactPoint("contact-lh", *m_robot, m_robot_util->m_hand_util.m_Left_Hand_Frame_Name, + contactNormal, mu, fMin, fMaxRF); + m_contactLH->Kp(kp_contact); + m_contactLH->Kd(kd_contact); + //m_invDyn->addRigidContact(*m_contactLH, w_forces, 10, 1); + m_taskForceLH = new TaskContactForceEquality("task-force-lh", *m_robot, m_contactLH->name()); + m_taskForceLH->Kp(kp_hands); + m_taskForceLH->Kd(kp_hands); + // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); if (m_ctrlMode == CONTROL_OUTPUT_VELOCITY){ @@ -772,13 +793,9 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_sampleAM = TrajectorySample(3); m_sampleWaist = TrajectorySample(6); m_samplePosture = TrajectorySample(m_robot->nv() - 6); - m_sampleRH = TrajectorySample(3); - - 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_sampleLH = TrajectorySample(3); + m_sampleLHForceRef = TrajectorySample(6); + m_sampleLHForceExt = TrajectorySample(6); const VectorN6& q_robot = m_qSIN(0); assert(q_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); @@ -931,6 +948,29 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_leftHandState = TASK_LEFT_HAND_ON; }*/ + if (m_f_ext_left_armSIN.isPlugged()){ + const Vector6& f_ext_left_hand = m_f_ext_left_armSIN(iter); + if ((not m_taskLHContactOn) && (f_ext_left_hand.head(3).norm() > ZERO_FORCE_THRESHOLD)){ + std::cout << "########### ADD TASK CONTACT LEFT HAND ################" << std::endl; + pinocchio::SE3 H_lh = m_robot->position(m_invDyn->data(), + m_robot->model().getJointId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name)); + m_contactLH->setReference(H_lh); + m_invDyn->addRigidContact(*m_contactLH, 1e-3, 10, 1); + m_invDyn->addForceTask(*m_taskForceLH, 100, 1); + m_taskLHContactOn = true; + } else if ((m_taskLHContactOn) && (f_ext_left_hand.head(3).norm() < ZERO_FORCE_THRESHOLD)){ + std::cout << "########### REMOVE TASK CONTACT LEFT HAND ################" << std::endl; + bool res = m_invDyn->removeRigidContact(m_contactLH->name(), 0.0); + 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; + } + } + getProfiler().start(PROFILE_READ_INPUT_SIGNALS); m_w_feetSIN(iter); m_active_joints_checkedSINNER(iter); @@ -1033,6 +1073,26 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_taskLH->Kd(kd_hands); } + if (m_taskLHContactOn) { + const Vector6& f_ref_LH = m_f_ref_left_armSIN(iter); + const Vector6& f_ext_wLH = m_f_ext_left_armSIN(iter); + Vector6 f_ext_LH; + int sensorFrameId = (int)m_robot->model().getFrameId("wrist_left_ft_link"); + pinocchio::SE3 sensorPlacement = (m_invDyn->data()).oMf[sensorFrameId]; + f_ext_LH = sensorPlacement.actInv(pinocchio::Force(f_ext_wLH)).toVector(); + const Vector6& kp_hands = m_kp_handsSIN(iter); + const Vector6& kd_hands = m_kd_handsSIN(iter); + const Vector6& ki = Vector::Ones(6); + m_sampleLHForceRef.pos = f_ref_LH; + m_sampleLHForceExt.pos = f_ext_LH; + + m_taskForceLH->setReference(m_sampleLHForceRef); + m_taskForceLH->setExternalForce(m_sampleLHForceExt); + m_taskForceLH->Kp(kp_hands); + m_taskForceLH->Kd(kd_hands); + m_taskForceLH->Ki(ki); + } + getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); getProfiler().start(PROFILE_PREPARE_INV_DYN); From 4b08c5761c3bf4ece42f9a20e6dc66acedbe6b2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Tue, 27 Jul 2021 10:27:59 +0200 Subject: [PATCH 36/47] =?UTF-8?q?[Energy]=C2=A0Add=20energy=20task=20in=20?= =?UTF-8?q?posture=20simu=20and=20add=20energy=20signals?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../inverse-dynamics-balance-controller.hh | 2 + include/sot/torque_control/posture-task.hh | 15 ++ src/inverse-dynamics-balance-controller.cpp | 43 ++++- src/posture-task.cpp | 172 +++++++++++++++++- 4 files changed, 215 insertions(+), 17 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index f0d67f2..b123084 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -231,6 +231,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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_const, double); DECLARE_SIGNAL_OUT(task_energy_bound, double); @@ -238,6 +239,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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 diff --git a/include/sot/torque_control/posture-task.hh b/include/sot/torque_control/posture-task.hh index 1443cfb..890dd77 100644 --- a/include/sot/torque_control/posture-task.hh +++ b/include/sot/torque_control/posture-task.hh @@ -53,6 +53,7 @@ #include #include #include +#include /* HELPER */ #include @@ -115,6 +116,19 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { 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_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); + /* --- COMMANDS --- */ void init(const double& dt, const std::string& robotRef); @@ -148,6 +162,7 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; tsid::tasks::TaskActuationBounds* m_taskActBounds; + tsid::tasks::TaskEnergy* m_taskEnergy; /// Trajectories of the tasks tsid::trajectories::TrajectorySample m_samplePosture; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index ae79dce..c4449fe 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -218,6 +218,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_energySOUT \ << m_energy_derivativeSOUT \ << m_energy_tankSOUT \ + << m_denergy_tankSOUT \ << m_energy_boundSOUT \ << m_task_energy_constSOUT \ << m_task_energy_boundSOUT \ @@ -225,6 +226,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << 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 @@ -372,6 +374,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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_const, double, m_tau_desSOUT), CONSTRUCT_SIGNAL_OUT(task_energy_bound, double, m_tau_desSOUT), @@ -379,6 +382,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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), @@ -640,7 +644,7 @@ void InverseDynamicsBalanceController::removeTaskLeftHand(const double& transiti } void InverseDynamicsBalanceController::addTaskEnergy(const double& transitionTime) { - m_invDyn->addEnergyTask(*m_taskEnergy, 20, 1, transitionTime); + m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0, transitionTime); } void InverseDynamicsBalanceController::removeTaskEnergy(const double& transitionTime) { @@ -862,7 +866,7 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& K_energy.head<6>() = 0.0 * Vector::Ones(6); K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; m_taskEnergy->K(K_energy); - m_invDyn->addEnergyTask(*m_taskEnergy, 10, 1); + m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0); // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); @@ -1128,11 +1132,10 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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); - Vector6 const_vel = 0.01 * Vector::Ones(6); + // Vector6 const_vel = 0.01 * Vector::Ones(6); m_sampleRH.pos = x_rh_ref; - // m_sampleRH.vel = dx_rh_ref; - // m_sampleRH.acc = ddx_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); @@ -1244,14 +1247,14 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { // q_ref_final_urdf.setZero(39); // m_robot_util->config_sot_to_urdf(q_ref_final, q_ref_final_urdf); - m_sampleEnergy.pos = q_ref_final; + // m_sampleEnergy.pos = q_ref_final; // m_sampleEnergy.vel = m_v_sot; - m_sampleEnergy.acc = m_JF; + // m_sampleEnergy.acc = m_JF; // m_sampleEnergy.acc = Vector::Ones(38); // m_sampleEnergy.acc.head<6>() = 0.0 * Vector::Ones(6); // m_sampleEnergy.acc.tail(m_robot_util->m_nbJoints) = tau_measured - m_tau_sot; // m_robot_util->joints_sot_to_urdf(ddq_ref, m_sampleEnergy.acc); - m_taskEnergy->setReference(m_sampleEnergy); + // m_taskEnergy->setReference(m_sampleEnergy); /*m_sampleRH.pos = x_rh_ref; m_sampleRH.vel = dx_rh_ref; @@ -2176,6 +2179,15 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy_tank, double) { 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!"); @@ -2183,10 +2195,10 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy_bound, double) { } 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); + // const VectorN& tau_measured = m_tau_measuredSIN(iter); // assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); m_tau_desSOUT(iter); - s = - v_robot.tail(m_robot_util->m_nbJoints).transpose() * tau_measured; + s = - v_robot.tail(m_robot_util->m_nbJoints).transpose() * m_tau_sot; return s; } @@ -2254,6 +2266,17 @@ DEFINE_SIGNAL_OUT_FUNCTION(task_energy_S, dynamicgraph::Vector) { 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; } diff --git a/src/posture-task.cpp b/src/posture-task.cpp index 9041ffd..7bdd6ef 100644 --- a/src/posture-task.cpp +++ b/src/posture-task.cpp @@ -93,8 +93,25 @@ using namespace dg::sot; << m_com_measuredSIN \ << m_active_jointsSIN -#define OUTPUT_SIGNALS m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_comSOUT \ - << m_right_foot_posSOUT << m_left_foot_posSOUT +#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_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 /// Define EntityClassName here rather than in the header file @@ -142,6 +159,18 @@ PostureTask(const std::string& name) , 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_bound, double, m_tau_desSOUT) + , 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) , m_t(0.0) , m_initSucceeded(false) , m_enabled(false) @@ -263,6 +292,19 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { 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, q_robot, v_robot, dt, 0.003); + Vector K_energy(m_robot_util->m_nbJoints + 6); + K_energy.head<6>() = 0.0 * Vector::Ones(6); + K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; + m_taskEnergy->K(K_energy); + m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0); + // TRAJECTORY INIT m_samplePosture = TrajectorySample(m_robot->nv() - 6); @@ -271,10 +313,6 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { // 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); @@ -328,7 +366,7 @@ DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector) { 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 */ @@ -524,6 +562,126 @@ DEFINE_SIGNAL_OUT_FUNCTION(right_foot_pos, dynamicgraph::Vector) { 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_bound, double) { + if (!m_initSucceeded) { + SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_bound before initialization!"); + return s; + } + m_q_desSOUT(iter); + const VectorN6& v_robot = m_vSIN(iter); + assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); + // double vJF = v_robot.transpose() * m_JF; + double lowB = m_taskEnergy->get_lowerBound() + v_robot.transpose() * m_robot->nonLinearEffects(m_invDyn->data()); + s = lowB; + 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 ---------------------------------------------------------- */ /* ------------------------------------------------------------------- */ From 7b4893fb4b1f75324ad91cb7d4df2c891c4c05ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Wed, 28 Jul 2021 16:20:51 +0200 Subject: [PATCH 37/47] [ForceTask] Add command to add/remove force task on left hand --- .../inverse-dynamics-balance-controller.hh | 2 + src/inverse-dynamics-balance-controller.cpp | 61 +++++++++++-------- 2 files changed, 37 insertions(+), 26 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 584d19b..57b4f6d 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -89,6 +89,8 @@ 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); /* --- SIGNALS --- */ DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector); diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 29399cf..ec9b992 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -428,6 +428,12 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st addCommand("removeTaskLeftHand", makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeTaskLeftHand, docCommandVoid1("lowers the left hand.", "Transition time in seconds (double)"))); + addCommand("addTaskLeftHandContact", + makeCommandVoid1(*this, &InverseDynamicsBalanceController::addTaskLeftHandContact, + docCommandVoid1("add left hand contact.", "Transition time in seconds (double)"))); + addCommand("removeTaskLeftHandContact", + makeCommandVoid1(*this, &InverseDynamicsBalanceController::removeTaskLeftHandContact, + docCommandVoid1("remove left hand contact.", "Transition time in seconds (double)"))); } Vector InverseDynamicsBalanceController::actFrame(pinocchio::SE3 frame, Vector vec) { @@ -604,6 +610,32 @@ void InverseDynamicsBalanceController::removeTaskLeftHand(const double& transiti } } +void InverseDynamicsBalanceController::addTaskLeftHandContact(const double& transitionTime) { + if (m_f_ext_left_armSIN.isPlugged()){ + std::cout << "########### ADD TASK CONTACT LEFT HAND ################" << std::endl; + pinocchio::SE3 H_lh = m_robot->position(m_invDyn->data(), + m_robot->model().getJointId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name)); + m_contactLH->setReference(H_lh); + m_invDyn->addRigidContact(*m_contactLH, 1e-3, 10, 1); + m_invDyn->addForceTask(*m_taskForceLH, 100, 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) { + std::cout << "########### REMOVE TASK CONTACT LEFT HAND ################" << std::endl; + 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); @@ -776,10 +808,10 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskLH->Kd(kd_hands); // HANDS CONTACTS (not yet added, only when force ext in hands sensors detected) - m_contactLH = new ContactPoint("contact-lh", *m_robot, m_robot_util->m_hand_util.m_Left_Hand_Frame_Name, + m_contactLH = new ContactPoint("contact-lh", *m_robot, "wrist_left_ft_link", contactNormal, mu, fMin, fMaxRF); - m_contactLH->Kp(kp_contact); - m_contactLH->Kd(kd_contact); + m_contactLH->Kp(kp_contact.head(3)); + m_contactLH->Kd(kd_contact.head(3)); //m_invDyn->addRigidContact(*m_contactLH, w_forces, 10, 1); m_taskForceLH = new TaskContactForceEquality("task-force-lh", *m_robot, m_contactLH->name()); m_taskForceLH->Kp(kp_hands); @@ -948,29 +980,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_leftHandState = TASK_LEFT_HAND_ON; }*/ - if (m_f_ext_left_armSIN.isPlugged()){ - const Vector6& f_ext_left_hand = m_f_ext_left_armSIN(iter); - if ((not m_taskLHContactOn) && (f_ext_left_hand.head(3).norm() > ZERO_FORCE_THRESHOLD)){ - std::cout << "########### ADD TASK CONTACT LEFT HAND ################" << std::endl; - pinocchio::SE3 H_lh = m_robot->position(m_invDyn->data(), - m_robot->model().getJointId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name)); - m_contactLH->setReference(H_lh); - m_invDyn->addRigidContact(*m_contactLH, 1e-3, 10, 1); - m_invDyn->addForceTask(*m_taskForceLH, 100, 1); - m_taskLHContactOn = true; - } else if ((m_taskLHContactOn) && (f_ext_left_hand.head(3).norm() < ZERO_FORCE_THRESHOLD)){ - std::cout << "########### REMOVE TASK CONTACT LEFT HAND ################" << std::endl; - bool res = m_invDyn->removeRigidContact(m_contactLH->name(), 0.0); - 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; - } - } - getProfiler().start(PROFILE_READ_INPUT_SIGNALS); m_w_feetSIN(iter); m_active_joints_checkedSINNER(iter); From 3e9d600db3cbd85f7c4d4a281bf95688b6e510a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Wed, 4 Aug 2021 10:21:05 +0200 Subject: [PATCH 38/47] =?UTF-8?q?[ForceTask]=C2=A0Add=20signal=20LHForce?= =?UTF-8?q?=5Fworld=20and=20fix=20frame=20name=20and=20gains?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../inverse-dynamics-balance-controller.hh | 1 + src/inverse-dynamics-balance-controller.cpp | 80 +++++++++++++------ 2 files changed, 58 insertions(+), 23 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 57b4f6d..703d772 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -229,6 +229,7 @@ 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); /// 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); diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index ec9b992..f9025fa 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -216,6 +216,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_right_hand_accSOUT \ << m_right_foot_acc_desSOUT \ << m_left_foot_acc_desSOUT \ + << m_LH_force_worldSOUT /// Define EntityClassName here rather than in the header file /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. @@ -360,6 +361,7 @@ 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_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN), m_t(0.0), m_initSucceeded(false), @@ -614,10 +616,14 @@ void InverseDynamicsBalanceController::addTaskLeftHandContact(const double& tran if (m_f_ext_left_armSIN.isPlugged()){ std::cout << "########### ADD TASK CONTACT LEFT HAND ################" << std::endl; pinocchio::SE3 H_lh = m_robot->position(m_invDyn->data(), - m_robot->model().getJointId(m_robot_util->m_hand_util.m_Left_Hand_Frame_Name)); + m_robot->model().getJointId("arm_left_7_joint")); m_contactLH->setReference(H_lh); + const double & w_hands = m_w_handsSIN.accessCopy(); + std::cout << "########### setReference ok ################" << std::endl; m_invDyn->addRigidContact(*m_contactLH, 1e-3, 10, 1); - m_invDyn->addForceTask(*m_taskForceLH, 100, 1); + std::cout << "########### Add rigid contact ok ################" << std::endl; + m_invDyn->addForceTask(*m_taskForceLH, w_hands, 1); + std::cout << "########### Add force contact ok ################" << std::endl; m_taskLHContactOn = true; } else { SEND_MSG("Error while adding left hand contact. No signal for external force", MSG_TYPE_ERROR); @@ -799,23 +805,36 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_invDyn->addActuationTask(*m_taskActBounds, 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); + // 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); - m_taskLH = new TaskSE3Equality("task-lh", *m_robot, m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); - m_taskLH->Kp(kp_hands); - m_taskLH->Kd(kd_hands); + // m_taskLH = new TaskSE3Equality("task-lh", *m_robot, m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); + // m_taskLH->Kp(kp_hands); + // m_taskLH->Kd(kd_hands); // HANDS CONTACTS (not yet added, only when force ext in hands sensors detected) + // Eigen::Matrix contactPointsHand; + // contactPointsHand << 0.01, 0.01, -0.01, -0.01; + // contactPointsHand << 0.01, 0.01, -0.01, -0.01; + // contactPointsHand << -0.1, -0.1, -0.1, -0.1; + + // m_contactLH = new Contact6d("contact-lh", *m_robot, "arm_left_7_joint", + // contactPointsHand, contactNormalHand, mu, fMin, fMaxRF); // + // m_contactLH->Kp(kp_contact); + // m_contactLH->Kd(kd_contact); + Eigen::Vector3d contactNormalHand = Vector::Zero(3); + contactNormalHand[0] = 1.0; m_contactLH = new ContactPoint("contact-lh", *m_robot, "wrist_left_ft_link", - contactNormal, mu, fMin, fMaxRF); + contactNormalHand, mu, fMin, fMaxRF); // m_contactLH->Kp(kp_contact.head(3)); m_contactLH->Kd(kd_contact.head(3)); //m_invDyn->addRigidContact(*m_contactLH, w_forces, 10, 1); m_taskForceLH = new TaskContactForceEquality("task-force-lh", *m_robot, m_contactLH->name()); m_taskForceLH->Kp(kp_hands); m_taskForceLH->Kd(kp_hands); + const Vector6& ki = Vector::Ones(6); + m_taskForceLH->Ki(ki); // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); @@ -1083,12 +1102,14 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { } if (m_taskLHContactOn) { - const Vector6& f_ref_LH = m_f_ref_left_armSIN(iter); - const Vector6& f_ext_wLH = m_f_ext_left_armSIN(iter); - Vector6 f_ext_LH; + const Vector6& f_ref_wLH = m_f_ref_left_armSIN(iter); + const Vector6& f_ext_LH = m_f_ext_left_armSIN(iter); + Vector6 f_ref_LH; int sensorFrameId = (int)m_robot->model().getFrameId("wrist_left_ft_link"); + pinocchio::framesForwardKinematics(m_robot->model(), m_invDyn->data(), m_q_urdf); pinocchio::SE3 sensorPlacement = (m_invDyn->data()).oMf[sensorFrameId]; - f_ext_LH = sensorPlacement.actInv(pinocchio::Force(f_ext_wLH)).toVector(); + f_ref_LH = sensorPlacement.actInv(pinocchio::Force(f_ref_wLH)).toVector(); + const Vector6& kp_hands = m_kp_handsSIN(iter); const Vector6& kd_hands = m_kd_handsSIN(iter); const Vector6& ki = Vector::Ones(6); @@ -1145,16 +1166,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskAM->name(), w_am); } - 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 Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); - const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); - const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); - Eigen::Matrix sum_x = x_lf_ref + x_rf_ref; - m_sampleWaist.pos = 0.5 * actFrame(m_transformFrameFeet, sum_x); //x_waist_ref; - m_sampleWaist.vel = 0.5 * (dx_lf_ref + dx_rf_ref); //dx_waist_ref; - m_sampleWaist.acc = 0.5 * (ddx_lf_ref + ddx_rf_ref); //ddx_waist_ref; + // 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 Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); + // const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); + // const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); + // Eigen::Matrix sum_x = x_lf_ref + x_rf_ref; + m_sampleWaist.pos = x_waist_ref; //0.5 * actFrame(m_transformFrameFeet, sum_x); + m_sampleWaist.vel = dx_waist_ref; //0.5 * (dx_lf_ref + dx_rf_ref); // + m_sampleWaist.acc = ddx_waist_ref; //0.5 * (ddx_lf_ref + ddx_rf_ref); // m_taskWaist->setReference(m_sampleWaist); m_taskWaist->Kp(kp_base_orientation); m_taskWaist->Kd(kd_base_orientation); @@ -2000,6 +2021,19 @@ 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(); + int sensorFrameId = (int)m_robot->model().getFrameId("wrist_left_ft_link"); + pinocchio::framesForwardKinematics(m_robot->model(), m_invDyn->data(), m_q_urdf); + pinocchio::SE3 sensorPlacement = (m_invDyn->data()).oMf[sensorFrameId]; + s = sensorPlacement.act(pinocchio::Force(f_ext_LH)).toVector(); + return s; +} + /* --- COMMANDS ---------------------------------------------------------- */ /* ------------------------------------------------------------------- */ From 839926ddf6f36dfb8e62c27e3a98a86931954bb6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Wed, 4 Aug 2021 16:04:51 +0200 Subject: [PATCH 39/47] [Posture Task] Add waist task in pb --- include/sot/torque_control/posture-task.hh | 12 +++++- src/posture-task.cpp | 44 ++++++++++++++++++++++ 2 files changed, 55 insertions(+), 1 deletion(-) diff --git a/include/sot/torque_control/posture-task.hh b/include/sot/torque_control/posture-task.hh index 890dd77..9d4fc88 100644 --- a/include/sot/torque_control/posture-task.hh +++ b/include/sot/torque_control/posture-task.hh @@ -102,6 +102,13 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { 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); @@ -162,13 +169,16 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; tsid::tasks::TaskActuationBounds* m_taskActBounds; - tsid::tasks::TaskEnergy* m_taskEnergy; + 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) diff --git a/src/posture-task.cpp b/src/posture-task.cpp index 7bdd6ef..6570ab5 100644 --- a/src/posture-task.cpp +++ b/src/posture-task.cpp @@ -88,6 +88,12 @@ using namespace dg::sot; << 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 \ @@ -147,6 +153,12 @@ PostureTask(const std::string& name) , 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) @@ -241,6 +253,9 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { 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)); @@ -286,6 +301,19 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { 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); @@ -407,6 +435,12 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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 m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos); @@ -420,6 +454,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture); } + m_sampleWaist.pos = x_waist_ref; + m_sampleWaist.vel = dx_waist_ref; + m_sampleWaist.acc = ddx_waist_ref; + m_taskWaist->setReference(m_sampleWaist); + m_taskWaist->Kp(kp_base_orientation); + m_taskWaist->Kd(kd_base_orientation); + if (m_w_base_orientation != w_base_orientation) { + m_w_base_orientation = w_base_orientation; + m_invDyn->updateTaskWeight(m_taskWaist->name(), w_base_orientation); + } if (m_firstTime) { m_firstTime = false; From 4517a629166d2761cbf0672d0e65c1b5e55ba530 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Wed, 4 Aug 2021 17:19:49 +0200 Subject: [PATCH 40/47] [Debug] Add signal waist orientation --- include/sot/torque_control/posture-task.hh | 1 + src/posture-task.cpp | 21 ++++++++++++++++++++- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/include/sot/torque_control/posture-task.hh b/include/sot/torque_control/posture-task.hh index 9d4fc88..c25b087 100644 --- a/include/sot/torque_control/posture-task.hh +++ b/include/sot/torque_control/posture-task.hh @@ -135,6 +135,7 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { 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, dynamicgraph::Vector); /* --- COMMANDS --- */ diff --git a/src/posture-task.cpp b/src/posture-task.cpp index 6570ab5..c3a9675 100644 --- a/src/posture-task.cpp +++ b/src/posture-task.cpp @@ -117,7 +117,8 @@ using namespace dg::sot; << m_task_energy_dSSOUT \ << m_task_energy_ASOUT \ << m_right_foot_posSOUT \ - << m_left_foot_posSOUT + << m_left_foot_posSOUT \ + << m_base_orientationSOUT /// Define EntityClassName here rather than in the header file @@ -183,6 +184,7 @@ PostureTask(const std::string& name) , 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) @@ -726,6 +728,23 @@ DEFINE_SIGNAL_OUT_FUNCTION(task_energy_A, double) { return s; } +DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, dynamicgraph::Vector) { + 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); + return s; +} + + /* --- COMMANDS ---------------------------------------------------------- */ /* ------------------------------------------------------------------- */ From 92afc94433043a3ea6348df88a28e20c2283d5ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Wed, 4 Aug 2021 17:43:01 +0200 Subject: [PATCH 41/47] =?UTF-8?q?[Debug]=C2=A0Norm=20error=20of=20waist=20?= =?UTF-8?q?task?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/sot/torque_control/posture-task.hh | 2 +- src/posture-task.cpp | 14 ++++++++------ 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/include/sot/torque_control/posture-task.hh b/include/sot/torque_control/posture-task.hh index c25b087..0438814 100644 --- a/include/sot/torque_control/posture-task.hh +++ b/include/sot/torque_control/posture-task.hh @@ -135,7 +135,7 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { 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, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(base_orientation, double); /* --- COMMANDS --- */ diff --git a/src/posture-task.cpp b/src/posture-task.cpp index c3a9675..40ae86d 100644 --- a/src/posture-task.cpp +++ b/src/posture-task.cpp @@ -728,7 +728,7 @@ DEFINE_SIGNAL_OUT_FUNCTION(task_energy_A, double) { return s; } -DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, dynamicgraph::Vector) { +DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, double) { if (!m_initSucceeded) { std::ostringstream oss("Cannot compute signal base_orientation before initialization! iter:"); oss << iter; @@ -736,11 +736,13 @@ DEFINE_SIGNAL_OUT_FUNCTION(base_orientation, dynamicgraph::Vector) { 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); + // 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; } From 9eca3af59e686133c0932ed2256176bebf37280d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Fri, 6 Aug 2021 11:32:36 +0200 Subject: [PATCH 42/47] [Posture Task] Add waist task in pb --- include/sot/torque_control/posture-task.hh | 10 +++++ src/posture-task.cpp | 44 ++++++++++++++++++++++ 2 files changed, 54 insertions(+) diff --git a/include/sot/torque_control/posture-task.hh b/include/sot/torque_control/posture-task.hh index 1443cfb..cf2e6a2 100644 --- a/include/sot/torque_control/posture-task.hh +++ b/include/sot/torque_control/posture-task.hh @@ -101,6 +101,13 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { 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); @@ -148,12 +155,15 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; tsid::tasks::TaskActuationBounds* m_taskActBounds; + 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) diff --git a/src/posture-task.cpp b/src/posture-task.cpp index 9041ffd..1946e51 100644 --- a/src/posture-task.cpp +++ b/src/posture-task.cpp @@ -88,6 +88,12 @@ using namespace dg::sot; << 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 \ @@ -130,6 +136,12 @@ PostureTask(const std::string& name) , 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) @@ -212,6 +224,9 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { 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)); @@ -257,6 +272,19 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { 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); @@ -369,6 +397,12 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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 m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos); @@ -382,6 +416,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture); } + m_sampleWaist.pos = x_waist_ref; + m_sampleWaist.vel = dx_waist_ref; + m_sampleWaist.acc = ddx_waist_ref; + m_taskWaist->setReference(m_sampleWaist); + m_taskWaist->Kp(kp_base_orientation); + m_taskWaist->Kd(kd_base_orientation); + if (m_w_base_orientation != w_base_orientation) { + m_w_base_orientation = w_base_orientation; + m_invDyn->updateTaskWeight(m_taskWaist->name(), w_base_orientation); + } if (m_firstTime) { m_firstTime = false; From 66cfef0f6f0e0d94cdf1d9c569d9ee3f6a0a88f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Tue, 10 Aug 2021 17:55:13 +0200 Subject: [PATCH 43/47] [ForceTask] Set force in tool frame - use contact6D --- .../inverse-dynamics-balance-controller.hh | 3 +- src/inverse-dynamics-balance-controller.cpp | 73 ++++++++++--------- 2 files changed, 38 insertions(+), 38 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 703d772..561fc43 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -286,7 +286,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::contacts::Contact6d* m_contactRF; tsid::contacts::Contact6d* m_contactLF; tsid::contacts::Contact6d* m_contactRH; - // tsid::contacts::Contact6d* m_contactLH; + tsid::contacts::Contact6d* m_contactLH; tsid::tasks::TaskComEquality* m_taskCom; tsid::tasks::TaskComEquality* m_taskComAdm; tsid::tasks::TaskAMEquality* m_taskAM; @@ -298,7 +298,6 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle tsid::tasks::TaskJointPosture* m_taskPosture; tsid::tasks::TaskJointPosture* m_taskBlockedJoints; tsid::tasks::TaskActuationBounds* m_taskActBounds; - tsid::contacts::ContactPoint* m_contactLH; tsid::tasks::TaskContactForceEquality* m_taskForceLH; tsid::trajectories::TrajectorySample m_sampleCom; diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index f9025fa..0095084 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -614,16 +614,13 @@ void InverseDynamicsBalanceController::removeTaskLeftHand(const double& transiti void InverseDynamicsBalanceController::addTaskLeftHandContact(const double& transitionTime) { if (m_f_ext_left_armSIN.isPlugged()){ - std::cout << "########### ADD TASK CONTACT LEFT HAND ################" << std::endl; - pinocchio::SE3 H_lh = m_robot->position(m_invDyn->data(), - m_robot->model().getJointId("arm_left_7_joint")); - m_contactLH->setReference(H_lh); + int sensorFrameId = (int)m_robot->model().getFrameId("wrist_left_ft_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(); - std::cout << "########### setReference ok ################" << std::endl; - m_invDyn->addRigidContact(*m_contactLH, 1e-3, 10, 1); - std::cout << "########### Add rigid contact ok ################" << std::endl; + m_invDyn->addRigidContact(*m_contactLH, 1e-6, 10, 1); m_invDyn->addForceTask(*m_taskForceLH, w_hands, 1); - std::cout << "########### Add force contact ok ################" << std::endl; m_taskLHContactOn = true; } else { SEND_MSG("Error while adding left hand contact. No signal for external force", MSG_TYPE_ERROR); @@ -631,7 +628,6 @@ void InverseDynamicsBalanceController::addTaskLeftHandContact(const double& tran } void InverseDynamicsBalanceController::removeTaskLeftHandContact(const double& transitionTime) { - std::cout << "########### REMOVE TASK CONTACT LEFT HAND ################" << std::endl; 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); @@ -814,25 +810,26 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& // m_taskLH->Kd(kd_hands); // HANDS CONTACTS (not yet added, only when force ext in hands sensors detected) - // Eigen::Matrix contactPointsHand; - // contactPointsHand << 0.01, 0.01, -0.01, -0.01; - // contactPointsHand << 0.01, 0.01, -0.01, -0.01; - // contactPointsHand << -0.1, -0.1, -0.1, -0.1; - - // m_contactLH = new Contact6d("contact-lh", *m_robot, "arm_left_7_joint", - // contactPointsHand, contactNormalHand, mu, fMin, fMaxRF); // - // m_contactLH->Kp(kp_contact); - // m_contactLH->Kd(kd_contact); Eigen::Vector3d contactNormalHand = Vector::Zero(3); - contactNormalHand[0] = 1.0; - m_contactLH = new ContactPoint("contact-lh", *m_robot, "wrist_left_ft_link", - contactNormalHand, mu, fMin, fMaxRF); // - m_contactLH->Kp(kp_contact.head(3)); - m_contactLH->Kd(kd_contact.head(3)); + contactNormalHand[2] = 1.0; + 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, "wrist_left_ft_link", + contactPointsHand, contactNormalHand, mu, fMin, fMaxRF); // + m_contactLH->Kp(kp_contact); + m_contactLH->Kd(kd_contact); + + // m_contactLH = new ContactPoint("contact-lh", *m_robot, "wrist_left_ft_link", + // contactNormalHand, mu, fMin, fMaxRF); // + // m_contactLH->Kp(kp_contact.head(3)); + // m_contactLH->Kd(kd_contact.head(3)); //m_invDyn->addRigidContact(*m_contactLH, w_forces, 10, 1); m_taskForceLH = new TaskContactForceEquality("task-force-lh", *m_robot, m_contactLH->name()); m_taskForceLH->Kp(kp_hands); - m_taskForceLH->Kd(kp_hands); + m_taskForceLH->Kd(kd_hands); const Vector6& ki = Vector::Ones(6); m_taskForceLH->Ki(ki); @@ -1102,19 +1099,21 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { } if (m_taskLHContactOn) { - const Vector6& f_ref_wLH = m_f_ref_left_armSIN(iter); + 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_LH; - int sensorFrameId = (int)m_robot->model().getFrameId("wrist_left_ft_link"); - pinocchio::framesForwardKinematics(m_robot->model(), m_invDyn->data(), m_q_urdf); - pinocchio::SE3 sensorPlacement = (m_invDyn->data()).oMf[sensorFrameId]; - f_ref_LH = sensorPlacement.actInv(pinocchio::Force(f_ref_wLH)).toVector(); + Vector6 f_ref_tool_LH, f_ext_tool_LH;; + pinocchio::SE3 toolPlacement = pinocchio::SE3::Identity(); + Vector translation = Vector::Zero(3); + translation[2] = -0.23; + toolPlacement.translation() = translation; + f_ref_tool_LH = f_ref_LH; + f_ext_tool_LH = toolPlacement.act(pinocchio::Force(f_ext_LH)).toVector(); const Vector6& kp_hands = m_kp_handsSIN(iter); const Vector6& kd_hands = m_kd_handsSIN(iter); const Vector6& ki = Vector::Ones(6); - m_sampleLHForceRef.pos = f_ref_LH; - m_sampleLHForceExt.pos = f_ext_LH; + m_sampleLHForceRef.pos = f_ref_tool_LH; + m_sampleLHForceExt.pos = f_ext_tool_LH; m_taskForceLH->setReference(m_sampleLHForceRef); m_taskForceLH->setExternalForce(m_sampleLHForceExt); @@ -2027,10 +2026,12 @@ DEFINE_SIGNAL_OUT_FUNCTION(LH_force_world, dynamicgraph::Vector) { return s; } const Vector6& f_ext_LH = m_f_ext_left_armSIN.accessCopy(); - int sensorFrameId = (int)m_robot->model().getFrameId("wrist_left_ft_link"); - pinocchio::framesForwardKinematics(m_robot->model(), m_invDyn->data(), m_q_urdf); - pinocchio::SE3 sensorPlacement = (m_invDyn->data()).oMf[sensorFrameId]; - s = sensorPlacement.act(pinocchio::Force(f_ext_LH)).toVector(); + 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; } From e6835acabc54816099beb30d12303f0fe2a302bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Fri, 6 Aug 2021 10:07:51 +0200 Subject: [PATCH 44/47] Comment useless signal --- .../inverse-dynamics-balance-controller.hh | 2 +- src/inverse-dynamics-balance-controller.cpp | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 8832782..8806ea1 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -186,7 +186,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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_IN(ref_pos_final, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(M, dynamicgraph::Matrix); diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 91bbda8..b1c9095 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -168,7 +168,7 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_wrench_right_footSIN \ << m_ref_phaseSIN \ << m_active_jointsSIN \ - << m_ref_pos_finalSIN + // << m_ref_pos_finalSIN #define OUTPUT_SIGNALS m_tau_desSOUT \ << m_MSOUT \ @@ -328,7 +328,7 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st CONSTRUCT_SIGNAL_IN(wrench_right_foot, 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_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), @@ -513,12 +513,13 @@ void InverseDynamicsBalanceController::updateComOffset() { } void InverseDynamicsBalanceController::setControlOutputType(const std::string& type) { - for (int i = 0; i < CONTROL_OUTPUT_SIZE; i++) + 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; } @@ -1137,8 +1138,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const Vector3& kd_com = m_kd_comSIN(iter); const Vector3& kp_am = m_kp_amSIN(iter); const Vector3& kd_am = m_kd_amSIN(iter); - const dg::sot::Vector6d& kp_base_orientation = m_kp_base_orientationSIN(0); - const dg::sot::Vector6d& kd_base_orientation = m_kd_base_orientationSIN(0); + const 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)); @@ -1324,8 +1325,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const VectorN& tau_measured = m_tau_measuredSIN(iter); assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); - const Vector& q_ref_final = m_ref_pos_finalSIN(iter); - assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); + // const Vector& q_ref_final = m_ref_pos_finalSIN(iter); + // assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); // Vector q_ref_final_urdf; // q_ref_final_urdf.setZero(39); // m_robot_util->config_sot_to_urdf(q_ref_final, q_ref_final_urdf); From 42fa1e58d6ee4babff0480831e6a4a74b917b4b6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Tue, 14 Sep 2021 14:29:19 +0200 Subject: [PATCH 45/47] =?UTF-8?q?[Energy]=C2=A0Clean=20files?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../inverse-dynamics-balance-controller.hh | 4 +- include/sot/torque_control/posture-task.hh | 1 - src/inverse-dynamics-balance-controller.cpp | 250 ++++-------------- src/posture-task.cpp | 22 +- 4 files changed, 48 insertions(+), 229 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 8806ea1..0a2cd44 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -128,7 +128,9 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); @@ -242,8 +244,6 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle DECLARE_SIGNAL_OUT(energy_tank, double); DECLARE_SIGNAL_OUT(denergy_tank, double); DECLARE_SIGNAL_OUT(energy_bound, double); - DECLARE_SIGNAL_OUT(task_energy_const, double); - DECLARE_SIGNAL_OUT(task_energy_bound, double); DECLARE_SIGNAL_OUT(task_energy_alpha, double); DECLARE_SIGNAL_OUT(task_energy_beta, double); DECLARE_SIGNAL_OUT(task_energy_gamma, double); diff --git a/include/sot/torque_control/posture-task.hh b/include/sot/torque_control/posture-task.hh index 0438814..77fbea7 100644 --- a/include/sot/torque_control/posture-task.hh +++ b/include/sot/torque_control/posture-task.hh @@ -128,7 +128,6 @@ class SOTPOSTURETASK_EXPORT PostureTask : public ::dynamicgraph::Entity { DECLARE_SIGNAL_OUT(energy_tank, double); DECLARE_SIGNAL_OUT(denergy_tank, double); DECLARE_SIGNAL_OUT(energy_bound, double); - DECLARE_SIGNAL_OUT(task_energy_bound, double); DECLARE_SIGNAL_OUT(task_energy_alpha, double); DECLARE_SIGNAL_OUT(task_energy_beta, double); DECLARE_SIGNAL_OUT(task_energy_gamma, double); diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index b1c9095..10e74da 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -116,7 +116,9 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << 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 \ @@ -223,8 +225,6 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << m_energy_tankSOUT \ << m_denergy_tankSOUT \ << m_energy_boundSOUT \ - << m_task_energy_constSOUT \ - << m_task_energy_boundSOUT \ << m_task_energy_alphaSOUT \ << m_task_energy_betaSOUT \ << m_task_energy_gammaSOUT \ @@ -276,7 +276,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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(f_ext_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), @@ -382,8 +384,6 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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_const, double, m_tau_desSOUT), - CONSTRUCT_SIGNAL_OUT(task_energy_bound, double, m_tau_desSOUT), 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), @@ -671,12 +671,12 @@ void InverseDynamicsBalanceController::setEnergyTank(const double& tankValue) { void InverseDynamicsBalanceController::addTaskLeftHandContact(const double& transitionTime) { if (m_f_ext_left_armSIN.isPlugged()){ - int sensorFrameId = (int)m_robot->model().getFrameId("wrist_left_ft_link"); + 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-6, 10, 1); + m_invDyn->addRigidContact(*m_contactLH, 1e-3, 10, 1); m_invDyn->addForceTask(*m_taskForceLH, w_hands, 1); m_taskLHContactOn = true; } else { @@ -790,17 +790,13 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& contactPoints, contactNormal, mu, fMin, fMaxRF); m_contactRF->Kp(kp_contact); m_contactRF->Kd(kd_contact); - Vector forceRef = Vector::Zero(6); - forceRef[2] = 450.0; - m_contactRF->setForceReference(forceRef); - m_invDyn->addRigidContact(*m_contactRF, w_forces, 1e3, 1); + 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_contactLF->setForceReference(forceRef); - m_invDyn->addRigidContact(*m_contactLF, w_forces, 1e3, 1); + 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()); @@ -824,9 +820,6 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskComAdm->setMask(mask_com_adm); m_invDyn->addMotionTask(*m_taskComAdm, m_w_com, 1); } - // Eigen::VectorXd mask_com(3); - // mask_com << 1, 1, 0; - // m_taskCom->setMask(mask_com); m_invDyn->addMotionTask(*m_taskCom, m_w_com, 1); // TASK ANGULAR MOMENTUM @@ -861,18 +854,10 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskPosture = new TaskJointPosture("task-posture", *m_robot); m_taskPosture->Kp(kp_posture); m_taskPosture->Kd(kd_posture); - //Eigen::VectorXd mask_armR(m_robot->nv() - 6); - //mask_armR = Vector::Ones(m_robot->nv() - 6); - //mask_armR[25] = 0.0; - //mask_armR.segment<8>(22) = Vector::Zero(8); - - //m_taskPosture->setMask(mask_armR); - m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1); // ACTUATION BOUNDS TASK - Vector tau_max = 0.8 * m_robot->model().effortLimit.tail(m_robot->nv() - 6); - std::cout << "################# tau_max : " << tau_max << " ############" << std::endl; + 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); @@ -883,10 +868,6 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_taskJointBounds->setVelocityBounds(-v_max, v_max); //m_invDyn->addMotionTask(*m_taskJointBounds, 1.0, 0); - // TASK CoP - // m_taskCoP = new TaskCopEquality("task-cop", *m_robot); - // m_invDyn->addForceTask(*m_taskCoP, 0.0, 1, 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, "gripper_right_base_link"); //m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); m_taskRH->Kp(kp_hands); @@ -908,20 +889,15 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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, "wrist_left_ft_link", - contactPointsHand, contactNormalHand, mu, fMin, fMaxRF); // + 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_contactLH = new ContactPoint("contact-lh", *m_robot, "wrist_left_ft_link", - // contactNormalHand, mu, fMin, fMaxRF); // - // m_contactLH->Kp(kp_contact.head(3)); - // m_contactLH->Kd(kd_contact.head(3)); - //m_invDyn->addRigidContact(*m_contactLH, w_forces, 10, 1); - m_taskForceLH = new TaskContactForceEquality("task-force-lh", *m_robot, m_contactLH->name()); + m_taskForceLH = new TaskContactForceEquality("task-force-lh", *m_robot, dt, m_contactLH->name()); m_taskForceLH->Kp(kp_hands); m_taskForceLH->Kd(kd_hands); - const Vector6& ki = Vector::Ones(6); + const Vector6& ki = 5* Vector::Ones(6); m_taskForceLH->Ki(ki); const VectorN6& q_robot = m_qSIN(0); @@ -930,13 +906,8 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); // ENERGY TASK - m_taskEnergy = new TaskEnergy("task-energy", *m_robot, q_robot, v_robot, dt, 0.003); - Vector K_energy(m_robot_util->m_nbJoints + 6); - //K_energy = 20.0 * Vector::Ones(m_robot_util->m_nbJoints + 6); - K_energy.head<6>() = 0.0 * Vector::Ones(6); - K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; - m_taskEnergy->K(K_energy); - m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0); + m_taskEnergy = new TaskEnergy("task-energy", *m_robot, dt); + // m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0); // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); @@ -955,7 +926,7 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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("gripper_right_base_link");//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); @@ -965,15 +936,11 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& Vector3 com_estim = data.com[0]; m_omega = std::sqrt(9.81 / com_estim[2]); - // int waistFrameId = (int)m_robot->model().getFrameId("root_joint"); - // pinocchio::SE3 se3Waist; - // m_robot->framePosition(data, waistFrameId, se3Waist); - // SEND_MSG("se3Waist: " + toString(se3Waist), MSG_TYPE_ERROR); 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_ERROR); + SEND_MSG("m_transformFrameCom: " + toString(m_transformFrameCom), MSG_TYPE_INFO); } m_transformFrameFeet = pinocchio::SE3::Identity(); @@ -984,16 +951,16 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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_ERROR); + 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_ERROR); + 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_ERROR); + SEND_MSG("Update COM: " + toString(m_com_offset), MSG_TYPE_INFO); } m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog-fast"); @@ -1145,10 +1112,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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); @@ -1186,17 +1149,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_taskLF->Kd(kd_feet); } - // if (m_contactState != DOUBLE_SUPPORT) { - // Vector3 zmpDes = (x_com_ref - m_com_offset) - ddx_com_ref/(m_omega*m_omega); - // zmpDes[2] = 0.0; - // m_taskCoP->setReference(zmpDes); - // m_invDyn->updateTaskWeight(m_taskCoP->name(), 1e-2); - // //m_invDyn->addForceTask(*m_taskCoP, 1e-3, 1, 0); - // } else { - // m_invDyn->updateTaskWeight(m_taskCoP->name(), 0.0); - // // m_invDyn->removeTask(m_taskCoP->name(), 0.0); - // } - 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); @@ -1229,19 +1181,28 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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;; + Vector6 f_ref_tool_LH, f_ext_tool_LH; pinocchio::SE3 toolPlacement = pinocchio::SE3::Identity(); Vector translation = Vector::Zero(3); - translation[2] = -0.23; + 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 Vector6& kp_hands = m_kp_handsSIN(iter); const Vector6& kd_hands = m_kd_handsSIN(iter); - const Vector6& ki = Vector::Ones(6); + const Vector6& ki = 5* Vector::Ones(6); m_sampleLHForceRef.pos = f_ref_tool_LH; m_sampleLHForceExt.pos = 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.vel = df_ext_tool_LH; + m_sampleLHForceRef.vel = df_ref_LH; + } m_taskForceLH->setReference(m_sampleLHForceRef); m_taskForceLH->setExternalForce(m_sampleLHForceExt); @@ -1293,16 +1254,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskAM->name(), w_am); } - // 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 Eigen::Matrix& x_rf_ref = m_rf_ref_posSIN(iter); - // const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter); - // const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter); - // Eigen::Matrix sum_x = x_lf_ref + x_rf_ref; - m_sampleWaist.pos = x_waist_ref; //0.5 * actFrame(m_transformFrameFeet, sum_x); - m_sampleWaist.vel = dx_waist_ref; //0.5 * (dx_lf_ref + dx_rf_ref); // - m_sampleWaist.acc = ddx_waist_ref; //0.5 * (ddx_lf_ref + ddx_rf_ref); // + m_sampleWaist.pos = x_waist_ref; + m_sampleWaist.vel = dx_waist_ref; + m_sampleWaist.acc = ddx_waist_ref; m_taskWaist->setReference(m_sampleWaist); m_taskWaist->Kp(kp_base_orientation); m_taskWaist->Kd(kd_base_orientation); @@ -1311,9 +1265,15 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskWaist->name(), w_base_orientation); } - m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos); - m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel); - m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc); + VectorN q_ref_energy = q_ref; + VectorN dq_ref_energy = dq_ref; + VectorN ddq_ref_energy = ddq_ref; + // if ((m_taskEnergy->get_beta() < 1) && (m_taskEnergy->get_gamma() < 0.3)){ + // q_ref_energy = m_q_urdf.tail(m_robot_util->m_nbJoints); + // } + m_robot_util->joints_sot_to_urdf(q_ref_energy, m_samplePosture.pos); + m_robot_util->joints_sot_to_urdf(dq_ref_energy, m_samplePosture.vel); + m_robot_util->joints_sot_to_urdf(ddq_ref_energy, m_samplePosture.acc); m_taskPosture->setReference(m_samplePosture); m_taskPosture->Kp(kp_posture); m_taskPosture->Kd(kd_posture); @@ -1325,27 +1285,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const VectorN& tau_measured = m_tau_measuredSIN(iter); assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); - // const Vector& q_ref_final = m_ref_pos_finalSIN(iter); - // assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); - // Vector q_ref_final_urdf; - // q_ref_final_urdf.setZero(39); - // m_robot_util->config_sot_to_urdf(q_ref_final, q_ref_final_urdf); - - // m_sampleEnergy.pos = q_ref_final; - // m_sampleEnergy.vel = m_v_sot; - // m_sampleEnergy.acc = m_JF; - // m_sampleEnergy.acc = Vector::Ones(38); - // m_sampleEnergy.acc.head<6>() = 0.0 * Vector::Ones(6); - // m_sampleEnergy.acc.tail(m_robot_util->m_nbJoints) = tau_measured - m_tau_sot; - // m_robot_util->joints_sot_to_urdf(ddq_ref, m_sampleEnergy.acc); - // m_taskEnergy->setReference(m_sampleEnergy); - - /*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); @@ -2189,26 +2128,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy, double) { SEND_WARNING_STREAM_MSG("Cannot compute signal energy before initialization!"); return s; } - //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& kp_posture = m_kp_postureSIN(iter); - // assert(K_energy.size() == static_cast(m_robot_util->m_nbJoints)); - //const Vector& q_ref_final = m_ref_pos_finalSIN(iter); - //assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); - // std::cout << "##################### INV DYN ENERGY ################################" << std::endl; - // std::cout << "q: " << q_robot << std::endl; - // std::cout << "v: " << v_robot << std::endl; - //Vector K_energy(m_robot_util->m_nbJoints + 6); - //K_energy = 20.0 * Vector::Ones(m_robot_util->m_nbJoints + 6); - //K_energy.head<6>() = 0.0 * Vector::Ones(6); - //K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; - //m_q_desSOUT(iter); - //Vector error_pos = q_robot - q_ref_final; - // std::cout << "############## mass matrix: " << m_invDyn->data().M.bottomRows(m_robot_util->m_nbJoints) << " ############" << std::endl; - //s = 0.5 * v_robot.transpose() * m_robot->mass(m_invDyn->data()) * v_robot; - //s += 0.5 * error_pos.transpose() * K_energy.cwiseProduct(error_pos); s = m_taskEnergy->get_H(); return s; } @@ -2218,18 +2137,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy_derivative, double) { SEND_WARNING_STREAM_MSG("Cannot compute signal energy_derivative before initialization!"); return s; } - // double energy = m_energySOUT(iter); - // std::cout << "############## previous_energy: " << previous_energy << " ############" << std::endl; - // std::cout << "############## energy: " << energy << " ############" << std::endl; - // s = (energy - m_previous_energy)/m_dt; - // m_q_desSOUT(iter); - // double E_c = m_taskEnergy->get_E_c(); - // double E_p = m_taskEnergy->get_E_p(); - // s = E_c + E_p; - // m_previous_energy = energy; - // Vector BK = m_taskEnergy->get_BK_vector(); - // double BKv = BK.transpose() * m_previous_vel; - // s = BKv/m_dt; s = m_taskEnergy->get_dH(); return s; } @@ -2239,41 +2146,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy_tank, double) { SEND_WARNING_STREAM_MSG("Cannot compute signal energy_tank before initialization!"); return s; } - //double m_time_preview = 0.003; - //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& previous_vel = m_vSIN(iter-1); - // //std::cout << "############## previous_vel: " << previous_vel << " ############" << std::endl; - // //assert(previous_vel.size() == static_cast(m_robot_util->m_nbJoints + 6)); - //const VectorN& kp_posture = m_kp_postureSIN(iter); - // //assert(K_energy.size() == static_cast(m_robot_util->m_nbJoints)); - //const Vector& q_ref_final = m_ref_pos_finalSIN(iter); - //assert(q_ref_final.size() == static_cast(m_robot_util->m_nbJoints + 6)); - - //Vector K_energy(m_robot_util->m_nbJoints + 6); - //K_energy = 20.0 * Vector::Ones(m_robot_util->m_nbJoints + 6); - // //K_energy.head<6>() = 0.0 * Vector::Ones(6); - // //K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; - //m_q_desSOUT(iter); - - //Vector error_pos = q_robot - q_ref_final; - //Vector error_pos_prev = q_ref_final - m_previous_q; - //double time_ratio = (m_time_preview * m_time_preview)/(2*m_dt); - //Vector v_des_t = error_pos_prev * time_ratio; - //Vector a_des_t = ((error_pos_prev/m_dt) - m_previous_vel) * time_ratio; - - //m_previous_q = q_robot; - //m_previous_vel = v_robot; - - // E_c - // s = v_robot.transpose() * m_robot->mass(m_invDyn->data()) * v_robot; - // dE_p - //s = (error_pos*m_time_preview + v_des_t).transpose() * K_energy.cwiseProduct(m_previous_vel/m_dt + m_dv_sot); - // s = v_robot.transpose() * K_energy.cwiseProduct(error_pos); - // dE_c - //s += ((1/m_dt) * (v_robot*m_time_preview) + a_des_t).transpose() * m_robot->mass(m_invDyn->data()) * m_dv_sot; s = m_taskEnergy->get_E_tank(); return s; } @@ -2294,43 +2166,11 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy_bound, double) { } 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_tau_desSOUT(iter); s = - v_robot.tail(m_robot_util->m_nbJoints).transpose() * m_tau_sot; return s; } -DEFINE_SIGNAL_OUT_FUNCTION(task_energy_const, double) { - if (!m_initSucceeded) { - SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_const before initialization!"); - return s; - } - m_q_desSOUT(iter); - const VectorN6& v_robot = m_vSIN(iter); - assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); - // Vector A = m_taskEnergy->get_A(); - // Vector BK = m_taskEnergy->get_BK_vector(); - double vJF = v_robot.transpose() * m_JF; - double AMdv = (- v_robot.transpose() * m_robot->mass(m_invDyn->data()))* m_dv_sot; // + BK.transpose() - s = AMdv + vJF; - return s; -} - -DEFINE_SIGNAL_OUT_FUNCTION(task_energy_bound, double) { - if (!m_initSucceeded) { - SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_bound before initialization!"); - return s; - } - m_q_desSOUT(iter); - const VectorN6& v_robot = m_vSIN(iter); - assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); - // double vJF = v_robot.transpose() * m_JF; - double lowB = m_taskEnergy->get_lowerBound() + v_robot.transpose() * m_robot->nonLinearEffects(m_invDyn->data()); - s = lowB; - 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!"); diff --git a/src/posture-task.cpp b/src/posture-task.cpp index 40ae86d..d4a24a7 100644 --- a/src/posture-task.cpp +++ b/src/posture-task.cpp @@ -109,7 +109,6 @@ using namespace dg::sot; << m_energy_tankSOUT \ << m_denergy_tankSOUT \ << m_energy_boundSOUT \ - << m_task_energy_boundSOUT \ << m_task_energy_alphaSOUT \ << m_task_energy_betaSOUT \ << m_task_energy_gammaSOUT \ @@ -177,7 +176,6 @@ PostureTask(const std::string& name) , 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_bound, double, m_tau_desSOUT) , 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) @@ -328,11 +326,7 @@ void PostureTask::init(const double& dt, const std::string& robotRef) { assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); // ENERGY TASK - m_taskEnergy = new TaskEnergy("task-energy", *m_robot, q_robot, v_robot, dt, 0.003); - Vector K_energy(m_robot_util->m_nbJoints + 6); - K_energy.head<6>() = 0.0 * Vector::Ones(6); - K_energy.tail(m_robot_util->m_nbJoints) = kp_posture; - m_taskEnergy->K(K_energy); + m_taskEnergy = new TaskEnergy("task-energy", *m_robot, dt); m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0); // TRAJECTORY INIT @@ -656,20 +650,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(energy_bound, double) { return s; } -DEFINE_SIGNAL_OUT_FUNCTION(task_energy_bound, double) { - if (!m_initSucceeded) { - SEND_WARNING_STREAM_MSG("Cannot compute signal task_energy_bound before initialization!"); - return s; - } - m_q_desSOUT(iter); - const VectorN6& v_robot = m_vSIN(iter); - assert(v_robot.size() == static_cast(m_robot_util->m_nbJoints + 6)); - // double vJF = v_robot.transpose() * m_JF; - double lowB = m_taskEnergy->get_lowerBound() + v_robot.transpose() * m_robot->nonLinearEffects(m_invDyn->data()); - s = lowB; - 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!"); From 1f1aaba78e5346df0b9b75f5c5860144964dd389 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Thu, 20 Jan 2022 16:58:42 +0100 Subject: [PATCH 46/47] Refactor TrajectorySample::pos, vel, acc as in TSID - Clean energy & force tasks --- .../inverse-dynamics-balance-controller.hh | 8 +- src/device-torque-ctrl.cpp | 8 +- src/inverse-dynamics-balance-controller.cpp | 180 +++++++++--------- src/posture-task.cpp | 19 +- src/simple-inverse-dyn.cpp | 25 ++- 5 files changed, 135 insertions(+), 105 deletions(-) diff --git a/include/sot/torque_control/inverse-dynamics-balance-controller.hh b/include/sot/torque_control/inverse-dynamics-balance-controller.hh index 0a2cd44..101aeff 100755 --- a/include/sot/torque_control/inverse-dynamics-balance-controller.hh +++ b/include/sot/torque_control/inverse-dynamics-balance-controller.hh @@ -92,7 +92,7 @@ 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 addTaskLeftHandContact(/*const double& transitionTime*/); void removeTaskLeftHandContact(const double& transitionTime); void addTaskEnergy(const double& transitionTime); void removeTaskEnergy(const double& transitionTime); @@ -143,7 +143,10 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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); @@ -384,6 +387,7 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle 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/src/device-torque-ctrl.cpp b/src/device-torque-ctrl.cpp index 4b7bd8d..eaf5dc2 100644 --- a/src/device-torque-ctrl.cpp +++ b/src/device-torque-ctrl.cpp @@ -186,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/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 10e74da..9a3a37c 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -131,6 +131,9 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17; << 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 \ @@ -291,6 +294,9 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st 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), @@ -401,7 +407,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st m_leftHandState(TASK_LEFT_HAND_OFF), m_timeLast(0), m_robot_util(RefVoidRobotUtil()), - m_ctrlMode(CONTROL_OUTPUT_TORQUE) { + m_ctrlMode(CONTROL_OUTPUT_TORQUE), + m_energyTask_enabled(false) { RESETDEBUG5(); Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); @@ -469,8 +476,8 @@ InverseDynamicsBalanceController::InverseDynamicsBalanceController(const std::st docCommandVoid1("Set the value of the energyTank", "Value of the tank in Joule (double)"))); addCommand("addTaskLeftHandContact", - makeCommandVoid1(*this, &InverseDynamicsBalanceController::addTaskLeftHandContact, - docCommandVoid1("add left hand contact.", "Transition time in seconds (double)"))); + 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)"))); @@ -599,16 +606,11 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit if (m_contactState == LEFT_SUPPORT) { SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); - // TrajectorySample traj_ref = m_taskRF->getReference(); pinocchio::SE3 ref; - // vectorToSE3(traj_ref.pos, ref); ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); m_contactRF->setReference(ref); - Vector forceRef = Vector::Zero(6); - forceRef[2] = 450.0; - m_contactRF->setForceReference(forceRef); m_invDyn->removeTask(m_taskRF->name(), transitionTime); - bool res = m_invDyn->addRigidContact(*m_contactRF, w_forces, 1e3, 1); + 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), @@ -622,16 +624,11 @@ void InverseDynamicsBalanceController::addLeftFootContact(const double& transiti if (m_contactState == RIGHT_SUPPORT) { SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); const double& w_forces = m_w_forcesSIN.accessCopy(); - // TrajectorySample traj_ref = m_taskLF->getReference(); pinocchio::SE3 ref; - // vectorToSE3(traj_ref.pos, ref); ref = m_robot->position(m_invDyn->data(), m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); m_contactLF->setReference(ref); m_invDyn->removeTask(m_taskLF->name(), transitionTime); - Vector forceRef = Vector::Zero(6); - forceRef[2] = 450.0; - m_contactLF->setForceReference(forceRef); - bool res = m_invDyn->addRigidContact(*m_contactLF, w_forces, 1e3, 1); + 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), @@ -658,18 +655,29 @@ 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) { - m_taskEnergy->set_E_tank(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) { +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); @@ -718,6 +726,9 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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); @@ -866,48 +877,49 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& 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); - //m_invDyn->addMotionTask(*m_taskJointBounds, 1.0, 0); + // 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, "gripper_right_base_link"); //m_robot_util->m_hand_util.m_Right_Hand_Frame_Name); + 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); - Eigen::VectorXd mask_hands = Vector::Ones(6); - mask_hands.tail(3) = Vector::Zero(3); - m_taskRH->setMask(mask_hands); m_taskLH = new TaskSE3Equality("task-lh", *m_robot, m_robot_util->m_hand_util.m_Left_Hand_Frame_Name); m_taskLH->Kp(kp_hands); m_taskLH->Kd(kd_hands); - m_taskLH->setMask(mask_hands); - // HANDS CONTACTS (not yet added, only when force ext in hands sensors detected) + // 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; - Eigen::Matrix contactPointsHand; + // 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); // + 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->name()); - m_taskForceLH->Kp(kp_hands); - m_taskForceLH->Kd(kd_hands); - const Vector6& ki = 5* Vector::Ones(6); - m_taskForceLH->Ki(ki); - + 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 + // ENERGY TASK, Added in the QP only if command addTaskEnergy() is called m_taskEnergy = new TaskEnergy("task-energy", *m_robot, dt); - // m_invDyn->addEnergyTask(*m_taskEnergy, 1, 0); // TRAJECTORIES INIT m_sampleCom = TrajectorySample(3); @@ -921,7 +933,6 @@ void InverseDynamicsBalanceController::init(const double& dt, const std::string& m_sampleLHForceExt = TrajectorySample(6); m_sampleRH = TrajectorySample(6); m_sampleLH = TrajectorySample(6); - m_sampleEnergy = TrajectorySample(m_robot->nv()); 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); @@ -1128,9 +1139,9 @@ 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 = actFrame(m_transformFrameFeet, 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); @@ -1141,9 +1152,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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 = actFrame(m_transformFrameFeet, 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); @@ -1156,10 +1167,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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); - // Vector6 const_vel = 0.01 * Vector::Ones(6); - 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); @@ -1170,9 +1180,9 @@ 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); @@ -1189,26 +1199,26 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { f_ref_tool_LH = f_ref_LH; f_ext_tool_LH = toolPlacement.act(pinocchio::Force(f_ext_LH)).toVector(); - const Vector6& kp_hands = m_kp_handsSIN(iter); - const Vector6& kd_hands = m_kd_handsSIN(iter); - const Vector6& ki = 5* Vector::Ones(6); - m_sampleLHForceRef.pos = f_ref_tool_LH; - m_sampleLHForceExt.pos = f_ext_tool_LH; + 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.vel = df_ext_tool_LH; - m_sampleLHForceRef.vel = df_ref_LH; + 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); - m_taskForceLH->Kd(kd_hands); - m_taskForceLH->Ki(ki); + m_taskForceLH->Kp(kp_hands_force); + m_taskForceLH->Kd(kd_hands_force); + m_taskForceLH->Ki(ki_hands_force); } getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); @@ -1219,9 +1229,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { 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.pos = x_com_adm_ref; - m_sampleComAdm.vel = dx_com_adm_ref; - m_sampleCom.acc = ddx_com_adm_ref; + 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); @@ -1231,9 +1241,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskComAdm->name(), w_com); } } - m_sampleCom.pos = actFrame(m_transformFrameCom, x_com_ref) - m_com_offset; // x_com_ref - m_com_offset; // - m_sampleCom.vel = dx_com_ref; - m_sampleCom.acc = ddx_com_ref; + 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); @@ -1243,8 +1253,8 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskCom->name(), w_com); } - m_sampleAM.vel = L_am_ref; - m_sampleAM.acc = dL_am_ref; + m_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); @@ -1254,9 +1264,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskAM->name(), w_am); } - m_sampleWaist.pos = x_waist_ref; - m_sampleWaist.vel = dx_waist_ref; - m_sampleWaist.acc = ddx_waist_ref; + m_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); @@ -1265,15 +1275,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskWaist->name(), w_base_orientation); } - VectorN q_ref_energy = q_ref; - VectorN dq_ref_energy = dq_ref; - VectorN ddq_ref_energy = ddq_ref; - // if ((m_taskEnergy->get_beta() < 1) && (m_taskEnergy->get_gamma() < 0.3)){ - // q_ref_energy = m_q_urdf.tail(m_robot_util->m_nbJoints); - // } - m_robot_util->joints_sot_to_urdf(q_ref_energy, m_samplePosture.pos); - m_robot_util->joints_sot_to_urdf(dq_ref_energy, m_samplePosture.vel); - m_robot_util->joints_sot_to_urdf(ddq_ref_energy, 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); @@ -1283,9 +1294,6 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture); } - const VectorN& tau_measured = m_tau_measuredSIN(iter); - assert(tau_measured.size() == static_cast(m_robot_util->m_nbJoints)); - 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); @@ -1461,11 +1469,11 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_pd_des, dynamicgraph::Vector) { 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)); + 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; } diff --git a/src/posture-task.cpp b/src/posture-task.cpp index d4a24a7..d44a460 100644 --- a/src/posture-task.cpp +++ b/src/posture-task.cpp @@ -439,9 +439,16 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { const double & w_base_orientation = m_w_base_orientationSIN(iter); // Update tasks - 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); @@ -450,9 +457,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture); } - 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_base_orientation); m_taskWaist->Kd(kd_base_orientation); diff --git a/src/simple-inverse-dyn.cpp b/src/simple-inverse-dyn.cpp index 0878f67..87e7158 100644 --- a/src/simple-inverse-dyn.cpp +++ b/src/simple-inverse-dyn.cpp @@ -466,9 +466,9 @@ DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { getProfiler().start(PROFILE_PREPARE_INV_DYN); // Update tasks - m_sampleCom.pos = x_com_ref - m_com_offset; - m_sampleCom.vel = dx_com_ref; - m_sampleCom.acc = ddx_com_ref; + 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); @@ -477,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); @@ -488,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); From dc1be02a6bd914a1389b0f3f11554b0dfe599dfe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Thu, 20 Jan 2022 18:00:16 +0100 Subject: [PATCH 47/47] Fix send message --- src/inverse-dynamics-balance-controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 9a3a37c..093302f 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -532,7 +532,7 @@ void InverseDynamicsBalanceController::setControlOutputType(const std::string& t void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); + SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); bool res = m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -552,7 +552,7 @@ void InverseDynamicsBalanceController::removeRightFootContact(const double& tran void InverseDynamicsBalanceController::removeLeftFootContact(const double& transitionTime) { if (m_contactState == DOUBLE_SUPPORT) { - SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); + SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); bool res = m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime); if (!res) { const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); @@ -604,7 +604,7 @@ void InverseDynamicsBalanceController::addTaskLeftHand(/*const double& transitio void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime) { if (m_contactState == LEFT_SUPPORT) { - SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); + SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); const double& w_forces = m_w_forcesSIN.accessCopy(); 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)); @@ -622,7 +622,7 @@ void InverseDynamicsBalanceController::addRightFootContact(const double& transit void InverseDynamicsBalanceController::addLeftFootContact(const double& transitionTime) { if (m_contactState == RIGHT_SUPPORT) { - SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR); + SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO); const double& w_forces = m_w_forcesSIN.accessCopy(); 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));