17 #include <boost/test/unit_test.hpp>
19 #include <tsid/utils/stop-watch.hpp>
20 #include <tsid/utils/statistics.hpp>
21 #include <tsid/solvers/solver-HQP-factory.hxx>
22 #include <tsid/solvers/utils.hpp>
23 #include <tsid/math/utils.hpp>
25 #include <dynamic-graph/factory.h>
27 #include <sot/core/debug.hh>
33 #define ODEBUG(x) std::cout << x << std::endl
37 #define ODEBUG3(x) std::cout << x << std::endl
39 #define DBGFILE "/tmp/debug-sot-torque-control.dat"
41 #define RESETDEBUG5() { std::ofstream DebugFile; \
42 DebugFile.open(DBGFILE,std::ofstream::out); \
44 #define ODEBUG5FULL(x) { std::ofstream DebugFile; \
45 DebugFile.open(DBGFILE,std::ofstream::app); \
46 DebugFile << __FILE__ << ":" \
47 << __FUNCTION__ << "(#" \
48 << __LINE__ << "):" << x << std::endl; \
50 #define ODEBUG5(x) { std::ofstream DebugFile; \
51 DebugFile.open(DBGFILE,std::ofstream::app); \
52 DebugFile << x << std::endl; \
56 #define ODEBUG4FULL(x)
62 namespace dg = ::dynamicgraph;
64 using namespace dg::command;
67 using namespace tsid::trajectories;
68 using namespace tsid::math;
69 using namespace tsid::contacts;
70 using namespace tsid::tasks;
71 using namespace tsid::solvers;
72 using namespace dg::sot;
74 #define REQUIRE_FINITE(A) assert(is_finite(A))
77 #define PROFILE_TAU_DES_COMPUTATION "SimpleInverseDyn: desired tau"
78 #define PROFILE_HQP_SOLUTION "SimpleInverseDyn: HQP"
79 #define PROFILE_PREPARE_INV_DYN "SimpleInverseDyn: prepare inv-dyn"
80 #define PROFILE_READ_INPUT_SIGNALS "SimpleInverseDyn: read input signals"
82 #define ZERO_FORCE_THRESHOLD 1e-3
84 #define INPUT_SIGNALS m_posture_ref_posSIN \
85 << m_posture_ref_velSIN \
86 << m_posture_ref_accSIN \
102 << m_contact_pointsSIN \
103 << m_contact_normalSIN \
106 << m_waist_ref_posSIN \
107 << m_waist_ref_velSIN \
108 << m_waist_ref_accSIN \
114 << m_active_jointsSIN
116 #define OUTPUT_SIGNALS m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT
123 typedef Eigen::Matrix<double, 2, 1>
Vector2;
124 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN;
125 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN6;
137 , CONSTRUCT_SIGNAL_IN(posture_ref_pos,
dynamicgraph::Vector)
138 , CONSTRUCT_SIGNAL_IN(posture_ref_vel,
dynamicgraph::Vector)
139 , CONSTRUCT_SIGNAL_IN(posture_ref_acc,
dynamicgraph::Vector)
142 , CONSTRUCT_SIGNAL_IN(w_posture, double)
145 , CONSTRUCT_SIGNAL_IN(com_ref_pos,
dynamicgraph::Vector)
146 , CONSTRUCT_SIGNAL_IN(com_ref_vel,
dynamicgraph::Vector)
147 , CONSTRUCT_SIGNAL_IN(com_ref_acc,
dynamicgraph::Vector)
150 , CONSTRUCT_SIGNAL_IN(w_com, double)
153 , CONSTRUCT_SIGNAL_IN(w_forces, double)
154 , CONSTRUCT_SIGNAL_IN(mu, double)
155 , CONSTRUCT_SIGNAL_IN(contact_points,
dynamicgraph::Matrix)
156 , CONSTRUCT_SIGNAL_IN(contact_normal,
dynamicgraph::Vector)
157 , CONSTRUCT_SIGNAL_IN(f_min, double)
158 , CONSTRUCT_SIGNAL_IN(f_max, double)
159 , CONSTRUCT_SIGNAL_IN(waist_ref_pos,
dynamicgraph::Vector)
160 , CONSTRUCT_SIGNAL_IN(waist_ref_vel,
dynamicgraph::Vector)
161 , CONSTRUCT_SIGNAL_IN(waist_ref_acc,
dynamicgraph::Vector)
164 , CONSTRUCT_SIGNAL_IN(w_waist, double)
167 , CONSTRUCT_SIGNAL_IN(active_joints,
dynamicgraph::Vector)
168 , CONSTRUCT_SIGNAL_INNER(active_joints_checked,
dg::Vector, m_active_jointsSIN)
170 , CONSTRUCT_SIGNAL_OUT(dv_des,
dg::Vector, m_tau_desSOUT)
171 , CONSTRUCT_SIGNAL_OUT(v_des,
dg::Vector, m_dv_desSOUT)
172 , CONSTRUCT_SIGNAL_OUT(q_des,
dg::Vector, m_v_desSOUT)
173 , CONSTRUCT_SIGNAL_OUT(u,
dg::Vector,
INPUT_SIGNALS << m_tau_desSOUT << m_v_desSOUT << m_q_desSOUT)
175 , m_initSucceeded(false)
179 , m_robot_util(RefVoidRobotUtil())
189 docCommandVoid2(
"Initialize the entity.",
190 "Time period in seconds (double)",
191 "Robot reference (string)")));
193 addCommand(
"setControlOutputType",
195 docCommandVoid1(
"Set the type of control output.",
196 "Control type: velocity or torque (string)")));
198 addCommand(
"updateComOffset",
200 docCommandVoid0(
"Update the offset on the CoM based on the CoP measurement.")));
207 SEND_MSG(
"CoM offset updated: " + toString(
m_com_offset), MSG_TYPE_INFO);
217 sotDEBUG(25) <<
"Unrecognized control output type: " << type << endl;
222 return SEND_MSG(
"Init failed: Timestep must be positive", MSG_TYPE_ERROR);
225 std::string localName(robotRef);
226 if (isNameInRobotUtil(localName))
229 SEND_MSG(
"You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
233 const Eigen::Matrix<double, 3, 4>& contactPoints = m_contact_pointsSIN(0);
234 const Eigen::Vector3d& contactNormal = m_contact_normalSIN(0);
235 const dg::sot::Vector6d& kp_contact = m_kp_contactSIN(0);
236 const dg::sot::Vector6d& kd_contact = m_kd_contactSIN(0);
237 const Eigen::Vector3d& kp_com = m_kp_comSIN(0);
238 const Eigen::Vector3d& kd_com = m_kd_comSIN(0);
239 const VectorN& kp_posture = m_kp_postureSIN(0);
240 const VectorN& kd_posture = m_kd_postureSIN(0);
241 const dg::sot::Vector6d& kp_waist = m_kp_waistSIN(0);
242 const dg::sot::Vector6d& kd_waist = m_kd_waistSIN(0);
244 assert(kp_posture.size() ==
static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
245 assert(kd_posture.size() ==
static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
250 const double & w_forces = m_w_forcesSIN(0);
251 const double & mu = m_muSIN(0);
252 const double & fMin = m_f_minSIN(0);
253 const double & fMax = m_f_maxSIN(0);
256 vector<string> package_dirs;
259 pinocchio::JointModelFreeFlyer());
278 contactPoints, contactNormal,
286 contactPoints, contactNormal,
305 Eigen::VectorXd mask_orientation(6);
306 mask_orientation << 0, 0, 0, 1, 1, 1;
322 m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
326 }
catch (
const std::exception& e) {
327 std::cout << e.what();
328 return SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
339 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints))
340 s.resize(m_robot_util->m_nbJoints);
342 const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
343 if (m_enabled ==
false) {
344 if (active_joints_sot.any()) {
347 s = active_joints_sot;
348 Eigen::VectorXd active_joints_urdf(m_robot_util->m_nbJoints);
349 m_robot_util->joints_sot_to_urdf(active_joints_sot, active_joints_urdf);
351 m_taskBlockedJoints =
new TaskJointPosture(
"task-posture-blocked", *m_robot);
352 Eigen::VectorXd blocked_joints(m_robot_util->m_nbJoints);
353 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
354 if (active_joints_urdf(i) == 0.0)
355 blocked_joints(i) = 1.0;
357 blocked_joints(i) = 0.0;
358 SEND_MSG(
"Blocked joints: " + toString(blocked_joints.transpose()), MSG_TYPE_INFO);
359 m_taskBlockedJoints->mask(blocked_joints);
360 TrajectorySample ref_zero(
static_cast<unsigned int>(m_robot_util->m_nbJoints));
361 m_taskBlockedJoints->setReference(ref_zero);
362 m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0);
364 }
else if (!active_joints_sot.any()) {
368 if (m_enabled ==
false)
369 for (
unsigned int i = 0; i < m_robot_util->m_nbJoints; i++)
375 if (!m_initSucceeded) {
376 SEND_WARNING_STREAM_MSG(
"Cannot compute signal tau_des before initialization!");
379 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints))
380 s.resize(m_robot_util->m_nbJoints);
386 m_active_joints_checkedSINNER(iter);
388 const VectorN6& q_robot = m_qSIN(iter);
389 assert(q_robot.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6));
390 const VectorN6& v_robot = m_vSIN(iter);
391 assert(v_robot.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6));
393 const Vector3& x_com_ref = m_com_ref_posSIN(iter);
394 const Vector3& dx_com_ref = m_com_ref_velSIN(iter);
395 const Vector3& ddx_com_ref = m_com_ref_accSIN(iter);
396 const VectorN& x_waist_ref = m_waist_ref_posSIN(iter);
397 const Vector6& dx_waist_ref = m_waist_ref_velSIN(iter);
398 const Vector6& ddx_waist_ref = m_waist_ref_accSIN(iter);
399 const VectorN& q_ref = m_posture_ref_posSIN(iter);
400 assert(q_ref.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
401 const VectorN& dq_ref = m_posture_ref_velSIN(iter);
402 assert(dq_ref.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
403 const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
404 assert(ddq_ref.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
406 const Vector6& kp_contact = m_kp_contactSIN(iter);
407 const Vector6& kd_contact = m_kd_contactSIN(iter);
408 const Vector3& kp_com = m_kp_comSIN(iter);
409 const Vector3& kd_com = m_kd_comSIN(iter);
410 const Vector6& kp_waist = m_kp_waistSIN(iter);
411 const Vector6& kd_waist = m_kd_waistSIN(iter);
413 const VectorN& kp_posture = m_kp_postureSIN(iter);
414 assert(kp_posture.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
415 const VectorN& kd_posture = m_kd_postureSIN(iter);
416 assert(kd_posture.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
419 const double & w_com = m_w_comSIN(iter);
420 const double & w_posture = m_w_postureSIN(iter);
421 const double & w_forces = m_w_forcesSIN(iter);
422 const double & w_waist = m_w_waistSIN(iter);
428 m_sampleCom.pos = x_com_ref - m_com_offset;
429 m_sampleCom.vel = dx_com_ref;
430 m_sampleCom.acc = ddx_com_ref;
431 m_taskCom->setReference(m_sampleCom);
432 m_taskCom->Kp(kp_com);
433 m_taskCom->Kd(kd_com);
434 if (m_w_com != w_com) {
436 m_invDyn->updateTaskWeight(m_taskCom->name(), w_com);
439 m_sampleWaist.pos = x_waist_ref;
440 m_sampleWaist.vel = dx_waist_ref;
441 m_sampleWaist.acc = ddx_waist_ref;
442 m_taskWaist->setReference(m_sampleWaist);
443 m_taskWaist->Kp(kp_waist);
444 m_taskWaist->Kd(kd_waist);
445 if (m_w_waist != w_waist) {
447 m_invDyn->updateTaskWeight(m_taskWaist->name(), w_waist);
450 m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos);
451 m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel);
452 m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc);
453 m_taskPosture->setReference(m_samplePosture);
454 m_taskPosture->Kp(kp_posture);
455 m_taskPosture->Kd(kd_posture);
456 if (m_w_posture != w_posture) {
457 m_w_posture = w_posture;
458 m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture);
461 const double & fMin = m_f_minSIN(0);
462 const double & fMax = m_f_maxSIN(iter);
463 m_contactLF->setMinNormalForce(fMin);
464 m_contactRF->setMinNormalForce(fMin);
465 m_contactLF->setMaxNormalForce(fMax);
466 m_contactRF->setMaxNormalForce(fMax);
467 m_contactLF->Kp(kp_contact);
468 m_contactLF->Kd(kd_contact);
469 m_contactRF->Kp(kp_contact);
470 m_contactRF->Kd(kd_contact);
471 m_invDyn->updateRigidContactWeights(m_contactLF->name(), w_forces);
472 m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces);
476 m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf);
477 m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf);
479 m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
481 pinocchio::SE3 H_lf = m_robot->position(m_invDyn->data(),
482 m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
483 m_contactLF->setReference(H_lf);
484 SEND_MSG(
"Setting left foot reference to " + toString(H_lf), MSG_TYPE_DEBUG);
486 pinocchio::SE3 H_rf = m_robot->position(m_invDyn->data(),
487 m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
488 m_contactRF->setReference(H_rf);
489 SEND_MSG(
"Setting right foot reference to " + toString(H_rf), MSG_TYPE_DEBUG);
490 }
else if (m_timeLast !=
static_cast<unsigned int>(iter - 1)) {
491 SEND_MSG(
"Last time " + toString(m_timeLast) +
" is not current time-1: " + toString(iter), MSG_TYPE_ERROR);
492 if (m_timeLast ==
static_cast<unsigned int>(iter)) {
502 m_timeLast =
static_cast<unsigned int>(iter);
504 const HQPData & hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
509 SolverHQPBase * solver = m_hqpSolver;
510 getStatistics().store(
"solver dynamic size", 1.0);
512 const HQPOutput & sol = solver->solve(hqpData);
515 if (sol.status != HQP_STATUS_OPTIMAL) {
516 SEND_ERROR_STREAM_MSG(
"HQP solver failed to find a solution: " + toString(sol.status));
517 SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData,
false));
518 SEND_DEBUG_STREAM_MSG(
"q=" + toString(q_robot.transpose(), 1, 5));
519 SEND_DEBUG_STREAM_MSG(
"v=" + toString(v_robot.transpose(), 1, 5));
524 getStatistics().store(
"active inequalities",
static_cast<double>(sol.activeSet.size()));
525 getStatistics().store(
"solver iterations", sol.iterations);
527 m_dv_urdf = m_invDyn->getAccelerations(sol);
528 m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot);
529 m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot);
540 if (!m_initSucceeded) {
541 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dv_des before initialization!");
544 if (s.size() != m_robot->nv())
545 s.resize(m_robot->nv());
552 if (!m_initSucceeded) {
553 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_des before initialization!");
556 if (s.size() != m_robot->nv())
557 s.resize(m_robot->nv());
559 tsid::math::Vector v_mean;
560 v_mean = m_v_urdf + 0.5 * m_dt * m_dv_urdf;
561 m_v_urdf = m_v_urdf + m_dt * m_dv_urdf;
562 m_q_urdf = pinocchio::integrate(m_robot->model(), m_q_urdf, m_dt * v_mean);
563 m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_v_urdf, m_v_sot);
569 if (!m_initSucceeded) {
570 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_des before initialization!");
573 if (s.size() != m_robot->nv())
574 s.resize(m_robot->nv());
576 m_robot_util->config_urdf_to_sot(m_q_urdf, m_q_sot);
582 if (!m_initSucceeded) {
583 SEND_WARNING_STREAM_MSG(
"Cannot compute signal u before initialization!");
586 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints))
587 s.resize(m_robot_util->m_nbJoints);
589 const VectorN& kp_pos = m_kp_posSIN(iter);
590 assert(kp_pos.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
591 const VectorN& kd_pos = m_kd_posSIN(iter);
592 assert(kd_pos.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints));
594 const VectorN6& q_robot = m_qSIN(iter);
595 assert(q_robot.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6));
596 const VectorN6& v_robot = m_vSIN(iter);
597 assert(v_robot.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6));
601 s = m_tau_sot + kp_pos.cwiseProduct(m_q_sot.tail(m_robot_util->m_nbJoints) - q_robot.tail(m_robot_util->m_nbJoints)) +
602 kd_pos.cwiseProduct(m_v_sot.tail(m_robot_util->m_nbJoints) - v_robot.tail(m_robot_util->m_nbJoints));
614 os <<
"SimpleInverseDyn " << getName();
616 getProfiler().report_all(3, os);
617 getStatistics().report_all(1, os);
619 }
catch (ExceptionSignal e) {}