144 const std::string& name)
161 CONSTRUCT_SIGNAL_IN(posture_ref_pos,
dynamicgraph::Vector),
162 CONSTRUCT_SIGNAL_IN(posture_ref_vel,
dynamicgraph::Vector),
163 CONSTRUCT_SIGNAL_IN(posture_ref_acc,
dynamicgraph::Vector),
164 CONSTRUCT_SIGNAL_IN(base_orientation_ref_pos,
dynamicgraph::Vector),
165 CONSTRUCT_SIGNAL_IN(base_orientation_ref_vel,
dynamicgraph::Vector),
166 CONSTRUCT_SIGNAL_IN(base_orientation_ref_acc,
dynamicgraph::Vector),
167 CONSTRUCT_SIGNAL_IN(f_ref_right_foot,
dynamicgraph::Vector),
168 CONSTRUCT_SIGNAL_IN(f_ref_left_foot,
dynamicgraph::Vector),
169 CONSTRUCT_SIGNAL_IN(kp_base_orientation,
dynamicgraph::Vector),
170 CONSTRUCT_SIGNAL_IN(kd_base_orientation,
dynamicgraph::Vector),
171 CONSTRUCT_SIGNAL_IN(kp_constraints,
dynamicgraph::Vector),
172 CONSTRUCT_SIGNAL_IN(kd_constraints,
dynamicgraph::Vector),
183 CONSTRUCT_SIGNAL_IN(w_com, double),
184 CONSTRUCT_SIGNAL_IN(w_feet, double),
185 CONSTRUCT_SIGNAL_IN(w_hands, double),
186 CONSTRUCT_SIGNAL_IN(w_posture, double),
187 CONSTRUCT_SIGNAL_IN(w_base_orientation, double),
188 CONSTRUCT_SIGNAL_IN(w_torques, double),
189 CONSTRUCT_SIGNAL_IN(w_forces, double),
190 CONSTRUCT_SIGNAL_IN(weight_contact_forces,
dynamicgraph::Vector),
191 CONSTRUCT_SIGNAL_IN(mu, double),
192 CONSTRUCT_SIGNAL_IN(contact_points,
dynamicgraph::Matrix),
193 CONSTRUCT_SIGNAL_IN(contact_normal,
dynamicgraph::Vector),
194 CONSTRUCT_SIGNAL_IN(f_min, double),
195 CONSTRUCT_SIGNAL_IN(f_max_right_foot, double),
196 CONSTRUCT_SIGNAL_IN(f_max_left_foot, double),
197 CONSTRUCT_SIGNAL_IN(rotor_inertias,
dynamicgraph::Vector),
204 CONSTRUCT_SIGNAL_IN(dt_joint_pos_limits, double),
205 CONSTRUCT_SIGNAL_IN(tau_estimated,
dynamicgraph::Vector),
209 CONSTRUCT_SIGNAL_IN(wrench_left_foot,
dynamicgraph::Vector),
210 CONSTRUCT_SIGNAL_IN(wrench_right_foot,
dynamicgraph::Vector),
211 CONSTRUCT_SIGNAL_IN(active_joints,
dynamicgraph::Vector),
213 CONSTRUCT_SIGNAL_OUT(M,
dg::Matrix, m_tau_desSOUT),
214 CONSTRUCT_SIGNAL_OUT(dv_des,
dg::Vector, m_tau_desSOUT),
215 CONSTRUCT_SIGNAL_OUT(f_des_right_foot,
dynamicgraph::Vector,
217 CONSTRUCT_SIGNAL_OUT(f_des_left_foot,
dynamicgraph::Vector,
219 CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot,
dynamicgraph::Vector,
220 m_f_des_right_footSOUT),
221 CONSTRUCT_SIGNAL_OUT(zmp_des_left_foot,
dynamicgraph::Vector,
222 m_f_des_left_footSOUT),
223 CONSTRUCT_SIGNAL_OUT(zmp_des_right_foot_local,
dynamicgraph::Vector,
224 m_f_des_right_footSOUT),
225 CONSTRUCT_SIGNAL_OUT(zmp_des_left_foot_local,
dynamicgraph::Vector,
226 m_f_des_left_footSOUT),
228 m_zmp_des_left_footSOUT << m_zmp_des_right_footSOUT),
230 m_f_ref_left_footSIN << m_f_ref_right_footSIN),
231 CONSTRUCT_SIGNAL_OUT(zmp_right_foot,
dg::Vector, m_wrench_right_footSIN),
232 CONSTRUCT_SIGNAL_OUT(zmp_left_foot,
dg::Vector, m_wrench_left_footSIN),
233 CONSTRUCT_SIGNAL_OUT(zmp,
dg::Vector,
234 m_wrench_left_footSIN << m_wrench_right_footSIN
235 << m_zmp_left_footSOUT
236 << m_zmp_right_footSOUT),
237 CONSTRUCT_SIGNAL_OUT(com,
dg::Vector, m_tau_desSOUT),
238 CONSTRUCT_SIGNAL_OUT(com_vel,
dg::Vector, m_tau_desSOUT),
239 CONSTRUCT_SIGNAL_OUT(com_acc,
dg::Vector, m_tau_desSOUT),
240 CONSTRUCT_SIGNAL_OUT(com_acc_des,
dg::Vector, m_tau_desSOUT),
241 CONSTRUCT_SIGNAL_OUT(base_orientation,
dg::Vector, m_tau_desSOUT),
242 CONSTRUCT_SIGNAL_OUT(right_foot_pos,
dg::Vector, m_tau_desSOUT),
243 CONSTRUCT_SIGNAL_OUT(left_foot_pos,
dg::Vector, m_tau_desSOUT),
244 CONSTRUCT_SIGNAL_OUT(right_hand_pos,
dg::Vector, m_tau_desSOUT),
245 CONSTRUCT_SIGNAL_OUT(left_hand_pos,
dg::Vector, m_tau_desSOUT),
246 CONSTRUCT_SIGNAL_OUT(right_foot_vel,
dg::Vector, m_tau_desSOUT),
247 CONSTRUCT_SIGNAL_OUT(left_foot_vel,
dg::Vector, m_tau_desSOUT),
248 CONSTRUCT_SIGNAL_OUT(right_hand_vel,
dg::Vector, m_tau_desSOUT),
249 CONSTRUCT_SIGNAL_OUT(left_hand_vel,
dg::Vector, m_tau_desSOUT),
250 CONSTRUCT_SIGNAL_OUT(right_foot_acc,
dg::Vector, m_tau_desSOUT),
251 CONSTRUCT_SIGNAL_OUT(left_foot_acc,
dg::Vector, m_tau_desSOUT),
252 CONSTRUCT_SIGNAL_OUT(right_hand_acc,
dg::Vector, m_tau_desSOUT),
253 CONSTRUCT_SIGNAL_OUT(left_hand_acc,
dg::Vector, m_tau_desSOUT),
254 CONSTRUCT_SIGNAL_OUT(right_foot_acc_des,
dg::Vector, m_tau_desSOUT),
255 CONSTRUCT_SIGNAL_OUT(left_foot_acc_des,
dg::Vector, m_tau_desSOUT),
256 CONSTRUCT_SIGNAL_INNER(active_joints_checked,
dg::Vector,
259 m_initSucceeded(false),
262 m_contactState(DOUBLE_SUPPORT),
263 m_rightHandState(TASK_RIGHT_HAND_OFF),
264 m_leftHandState(TASK_LEFT_HAND_OFF),
266 m_robot_util(RefVoidRobotUtil()) {
285 docCommandVoid2(
"Initialize the entity.",
286 "Time period in seconds (double)",
287 "Robot reference (string)")));
294 "Update the offset on the CoM based on the CoP measurement.")));
297 "removeRightFootContact",
300 docCommandVoid1(
"Remove the contact at the right foot.",
301 "Transition time in seconds (double)")));
304 "removeLeftFootContact",
305 makeCommandVoid1(*
this,
307 docCommandVoid1(
"Remove the contact at the left foot.",
308 "Transition time in seconds (double)")));
309 addCommand(
"addTaskRightHand",
312 docCommandVoid0(
"Adds an SE3 task for the right hand.")));
313 addCommand(
"removeTaskRightHand",
316 docCommandVoid1(
"Removes the SE3 task for the right hand.",
317 "Transition time in seconds (double)")));
318 addCommand(
"addTaskLeftHand",
321 docCommandVoid0(
"Raises the left hand.")));
322 addCommand(
"removeTaskLeftHand",
325 docCommandVoid1(
"lowers the left hand.",
326 "Transition time in seconds (double)")));
466 const std::string& robotRef) {
468 return SEND_MSG(
"Init failed: Timestep must be positive", MSG_TYPE_ERROR);
471 std::string localName(robotRef);
472 if (isNameInRobotUtil(localName))
475 SEND_MSG(
"You should have an entity controller manager initialized before",
480 const Eigen::Matrix<double, 3, 4>& contactPoints = m_contact_pointsSIN(0);
481 const Eigen::Vector3d& contactNormal = m_contact_normalSIN(0);
483 const dg::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0);
484 const dg::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0);
485 const Eigen::Vector3d& kp_com = m_kp_comSIN(0);
486 const Eigen::Vector3d& kd_com = m_kd_comSIN(0);
487 const dg::sot::Vector6d& kd_hands = m_kd_handsSIN(0);
488 const dg::sot::Vector6d& kp_hands = m_kp_handsSIN(0);
489 const dg::sot::Vector6d& kp_feet = m_kp_feetSIN(0);
490 const dg::sot::Vector6d& kd_feet = m_kd_feetSIN(0);
491 const VectorN& kp_posture = m_kp_postureSIN(0);
492 const VectorN& kd_posture = m_kd_postureSIN(0);
493 const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0);
494 const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0);
496 assert(kp_posture.size() ==
497 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
498 assert(kd_posture.size() ==
499 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
500 assert(rotor_inertias_sot.size() ==
501 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
502 assert(gear_ratios_sot.size() ==
503 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
508 const double& w_forces = m_w_forcesSIN(0);
511 const double& mu = m_muSIN(0);
512 const double& fMin = m_f_minSIN(0);
513 const double& fMaxRF = m_f_max_right_footSIN(0);
514 const double& fMaxLF = m_f_max_left_footSIN(0);
517 vector<string> package_dirs;
519 new robots::RobotWrapper(
m_robot_util->m_urdf_filename, package_dirs,
520 pinocchio::JointModelFreeFlyer());
525 Vector rotor_inertias_urdf(rotor_inertias_sot.size());
526 Vector gear_ratios_urdf(gear_ratios_sot.size());
527 m_robot_util->joints_sot_to_urdf(rotor_inertias_sot, rotor_inertias_urdf);
528 m_robot_util->joints_sot_to_urdf(gear_ratios_sot, gear_ratios_urdf);
529 m_robot->rotor_inertias(rotor_inertias_urdf);
530 m_robot->gear_ratios(gear_ratios_urdf);
543 new Contact6d(
"contact_rfoot", *
m_robot,
545 contactPoints, contactNormal, mu, fMin, fMaxRF);
551 new Contact6d(
"contact_lfoot", *
m_robot,
553 contactPoints, contactNormal, mu, fMin, fMaxLF);
558 if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) {
559 m_contactLF->setRegularizationTaskWeightVector(Vector6::Ones());
560 m_contactRF->setRegularizationTaskWeightVector(Vector6::Ones());
607 m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
611 SOLVER_HQP_EIQUADPROG_RT,
"eiquadprog_rt_60_36_34");
613 SOLVER_HQP_EIQUADPROG_RT,
"eiquadprog_rt_48_30_17");
614 }
catch (
const std::exception& e) {
615 std::cout << e.what();
617 "Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename,
669 if (!m_initSucceeded) {
670 SEND_WARNING_STREAM_MSG(
671 "Cannot compute signal tau_des before initialization!");
674 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints))
675 s.resize(m_robot_util->m_nbJoints);
680 if (m_f_ref_left_footSIN.isPlugged() && m_f_ref_right_footSIN.isPlugged()) {
681 const Vector6& f_ref_left_foot = m_f_ref_left_footSIN(iter);
682 const Vector6& f_ref_right_foot = m_f_ref_right_footSIN(iter);
683 m_contactLF->setForceReference(f_ref_left_foot);
684 m_contactRF->setForceReference(f_ref_right_foot);
686 if (m_contactState == DOUBLE_SUPPORT) {
688 removeLeftFootContact(0.0);
690 removeRightFootContact(0.0);
692 }
else if (m_contactState == LEFT_SUPPORT &&
694 addRightFootContact(0.0);
695 }
else if (m_contactState == RIGHT_SUPPORT &&
697 addLeftFootContact(0.0);
701 if (m_contactState == RIGHT_SUPPORT_TRANSITION &&
702 m_t >= m_contactTransitionTime) {
703 m_contactState = RIGHT_SUPPORT;
704 }
else if (m_contactState == LEFT_SUPPORT_TRANSITION &&
705 m_t >= m_contactTransitionTime) {
706 m_contactState = LEFT_SUPPORT;
721 m_active_joints_checkedSINNER(iter);
722 const VectorN6& q_sot = m_qSIN(iter);
723 assert(q_sot.size() ==
724 static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6));
725 const VectorN6& v_sot = m_vSIN(iter);
726 assert(v_sot.size() ==
727 static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6));
728 const Vector3& x_com_ref = m_com_ref_posSIN(iter);
729 const Vector3& dx_com_ref = m_com_ref_velSIN(iter);
730 const Vector3& ddx_com_ref = m_com_ref_accSIN(iter);
731 const VectorN& q_ref = m_posture_ref_posSIN(iter);
732 assert(q_ref.size() ==
733 static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
734 const VectorN& dq_ref = m_posture_ref_velSIN(iter);
735 assert(dq_ref.size() ==
736 static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
737 const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
738 assert(ddq_ref.size() ==
739 static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
741 const Vector6& kp_contact = m_kp_constraintsSIN(iter);
742 const Vector6& kd_contact = m_kd_constraintsSIN(iter);
743 const Vector3& kp_com = m_kp_comSIN(iter);
744 const Vector3& kd_com = m_kd_comSIN(iter);
746 const VectorN& kp_posture = m_kp_postureSIN(iter);
747 assert(kp_posture.size() ==
748 static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
749 const VectorN& kd_posture = m_kd_postureSIN(iter);
750 assert(kd_posture.size() ==
751 static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
752 const VectorN& kp_pos = m_kp_posSIN(iter);
753 assert(kp_pos.size() ==
754 static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
755 const VectorN& kd_pos = m_kd_posSIN(iter);
756 assert(kd_pos.size() ==
757 static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
760 const double& w_com = m_w_comSIN(iter);
761 const double& w_posture = m_w_postureSIN(iter);
762 const double& w_forces = m_w_forcesSIN(iter);
764 if (m_contactState == LEFT_SUPPORT ||
765 m_contactState == LEFT_SUPPORT_TRANSITION) {
766 const Eigen::Matrix<double, 12, 1>& x_rf_ref = m_rf_ref_posSIN(iter);
767 const Vector6& dx_rf_ref = m_rf_ref_velSIN(iter);
768 const Vector6& ddx_rf_ref = m_rf_ref_accSIN(iter);
769 const Vector6& kp_feet = m_kp_feetSIN(iter);
770 const Vector6& kd_feet = m_kd_feetSIN(iter);
771 m_sampleRF.pos = x_rf_ref;
772 m_sampleRF.vel = dx_rf_ref;
773 m_sampleRF.acc = ddx_rf_ref;
774 m_taskRF->setReference(m_sampleRF);
775 m_taskRF->Kp(kp_feet);
776 m_taskRF->Kd(kd_feet);
777 }
else if (m_contactState == RIGHT_SUPPORT ||
778 m_contactState == RIGHT_SUPPORT_TRANSITION) {
779 const Eigen::Matrix<double, 12, 1>& x_lf_ref = m_lf_ref_posSIN(iter);
780 const Vector6& dx_lf_ref = m_lf_ref_velSIN(iter);
781 const Vector6& ddx_lf_ref = m_lf_ref_accSIN(iter);
782 const Vector6& kp_feet = m_kp_feetSIN(iter);
783 const Vector6& kd_feet = m_kd_feetSIN(iter);
784 m_sampleLF.pos = x_lf_ref;
785 m_sampleLF.vel = dx_lf_ref;
786 m_sampleLF.acc = ddx_lf_ref;
787 m_taskLF->setReference(m_sampleLF);
788 m_taskLF->Kp(kp_feet);
789 m_taskLF->Kd(kd_feet);
791 if (m_rightHandState == TASK_RIGHT_HAND_ON ) {
792 const Eigen::Matrix<double, 12, 1>& x_rh_ref = m_rh_ref_posSIN(iter);
793 const Vector6& dx_rh_ref = m_rh_ref_velSIN(iter);
794 const Vector6& ddx_rh_ref = m_rh_ref_accSIN(iter);
795 const Vector6& kp_hands = m_kp_handsSIN(iter);
796 const Vector6& kd_hands = m_kd_handsSIN(iter);
797 m_sampleRH.pos = x_rh_ref;
798 m_sampleRH.vel = dx_rh_ref;
799 m_sampleRH.acc = ddx_rh_ref;
800 m_taskRH->setReference(m_sampleRH);
801 m_taskRH->Kp(kp_hands);
802 m_taskRH->Kd(kd_hands);
804 if (m_leftHandState ==
805 TASK_LEFT_HAND_ON ) {
806 const Eigen::Matrix<double, 12, 1>& x_lh_ref = m_lh_ref_posSIN(iter);
807 const Vector6& dx_lh_ref = m_lh_ref_velSIN(iter);
808 const Vector6& ddx_lh_ref = m_lh_ref_accSIN(iter);
809 const Vector6& kp_hands = m_kp_handsSIN(iter);
810 const Vector6& kd_hands = m_kd_handsSIN(iter);
811 m_sampleLH.pos = x_lh_ref;
812 m_sampleLH.vel = dx_lh_ref;
813 m_sampleLH.acc = ddx_lh_ref;
814 m_taskLH->setReference(m_sampleLH);
815 m_taskLH->Kp(kp_hands);
816 m_taskLH->Kd(kd_hands);
822 m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf);
823 m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf);
825 m_sampleCom.pos = x_com_ref - m_com_offset;
826 m_sampleCom.vel = dx_com_ref;
827 m_sampleCom.acc = ddx_com_ref;
828 m_taskCom->setReference(m_sampleCom);
829 m_taskCom->Kp(kp_com);
830 m_taskCom->Kd(kd_com);
831 if (m_w_com != w_com) {
835 m_invDyn->updateTaskWeight(m_taskCom->name(), w_com);
838 m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos);
839 m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel);
840 m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc);
841 m_taskPosture->setReference(m_samplePosture);
842 m_taskPosture->Kp(kp_posture);
843 m_taskPosture->Kd(kd_posture);
844 if (m_w_posture != w_posture) {
847 m_w_posture = w_posture;
848 m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture);
858 const double& fMin = m_f_minSIN(0);
859 const double& fMaxRF = m_f_max_right_footSIN(iter);
860 const double& fMaxLF = m_f_max_left_footSIN(iter);
861 m_contactLF->setMinNormalForce(fMin);
862 m_contactRF->setMinNormalForce(fMin);
863 m_contactLF->setMaxNormalForce(fMaxLF);
864 m_contactRF->setMaxNormalForce(fMaxRF);
865 m_contactLF->Kp(kp_contact);
866 m_contactLF->Kd(kd_contact);
867 m_contactRF->Kp(kp_contact);
868 m_contactRF->Kd(kd_contact);
869 m_invDyn->updateRigidContactWeights(m_contactLF->name(), w_forces);
870 m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces);
874 m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
876 pinocchio::SE3 H_lf = m_robot->position(
878 m_robot->model().getJointId(
879 m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
880 m_contactLF->setReference(H_lf);
881 SEND_MSG(
"Setting left foot reference to " + toString(H_lf),
884 pinocchio::SE3 H_rf = m_robot->position(
886 m_robot->model().getJointId(
887 m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
888 m_contactRF->setReference(H_rf);
889 SEND_MSG(
"Setting right foot reference to " + toString(H_rf),
891 }
else if (m_timeLast !=
static_cast<unsigned int>(iter - 1)) {
892 SEND_MSG(
"Last time " + toString(m_timeLast) +
893 " is not current time-1: " + toString(iter),
895 if (m_timeLast ==
static_cast<unsigned int>(iter)) {
900 m_timeLast =
static_cast<unsigned int>(iter);
902 const HQPData& hqpData =
903 m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
908 SolverHQPBase* solver = m_hqpSolver;
909 if (m_invDyn->nVar() == 60 && m_invDyn->nEq() == 36 &&
910 m_invDyn->nIn() == 34) {
911 solver = m_hqpSolver_60_36_34;
912 getStatistics().store(
"solver fixed size 60_36_34", 1.0);
913 }
else if (m_invDyn->nVar() == 48 && m_invDyn->nEq() == 30 &&
914 m_invDyn->nIn() == 17) {
915 solver = m_hqpSolver_48_30_17;
916 getStatistics().store(
"solver fixed size 48_30_17", 1.0);
918 getStatistics().store(
"solver dynamic size", 1.0);
920 const HQPOutput& sol = solver->solve(hqpData);
923 if (sol.status != HQP_STATUS_OPTIMAL) {
924 SEND_ERROR_STREAM_MSG(
"HQP solver failed to find a solution: " +
925 toString(sol.status));
926 SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData,
false));
927 SEND_DEBUG_STREAM_MSG(
"q=" + toString(q_sot.transpose(), 1, 5));
928 SEND_DEBUG_STREAM_MSG(
"v=" + toString(v_sot.transpose(), 1, 5));
933 getStatistics().store(
"active inequalities",
934 static_cast<double>(sol.activeSet.size()));
935 getStatistics().store(
"solver iterations", sol.iterations);
936 if (ddx_com_ref.norm() > 1e-3)
937 getStatistics().store(
939 ddx_com_ref.norm() / m_taskCom->getConstraint().vector().norm());
941 m_dv_urdf = m_invDyn->getAccelerations(sol);
942 m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot);
943 Eigen::Matrix<double, 12, 1> tmp;
944 if (m_invDyn->getContactForces(m_contactRF->name(), sol, tmp))
945 m_f_RF = m_contactRF->getForceGeneratorMatrix() * tmp;
946 if (m_invDyn->getContactForces(m_contactLF->name(), sol, tmp))
947 m_f_LF = m_contactLF->getForceGeneratorMatrix() * tmp;
948 m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot);
951 kp_pos.cwiseProduct(q_ref - q_sot.tail(m_robot_util->m_nbJoints)) +
952 kd_pos.cwiseProduct(dq_ref - v_sot.tail(m_robot_util->m_nbJoints));