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;
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))
342 const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
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);
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);
360 TrajectorySample ref_zero(static_cast<unsigned int>(
m_robot_util->m_nbJoints));
364 }
else if (!active_joints_sot.any()) {
369 for (
unsigned int i = 0; i <
m_robot_util->m_nbJoints; i++)
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))
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);
461 const double & fMin = m_f_minSIN(0);
462 const double & fMax = m_f_maxSIN(iter);
484 SEND_MSG(
"Setting left foot reference to " + toString(H_lf), MSG_TYPE_DEBUG);
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)) {
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);
541 SEND_WARNING_STREAM_MSG(
"Cannot compute signal dv_des before initialization!");
553 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v_des before initialization!");
559 tsid::math::Vector v_mean;
570 SEND_WARNING_STREAM_MSG(
"Cannot compute signal q_des before initialization!");
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))
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));
614 os <<
"SimpleInverseDyn " << getName();
616 getProfiler().report_all(3, os);
617 getStatistics().report_all(1, os);
619 }
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
desired and current positions (urdf order) (close the TSID loop on it)
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
desired and current velocities (urdf order) (close the TSID loop on it)
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