From 27bac456f165462dc10b52b51f91dbab65341deb Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 6 Apr 2020 16:33:59 +0200 Subject: [PATCH 01/20] [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/20] [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/20] [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/20] =?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/20] [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/20] [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/20] [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/20] [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 f83a15d64769dbec5a6bc341969d3c393ee720c9 Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Thu, 23 Jul 2020 19:12:44 +0200 Subject: [PATCH 09/20] fix namespace issues after https://github.com/stack-of-tasks/sot-core/pull/148 --- include/sot/torque_control/commands-helper.hh | 1 + tests/test-control-manager.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/include/sot/torque_control/commands-helper.hh b/include/sot/torque_control/commands-helper.hh index 96d56ae..1fbb2a7 100644 --- a/include/sot/torque_control/commands-helper.hh +++ b/include/sot/torque_control/commands-helper.hh @@ -17,6 +17,7 @@ /* --- HELPER ---------------------------------------------------------- */ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; namespace torquecontrol { using ::dynamicgraph::command::docCommandVerbose; using ::dynamicgraph::command::docCommandVoid0; diff --git a/tests/test-control-manager.cpp b/tests/test-control-manager.cpp index fd7ea57..acae3cf 100644 --- a/tests/test-control-manager.cpp +++ b/tests/test-control-manager.cpp @@ -40,5 +40,5 @@ BOOST_AUTO_TEST_CASE(testControlManager) { dynamicgraph::Vector av; a_control_manager.m_uSOUT.needUpdate(6); - RealTimeLogger::destroy(); + dynamicgraph::RealTimeLogger::destroy(); } From de4ac017d59cd56f398549671b73385d76521c5a Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Thu, 23 Jul 2020 19:13:22 +0200 Subject: [PATCH 10/20] sync submodule --- cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake b/cmake index 321eb1c..fb4c22c 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e +Subproject commit fb4c22c319ec5320f9a85527eb1a4130954846f5 From a6639c46caa40ce081de7e643ad6691152eed333 Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Thu, 23 Jul 2020 19:13:45 +0200 Subject: [PATCH 11/20] v1.5.3 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index cf0ddf6..817903f 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ sot-torque-control - 1.5.2 + 1.5.3 Collection of dynamic-graph entities aimed at implementing torque control on different robots. From da0fd7d6186d26796af3d90962641a58d9abead4 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 6 Apr 2020 16:33:59 +0200 Subject: [PATCH 12/20] [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 13/20] [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 14/20] [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 15/20] =?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 16/20] [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 17/20] [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 18/20] [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 19/20] [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 d7edbda933faf2cf7a7c5ef6ae62dea95dc2416b Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Fri, 31 Jul 2020 12:07:03 +0200 Subject: [PATCH 20/20] =?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 6d7e4b5..7f389f2 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -427,7 +427,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); @@ -447,7 +447,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); @@ -499,7 +499,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; @@ -519,7 +519,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; @@ -720,7 +720,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); @@ -728,7 +727,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");