17 #include <dynamic-graph/factory.h> 19 #include <boost/test/unit_test.hpp> 20 #include <sot/core/debug.hh> 23 #include <tsid/math/utils.hpp> 24 #include <tsid/solvers/solver-HQP-factory.hxx> 25 #include <tsid/solvers/utils.hpp> 26 #include <tsid/utils/statistics.hpp> 27 #include <tsid/utils/stop-watch.hpp> 30 #define ODEBUG(x) std::cout << x << std::endl 34 #define ODEBUG3(x) std::cout << x << std::endl 36 #define DBGFILE "/tmp/debug-sot-torque-control.dat" 38 #define RESETDEBUG5() \ 40 std::ofstream DebugFile; \ 41 DebugFile.open(DBGFILE, std::ofstream::out); \ 44 #define ODEBUG5FULL(x) \ 46 std::ofstream DebugFile; \ 47 DebugFile.open(DBGFILE, std::ofstream::app); \ 48 DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \ 49 << "):" << x << std::endl; \ 54 std::ofstream DebugFile; \ 55 DebugFile.open(DBGFILE, std::ofstream::app); \ 56 DebugFile << x << std::endl; \ 61 #define ODEBUG4FULL(x) 67 namespace dg = ::dynamicgraph;
79 #define REQUIRE_FINITE(A) assert(is_finite(A)) 82 #define PROFILE_TAU_DES_COMPUTATION "SimpleInverseDyn: desired tau" 83 #define PROFILE_HQP_SOLUTION "SimpleInverseDyn: HQP" 84 #define PROFILE_PREPARE_INV_DYN "SimpleInverseDyn: prepare inv-dyn" 85 #define PROFILE_READ_INPUT_SIGNALS "SimpleInverseDyn: read input signals" 87 #define ZERO_FORCE_THRESHOLD 1e-3 89 #define INPUT_SIGNALS \ 90 m_posture_ref_posSIN << m_posture_ref_velSIN << m_posture_ref_accSIN \ 91 << m_kp_postureSIN << m_kd_postureSIN << m_w_postureSIN \ 92 << m_kp_posSIN << m_kd_posSIN << m_com_ref_posSIN \ 93 << m_com_ref_velSIN << m_com_ref_accSIN << m_kp_comSIN \ 94 << m_kd_comSIN << m_w_comSIN << m_kp_contactSIN \ 95 << m_kd_contactSIN << m_w_forcesSIN << m_muSIN \ 96 << m_contact_pointsSIN << m_contact_normalSIN \ 97 << m_f_minSIN << m_f_maxSIN << m_waist_ref_posSIN \ 98 << m_waist_ref_velSIN << m_waist_ref_accSIN \ 99 << m_kp_waistSIN << m_kd_waistSIN << m_w_waistSIN \ 100 << m_qSIN << m_vSIN << m_active_jointsSIN 102 #define OUTPUT_SIGNALS \ 103 m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT 109 typedef Eigen::Matrix<double, 2, 1>
Vector2;
110 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN;
111 typedef Eigen::Matrix<double, Eigen::Dynamic, 1>
VectorN6;
122 CONSTRUCT_SIGNAL_IN(posture_ref_pos,
dynamicgraph::Vector),
123 CONSTRUCT_SIGNAL_IN(posture_ref_vel,
dynamicgraph::Vector),
124 CONSTRUCT_SIGNAL_IN(posture_ref_acc,
dynamicgraph::Vector),
127 CONSTRUCT_SIGNAL_IN(w_posture, double),
135 CONSTRUCT_SIGNAL_IN(w_com, double),
138 CONSTRUCT_SIGNAL_IN(w_forces, double),
139 CONSTRUCT_SIGNAL_IN(mu, double),
140 CONSTRUCT_SIGNAL_IN(contact_points,
dynamicgraph::Matrix),
141 CONSTRUCT_SIGNAL_IN(contact_normal,
dynamicgraph::Vector),
142 CONSTRUCT_SIGNAL_IN(f_min, double),
143 CONSTRUCT_SIGNAL_IN(f_max, double),
144 CONSTRUCT_SIGNAL_IN(waist_ref_pos,
dynamicgraph::Vector),
145 CONSTRUCT_SIGNAL_IN(waist_ref_vel,
dynamicgraph::Vector),
146 CONSTRUCT_SIGNAL_IN(waist_ref_acc,
dynamicgraph::Vector),
149 CONSTRUCT_SIGNAL_IN(w_waist, double),
152 CONSTRUCT_SIGNAL_IN(active_joints,
dynamicgraph::Vector),
153 CONSTRUCT_SIGNAL_INNER(active_joints_checked,
dg::Vector,
156 CONSTRUCT_SIGNAL_OUT(dv_des,
dg::Vector, m_tau_desSOUT),
157 CONSTRUCT_SIGNAL_OUT(v_des,
dg::Vector, m_dv_desSOUT),
158 CONSTRUCT_SIGNAL_OUT(q_des,
dg::Vector, m_v_desSOUT),
159 CONSTRUCT_SIGNAL_OUT(
161 INPUT_SIGNALS << m_tau_desSOUT << m_v_desSOUT << m_q_desSOUT),
163 m_initSucceeded(false),
167 m_robot_util(RefVoidRobotUtil()),
177 docCommandVoid2(
"Initialize the entity.",
178 "Time period in seconds (double)",
179 "Robot reference (string)")));
181 addCommand(
"setControlOutputType",
184 docCommandVoid1(
"Set the type of control output.",
185 "Control type: velocity or torque (string)")));
192 "Update the offset on the CoM based on the CoP measurement.")));
198 SEND_MSG(
"CoM offset updated: " + toString(
m_com_offset), MSG_TYPE_INFO);
208 sotDEBUG(25) <<
"Unrecognized control output type: " << type << endl;
213 return SEND_MSG(
"Init failed: Timestep must be positive", MSG_TYPE_ERROR);
216 std::string localName(robotRef);
217 if (isNameInRobotUtil(localName))
220 SEND_MSG(
"You should have an entity controller manager initialized before",
225 const Eigen::Matrix<double, 3, 4>& contactPoints = m_contact_pointsSIN(0);
226 const Eigen::Vector3d& contactNormal = m_contact_normalSIN(0);
227 const dg::sot::Vector6d& kp_contact = m_kp_contactSIN(0);
228 const dg::sot::Vector6d& kd_contact = m_kd_contactSIN(0);
229 const Eigen::Vector3d& kp_com = m_kp_comSIN(0);
230 const Eigen::Vector3d& kd_com = m_kd_comSIN(0);
231 const VectorN& kp_posture = m_kp_postureSIN(0);
232 const VectorN& kd_posture = m_kd_postureSIN(0);
233 const dg::sot::Vector6d& kp_waist = m_kp_waistSIN(0);
234 const dg::sot::Vector6d& kd_waist = m_kd_waistSIN(0);
236 assert(kp_posture.size() ==
237 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
238 assert(kd_posture.size() ==
239 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
244 const double& w_forces = m_w_forcesSIN(0);
245 const double& mu = m_muSIN(0);
246 const double& fMin = m_f_minSIN(0);
247 const double& fMax = m_f_maxSIN(0);
250 vector<string> package_dirs;
252 new robots::RobotWrapper(
m_robot_util->m_urdf_filename, package_dirs,
253 pinocchio::JointModelFreeFlyer());
271 new Contact6d(
"contact_rfoot", *
m_robot,
273 contactPoints, contactNormal, mu, fMin, fMax);
279 new Contact6d(
"contact_lfoot", *
m_robot,
281 contactPoints, contactNormal, mu, fMin, fMax);
300 Eigen::VectorXd mask_orientation(6);
301 mask_orientation << 0, 0, 0, 1, 1, 1;
318 m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
322 }
catch (
const std::exception& e) {
323 std::cout << e.what();
325 "Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename,
338 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints))
341 const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
343 if (active_joints_sot.any()) {
346 s = active_joints_sot;
347 Eigen::VectorXd active_joints_urdf(
m_robot_util->m_nbJoints);
348 m_robot_util->joints_sot_to_urdf(active_joints_sot, active_joints_urdf);
351 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()),
361 TrajectorySample ref_zero(
366 }
else if (!active_joints_sot.any()) {
371 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++) s(i) =
false;
377 SEND_WARNING_STREAM_MSG(
378 "Cannot compute signal tau_des before initialization!");
381 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints))
388 m_active_joints_checkedSINNER(iter);
390 const VectorN6& q_robot = m_qSIN(iter);
391 assert(q_robot.size() ==
392 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints + 6));
393 const VectorN6& v_robot = m_vSIN(iter);
394 assert(v_robot.size() ==
395 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints + 6));
397 const Vector3& x_com_ref = m_com_ref_posSIN(iter);
398 const Vector3& dx_com_ref = m_com_ref_velSIN(iter);
399 const Vector3& ddx_com_ref = m_com_ref_accSIN(iter);
400 const VectorN& x_waist_ref = m_waist_ref_posSIN(iter);
401 const Vector6& dx_waist_ref = m_waist_ref_velSIN(iter);
402 const Vector6& ddx_waist_ref = m_waist_ref_accSIN(iter);
403 const VectorN& q_ref = m_posture_ref_posSIN(iter);
404 assert(q_ref.size() ==
405 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
406 const VectorN& dq_ref = m_posture_ref_velSIN(iter);
407 assert(dq_ref.size() ==
408 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
409 const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
410 assert(ddq_ref.size() ==
411 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
413 const Vector6& kp_contact = m_kp_contactSIN(iter);
414 const Vector6& kd_contact = m_kd_contactSIN(iter);
415 const Vector3& kp_com = m_kp_comSIN(iter);
416 const Vector3& kd_com = m_kd_comSIN(iter);
417 const Vector6& kp_waist = m_kp_waistSIN(iter);
418 const Vector6& kd_waist = m_kd_waistSIN(iter);
420 const VectorN& kp_posture = m_kp_postureSIN(iter);
421 assert(kp_posture.size() ==
422 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
423 const VectorN& kd_posture = m_kd_postureSIN(iter);
424 assert(kd_posture.size() ==
425 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
428 const double& w_com = m_w_comSIN(iter);
429 const double& w_posture = m_w_postureSIN(iter);
430 const double& w_forces = m_w_forcesSIN(iter);
431 const double& w_waist = m_w_waistSIN(iter);
470 const double& fMin = m_f_minSIN(0);
471 const double& fMax = m_f_maxSIN(iter);
490 pinocchio::SE3 H_lf =
m_robot->position(
495 SEND_MSG(
"Setting left foot reference to " + toString(H_lf),
498 pinocchio::SE3 H_rf =
m_robot->position(
503 SEND_MSG(
"Setting right foot reference to " + toString(H_rf),
505 }
else if (
m_timeLast != static_cast<unsigned int>(iter - 1)) {
506 SEND_MSG(
"Last time " + toString(
m_timeLast) +
507 " is not current time-1: " + toString(iter),
509 if (
m_timeLast == static_cast<unsigned int>(iter)) {
522 const HQPData& hqpData =
529 getStatistics().store(
"solver dynamic size", 1.0);
531 const HQPOutput& sol = solver->solve(hqpData);
534 if (sol.status != HQP_STATUS_OPTIMAL) {
535 SEND_ERROR_STREAM_MSG(
"HQP solver failed to find a solution: " +
536 toString(sol.status));
537 SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData,
false));
538 SEND_DEBUG_STREAM_MSG(
"q=" + toString(q_robot.transpose(), 1, 5));
539 SEND_DEBUG_STREAM_MSG(
"v=" + toString(v_robot.transpose(), 1, 5));
544 getStatistics().store(
"active inequalities",
545 static_cast<double>(sol.activeSet.size()));
546 getStatistics().store(
"solver iterations", sol.iterations);
562 SEND_WARNING_STREAM_MSG(
563 "Cannot compute signal dv_des before initialization!");
574 SEND_WARNING_STREAM_MSG(
575 "Cannot compute signal v_des before initialization!");
580 tsid::math::Vector v_mean;
591 SEND_WARNING_STREAM_MSG(
592 "Cannot compute signal q_des before initialization!");
604 SEND_WARNING_STREAM_MSG(
"Cannot compute signal u before initialization!");
607 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints))
610 const VectorN& kp_pos = m_kp_posSIN(iter);
611 assert(kp_pos.size() ==
612 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
613 const VectorN& kd_pos = m_kd_posSIN(iter);
614 assert(kd_pos.size() ==
615 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints));
617 const VectorN6& q_robot = m_qSIN(iter);
618 assert(q_robot.size() ==
619 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints + 6));
620 const VectorN6& v_robot = m_vSIN(iter);
621 assert(v_robot.size() ==
622 static_cast<Eigen::VectorXd::Index
>(
m_robot_util->m_nbJoints + 6));
642 os <<
"SimpleInverseDyn " << getName();
644 getProfiler().report_all(3, os);
645 getStatistics().report_all(1, os);
647 <<
" nIn " <<
m_invDyn->nIn() <<
"\n";
648 }
catch (ExceptionSignal e) {
tsid::math::Vector m_dv_urdf
desired accelerations (sot order)
#define PROFILE_READ_INPUT_SIGNALS
tsid::tasks::TaskComEquality * m_taskCom
tsid::tasks::TaskSE3Equality * m_taskWaist
virtual void setControlOutputType(const std::string &type)
double m_t
control loop time period
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6
tsid::InverseDynamicsFormulationAccForce * m_invDyn
tsid::solvers::SolverHQPBase * m_hqpSolver
Solver and problem formulation.
Eigen::Matrix< double, 3, 1 > Vector3
tsid::trajectories::TrajectorySample m_sampleWaist
tsid::tasks::TaskJointPosture * m_taskBlockedJoints
unsigned int m_timeLast
3d CoM offset
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleInverseDyn(const std::string &name)
bool m_firstTime
True if controler is enabled.
tsid::contacts::Contact6d * m_contactRF
TASKS.
bool m_enabled
true if the entity has been successfully initialized
tsid::math::Vector3 m_com_offset
desired torques (sot order)
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
tsid::tasks::TaskJointPosture * m_taskPosture
Eigen::Matrix< double, 2, 1 > Vector2
tsid::math::Vector m_v_sot
desired accelerations (urdf order)
bool m_initSucceeded
current time
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
tsid::trajectories::TrajectorySample m_sampleCom
Trajectories of the tasks.
RobotUtilShrPtr m_robot_util
Final time of the control loop.
#define PROFILE_HQP_SOLUTION
tsid::math::Vector m_tau_sot
Eigen::Matrix< double, 6, 1 > Vector6
tsid::math::Vector m_v_urdf
desired velocities (sot order)
tsid::trajectories::TrajectorySample m_samplePosture
double m_w_com
Weights of the Tasks (which can be changed)
tsid::math::Vector m_q_sot
AdmittanceController EntityClassName
const std::string ControlOutput_s[]
#define PROFILE_TAU_DES_COMPUTATION
ControlOutput m_ctrlMode
Share pointer to the robot utils methods.
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
tsid::robots::RobotWrapper * m_robot
True at the first iteration of the controller.
virtual void display(std::ostream &os) const
tsid::math::Vector m_q_urdf
desired positions (sot order)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
void init(const double &dt, const std::string &robotRef)
tsid::math::Vector m_dv_sot
Computed solutions (accelerations and torques) and their derivatives.
#define PROFILE_PREPARE_INV_DYN
tsid::contacts::Contact6d * m_contactLF