sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
simple-inverse-dyn.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3 *
4 * This file is part of sot-torque-control.
5 * sot-torque-control is free software: you can redistribute it and/or
6 * modify it under the terms of the GNU Lesser General Public License
7 * as published by the Free Software Foundation, either version 3 of
8 * the License, or (at your option) any later version.
9 * sot-torque-control is distributed in the hope that it will be
10 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU Lesser General Public License for more details. You should
13 * have received a copy of the GNU Lesser General Public License along
14 * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
15 */
16
17#include <dynamic-graph/factory.h>
18
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>
28
29#if DEBUG
30#define ODEBUG(x) std::cout << x << std::endl
31#else
32#define ODEBUG(x)
33#endif
34#define ODEBUG3(x) std::cout << x << std::endl
35
36#define DBGFILE "/tmp/debug-sot-torque-control.dat"
37
38#define RESETDEBUG5() \
39 { \
40 std::ofstream DebugFile; \
41 DebugFile.open(DBGFILE, std::ofstream::out); \
42 DebugFile.close(); \
43 }
44#define ODEBUG5FULL(x) \
45 { \
46 std::ofstream DebugFile; \
47 DebugFile.open(DBGFILE, std::ofstream::app); \
48 DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
49 << "):" << x << std::endl; \
50 DebugFile.close(); \
51 }
52#define ODEBUG5(x) \
53 { \
54 std::ofstream DebugFile; \
55 DebugFile.open(DBGFILE, std::ofstream::app); \
56 DebugFile << x << std::endl; \
57 DebugFile.close(); \
58 }
59
60#define RESETDEBUG4()
61#define ODEBUG4FULL(x)
62#define ODEBUG4(x)
63
64namespace dynamicgraph {
65namespace sot {
66namespace torque_control {
67namespace dg = ::dynamicgraph;
68using namespace dg;
69using namespace dg::command;
70using namespace std;
71using namespace tsid;
72using namespace tsid::trajectories;
73using namespace tsid::math;
74using namespace tsid::contacts;
75using namespace tsid::tasks;
76using namespace tsid::solvers;
77using namespace dg::sot;
78
79#define REQUIRE_FINITE(A) assert(is_finite(A))
80
81// Size to be aligned "-------------------------------------------------------"
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"
86
87#define ZERO_FORCE_THRESHOLD 1e-3
88
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
101
102#define OUTPUT_SIGNALS \
103 m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT
104
107typedef SimpleInverseDyn EntityClassName;
108
109typedef Eigen::Matrix<double, 2, 1> Vector2;
110typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN;
111typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN6;
112/* --- DG FACTORY ---------------------------------------------------- */
114
115/* ------------------------------------------------------------------- */
116/* --- CONSTRUCTION -------------------------------------------------- */
117/* ------------------------------------------------------------------- */
119 : Entity(name)
120
121 ,
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),
125 CONSTRUCT_SIGNAL_IN(kp_posture, dynamicgraph::Vector),
126 CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector),
127 CONSTRUCT_SIGNAL_IN(w_posture, double),
128 CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector),
129 CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector),
130 CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector),
131 CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector),
132 CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector),
133 CONSTRUCT_SIGNAL_IN(kp_com, dynamicgraph::Vector),
134 CONSTRUCT_SIGNAL_IN(kd_com, dynamicgraph::Vector),
135 CONSTRUCT_SIGNAL_IN(w_com, double),
136 CONSTRUCT_SIGNAL_IN(kp_contact, dynamicgraph::Vector),
137 CONSTRUCT_SIGNAL_IN(kd_contact, dynamicgraph::Vector),
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),
147 CONSTRUCT_SIGNAL_IN(kp_waist, dynamicgraph::Vector),
148 CONSTRUCT_SIGNAL_IN(kd_waist, dynamicgraph::Vector),
149 CONSTRUCT_SIGNAL_IN(w_waist, double),
150 CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector),
151 CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector),
152 CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector),
153 CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector,
154 m_active_jointsSIN),
155 CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS),
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(
160 u, dg::Vector,
161 INPUT_SIGNALS << m_tau_desSOUT << m_v_desSOUT << m_q_desSOUT),
162 m_t(0.0),
163 m_initSucceeded(false),
164 m_enabled(false),
165 m_firstTime(true),
166 m_timeLast(0),
167 m_robot_util(RefVoidRobotUtil()),
168 m_ctrlMode(CONTROL_OUTPUT_VELOCITY) {
169 RESETDEBUG5();
170 Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
171
172 m_com_offset.setZero();
173
174 /* Commands. */
175 addCommand("init",
176 makeCommandVoid2(*this, &SimpleInverseDyn::init,
177 docCommandVoid2("Initialize the entity.",
178 "Time period in seconds (double)",
179 "Robot reference (string)")));
180 /* SET of control output type. */
181 addCommand("setControlOutputType",
182 makeCommandVoid1(
184 docCommandVoid1("Set the type of control output.",
185 "Control type: velocity or torque (string)")));
186
187 addCommand(
188 "updateComOffset",
189 makeCommandVoid0(
191 docCommandVoid0(
192 "Update the offset on the CoM based on the CoP measurement.")));
193}
194
196 const Vector3& com = m_robot->com(m_invDyn->data());
197 m_com_offset = com;
198 SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO);
199}
200
201void SimpleInverseDyn::setControlOutputType(const std::string& type) {
202 for (int i = 0; i < CONTROL_OUTPUT_SIZE; i++)
203 if (type == ControlOutput_s[i]) {
205 sotDEBUG(25) << "Control output type: " << ControlOutput_s[i] << endl;
206 return;
207 }
208 sotDEBUG(25) << "Unrecognized control output type: " << type << endl;
209}
210
211void SimpleInverseDyn::init(const double& dt, const std::string& robotRef) {
212 if (dt <= 0.0)
213 return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR);
214
215 /* Retrieve m_robot_util informations */
216 std::string localName(robotRef);
217 if (isNameInRobotUtil(localName))
218 m_robot_util = getRobotUtil(localName);
219 else {
220 SEND_MSG("You should have an entity controller manager initialized before",
221 MSG_TYPE_ERROR);
222 return;
223 }
224
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);
235
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));
240
241 m_w_com = m_w_comSIN(0);
242 m_w_posture = m_w_postureSIN(0);
243 m_w_waist = m_w_waistSIN(0);
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);
248
249 try {
250 vector<string> package_dirs;
251 m_robot =
252 new robots::RobotWrapper(m_robot_util->m_urdf_filename, package_dirs,
253 pinocchio::JointModelFreeFlyer());
254
255 assert(m_robot->nv() >= 6);
256 m_robot_util->m_nbJoints = m_robot->nv() - 6;
257
258 m_q_sot.setZero(m_robot->nv());
259 m_v_sot.setZero(m_robot->nv());
260 m_dv_sot.setZero(m_robot->nv());
261 m_tau_sot.setZero(m_robot->nv() - 6);
262 // m_f.setZero(24);
263 m_q_urdf.setZero(m_robot->nq());
264 m_v_urdf.setZero(m_robot->nv());
265 m_dv_urdf.setZero(m_robot->nv());
266
267 m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot);
268
269 // CONTACT 6D TASKS
271 new Contact6d("contact_rfoot", *m_robot,
272 m_robot_util->m_foot_util.m_Right_Foot_Frame_Name,
273 contactPoints, contactNormal, mu, fMin, fMax);
274 m_contactRF->Kp(kp_contact);
275 m_contactRF->Kd(kd_contact);
276 m_invDyn->addRigidContact(*m_contactRF, w_forces);
277
279 new Contact6d("contact_lfoot", *m_robot,
280 m_robot_util->m_foot_util.m_Left_Foot_Frame_Name,
281 contactPoints, contactNormal, mu, fMin, fMax);
282 m_contactLF->Kp(kp_contact);
283 m_contactLF->Kd(kd_contact);
284 m_invDyn->addRigidContact(*m_contactLF, w_forces);
285
286 // TASK COM
287 m_taskCom = new TaskComEquality("task-com", *m_robot);
288 m_taskCom->Kp(kp_com);
289 m_taskCom->Kd(kd_com);
290 m_invDyn->addMotionTask(*m_taskCom, m_w_com, 0);
291
292 // WAIST Task
293 m_taskWaist = new TaskSE3Equality("task-waist", *m_robot, "root_joint");
294 m_taskWaist->Kp(kp_waist);
295 m_taskWaist->Kd(kd_waist);
296 // Add a Mask to the task which will select the vector dimensions on which
297 // the task will act. In this case the waist configuration is a vector 6d
298 // (position and orientation -> SE3) Here we set a mask = [0 0 0 1 1 1] so
299 // the task on the waist will act on the orientation of the robot
300 Eigen::VectorXd mask_orientation(6);
301 mask_orientation << 0, 0, 0, 1, 1, 1;
302 m_taskWaist->setMask(mask_orientation);
303 // Add the task to the HQP with weight = 1.0, priority level = 1 (in the
304 // cost function) and a transition duration = 0.0
305 m_invDyn->addMotionTask(*m_taskWaist, m_w_waist, 1);
306
307 // POSTURE TASK
308 m_taskPosture = new TaskJointPosture("task-posture", *m_robot);
309 m_taskPosture->Kp(kp_posture);
310 m_taskPosture->Kd(kd_posture);
311 m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1);
312
313 // TRAJECTORY INIT
314 m_sampleCom = TrajectorySample(3);
315 m_sampleWaist = TrajectorySample(6);
316 m_samplePosture = TrajectorySample(m_robot->nv() - 6);
317
318 m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
319 "eiquadprog-fast");
320 m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn());
321
322 } catch (const std::exception& e) {
323 std::cout << e.what();
324 return SEND_MSG(
325 "Init failed: Could load URDF :" + m_robot_util->m_urdf_filename,
326 MSG_TYPE_ERROR);
327 }
328 m_dt = dt;
329 m_initSucceeded = true;
330}
331
332/* ------------------------------------------------------------------- */
333/* --- SIGNALS ------------------------------------------------------- */
334/* ------------------------------------------------------------------- */
337DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector) {
338 if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
339 s.resize(m_robot_util->m_nbJoints);
340
341 const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
342 if (m_enabled == false) {
343 if (active_joints_sot.any()) {
344 /* from all OFF to some ON */
345 m_enabled = true;
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);
349
350 m_taskBlockedJoints =
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;
356 else
357 blocked_joints(i) = 0.0;
358 SEND_MSG("Blocked joints: " + toString(blocked_joints.transpose()),
359 MSG_TYPE_INFO);
360 m_taskBlockedJoints->mask(blocked_joints);
361 TrajectorySample ref_zero(
362 static_cast<unsigned int>(m_robot_util->m_nbJoints));
363 m_taskBlockedJoints->setReference(ref_zero);
364 m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0);
365 }
366 } else if (!active_joints_sot.any()) {
367 /* from some ON to all OFF */
368 m_enabled = false;
369 }
370 if (m_enabled == false)
371 for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = false;
372 return s;
373}
374
375DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) {
376 if (!m_initSucceeded) {
377 SEND_WARNING_STREAM_MSG(
378 "Cannot compute signal tau_des before initialization!");
379 return s;
380 }
381 if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
382 s.resize(m_robot_util->m_nbJoints);
383
384 getProfiler().start(PROFILE_TAU_DES_COMPUTATION);
385
386 getProfiler().start(PROFILE_READ_INPUT_SIGNALS);
387
388 m_active_joints_checkedSINNER(iter);
389
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));
396
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));
412
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);
419
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));
426
427 /*const double & w_hands = m_w_handsSIN(iter);*/
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);
432
433 getProfiler().stop(PROFILE_READ_INPUT_SIGNALS);
434
435 getProfiler().start(PROFILE_PREPARE_INV_DYN);
436
437 m_sampleCom.pos = x_com_ref - m_com_offset;
438 m_sampleCom.vel = dx_com_ref;
439 m_sampleCom.acc = ddx_com_ref;
440 m_taskCom->setReference(m_sampleCom);
441 m_taskCom->Kp(kp_com);
442 m_taskCom->Kd(kd_com);
443 if (m_w_com != w_com) {
444 m_w_com = w_com;
445 m_invDyn->updateTaskWeight(m_taskCom->name(), w_com);
446 }
447
448 m_sampleWaist.pos = x_waist_ref;
449 m_sampleWaist.vel = dx_waist_ref;
450 m_sampleWaist.acc = ddx_waist_ref;
451 m_taskWaist->setReference(m_sampleWaist);
452 m_taskWaist->Kp(kp_waist);
453 m_taskWaist->Kd(kd_waist);
454 if (m_w_waist != w_waist) {
455 m_w_waist = w_waist;
456 m_invDyn->updateTaskWeight(m_taskWaist->name(), w_waist);
457 }
458
459 m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos);
460 m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel);
461 m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc);
462 m_taskPosture->setReference(m_samplePosture);
463 m_taskPosture->Kp(kp_posture);
464 m_taskPosture->Kd(kd_posture);
465 if (m_w_posture != w_posture) {
466 m_w_posture = w_posture;
467 m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture);
468 }
469
470 const double& fMin = m_f_minSIN(0);
471 const double& fMax = m_f_maxSIN(iter);
472 m_contactLF->setMinNormalForce(fMin);
473 m_contactRF->setMinNormalForce(fMin);
474 m_contactLF->setMaxNormalForce(fMax);
475 m_contactRF->setMaxNormalForce(fMax);
476 m_contactLF->Kp(kp_contact);
477 m_contactLF->Kd(kd_contact);
478 m_contactRF->Kp(kp_contact);
479 m_contactRF->Kd(kd_contact);
480 m_invDyn->updateRigidContactWeights(m_contactLF->name(), w_forces);
481 m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces);
482
483 if (m_firstTime) {
484 m_firstTime = false;
485 m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf);
486 m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf);
487
488 m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
489 // m_robot->computeAllTerms(m_invDyn->data(), q, v);
490 pinocchio::SE3 H_lf = m_robot->position(
491 m_invDyn->data(),
492 m_robot->model().getJointId(
493 m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
494 m_contactLF->setReference(H_lf);
495 SEND_MSG("Setting left foot reference to " + toString(H_lf),
496 MSG_TYPE_DEBUG);
497
498 pinocchio::SE3 H_rf = m_robot->position(
499 m_invDyn->data(),
500 m_robot->model().getJointId(
501 m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
502 m_contactRF->setReference(H_rf);
503 SEND_MSG("Setting right foot reference to " + toString(H_rf),
504 MSG_TYPE_DEBUG);
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),
508 MSG_TYPE_ERROR);
509 if (m_timeLast == static_cast<unsigned int>(iter)) {
510 s = m_tau_sot;
511 return s;
512 }
513 }
514 // else if (m_ctrlMode == CONTROL_OUTPUT_TORQUE){
515 // // In velocity close the TSID loop on itself (v_des, q_des), in torque on
516 // the (q,v) of the robot. m_robot_util->config_sot_to_urdf(q_robot,
517 // m_q_urdf); m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot,
518 // m_v_urdf);
519 // }
520 m_timeLast = static_cast<unsigned int>(iter);
521
522 const HQPData& hqpData =
523 m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
524
525 getProfiler().stop(PROFILE_PREPARE_INV_DYN);
526
527 getProfiler().start(PROFILE_HQP_SOLUTION);
528 SolverHQPBase* solver = m_hqpSolver;
529 getStatistics().store("solver dynamic size", 1.0);
530
531 const HQPOutput& sol = solver->solve(hqpData);
532 getProfiler().stop(PROFILE_HQP_SOLUTION);
533
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));
540 s.setZero();
541 return s;
542 }
543
544 getStatistics().store("active inequalities",
545 static_cast<double>(sol.activeSet.size()));
546 getStatistics().store("solver iterations", sol.iterations);
547
548 m_dv_urdf = m_invDyn->getAccelerations(sol);
549 m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot);
550 m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot);
551
552 getProfiler().stop(PROFILE_TAU_DES_COMPUTATION);
553 m_t += m_dt;
554
555 s = m_tau_sot;
556
557 return s;
558}
559
560DEFINE_SIGNAL_OUT_FUNCTION(dv_des, dynamicgraph::Vector) {
561 if (!m_initSucceeded) {
562 SEND_WARNING_STREAM_MSG(
563 "Cannot compute signal dv_des before initialization!");
564 return s;
565 }
566 if (s.size() != m_robot->nv()) s.resize(m_robot->nv());
567 m_tau_desSOUT(iter);
568 s = m_dv_sot;
569 return s;
570}
571
572DEFINE_SIGNAL_OUT_FUNCTION(v_des, dynamicgraph::Vector) {
573 if (!m_initSucceeded) {
574 SEND_WARNING_STREAM_MSG(
575 "Cannot compute signal v_des before initialization!");
576 return s;
577 }
578 if (s.size() != m_robot->nv()) s.resize(m_robot->nv());
579 m_dv_desSOUT(iter);
580 tsid::math::Vector v_mean;
581 v_mean = m_v_urdf + 0.5 * m_dt * m_dv_urdf;
582 m_v_urdf = m_v_urdf + m_dt * m_dv_urdf;
583 m_q_urdf = pinocchio::integrate(m_robot->model(), m_q_urdf, m_dt * v_mean);
584 m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_v_urdf, m_v_sot);
585 s = m_v_sot;
586 return s;
587}
588
589DEFINE_SIGNAL_OUT_FUNCTION(q_des, dynamicgraph::Vector) {
590 if (!m_initSucceeded) {
591 SEND_WARNING_STREAM_MSG(
592 "Cannot compute signal q_des before initialization!");
593 return s;
594 }
595 if (s.size() != m_robot->nv()) s.resize(m_robot->nv());
596 m_v_desSOUT(iter);
597 m_robot_util->config_urdf_to_sot(m_q_urdf, m_q_sot);
598 s = m_q_sot;
599 return s;
600}
601
602DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
603 if (!m_initSucceeded) {
604 SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!");
605 return s;
606 }
607 if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
608 s.resize(m_robot_util->m_nbJoints);
609
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));
616
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));
623
624 m_q_desSOUT(iter);
625
626 s = m_tau_sot +
627 kp_pos.cwiseProduct(m_q_sot.tail(m_robot_util->m_nbJoints) -
628 q_robot.tail(m_robot_util->m_nbJoints)) +
629 kd_pos.cwiseProduct(m_v_sot.tail(m_robot_util->m_nbJoints) -
630 v_robot.tail(m_robot_util->m_nbJoints));
631
632 return s;
633}
634
635/* --- COMMANDS ---------------------------------------------------------- */
636
637/* ------------------------------------------------------------------- */
638/* --- ENTITY -------------------------------------------------------- */
639/* ------------------------------------------------------------------- */
640
641void SimpleInverseDyn::display(std::ostream& os) const {
642 os << "SimpleInverseDyn " << getName();
643 try {
644 getProfiler().report_all(3, os);
645 getStatistics().report_all(1, os);
646 os << "QP size: nVar " << m_invDyn->nVar() << " nEq " << m_invDyn->nEq()
647 << " nIn " << m_invDyn->nIn() << "\n";
648 } catch (ExceptionSignal e) {
649 }
650}
651} // namespace torque_control
652} // namespace sot
653} // namespace dynamicgraph
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
double m_w_com
Weights of the Tasks (which can be changed)
tsid::solvers::SolverHQPBase * m_hqpSolver
Solver and problem formulation.
RobotUtilShrPtr m_robot_util
Final time of the control loop.
tsid::math::Vector m_dv_sot
Computed solutions (accelerations and torques) and their derivatives.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimpleInverseDyn(const std::string &name)
tsid::InverseDynamicsFormulationAccForce * m_invDyn
tsid::math::Vector m_v_urdf
desired velocities (sot order)
tsid::robots::RobotWrapper * m_robot
True at the first iteration of the controller.
tsid::math::Vector m_q_urdf
desired positions (sot order)
tsid::math::Vector m_v_sot
desired accelerations (urdf order)
tsid::trajectories::TrajectorySample m_samplePosture
tsid::trajectories::TrajectorySample m_sampleCom
Trajectories of the tasks.
tsid::math::Vector m_dv_urdf
desired accelerations (sot order)
tsid::math::Vector3 m_com_offset
desired torques (sot order)
ControlOutput m_ctrlMode
Share pointer to the robot utils methods.
void init(const double &dt, const std::string &robotRef)
tsid::trajectories::TrajectorySample m_sampleWaist
virtual void setControlOutputType(const std::string &type)
#define RESETDEBUG5()
#define PROFILE_TAU_DES_COMPUTATION
#define PROFILE_READ_INPUT_SIGNALS
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
Eigen::Matrix< double, 3, 1 > Vector3
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
Eigen::Matrix< double, 6, 1 > Vector6
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6
to read text file
Definition: treeview.dox:22