robotoc
robotoc - efficient ROBOT Optimal Control solvers
Loading...
Searching...
No Matches
robot.hxx
Go to the documentation of this file.
1#ifndef ROBOTOC_ROBOT_HXX_
2#define ROBOTOC_ROBOT_HXX_
3
4#include "robotoc/robot/robot.hpp"
5
6#include "pinocchio/parsers/urdf.hpp"
7#include "pinocchio/algorithm/joint-configuration.hpp"
8#include "pinocchio/algorithm/kinematics-derivatives.hpp"
9#include "pinocchio/algorithm/frames.hpp"
10#include "pinocchio/algorithm/frames-derivatives.hpp"
11#include "pinocchio/algorithm/crba.hpp"
12#include "pinocchio/algorithm/rnea.hpp"
13#include "pinocchio/algorithm/rnea-derivatives.hpp"
14#include "pinocchio/algorithm/aba.hpp"
15#include "pinocchio/algorithm/cholesky.hpp"
16#include "pinocchio/algorithm/contact-dynamics.hpp"
17
18#include <cassert>
19
20namespace robotoc {
21
22template <typename TangentVectorType, typename ConfigVectorType>
23inline void Robot::integrateConfiguration(
24 const Eigen::MatrixBase<TangentVectorType>& v,
25 const double integration_length,
26 const Eigen::MatrixBase<ConfigVectorType>& q) const {
27 assert(v.size() == dimv_);
28 assert(q.size() == dimq_);
29 if (info_.base_joint_type == BaseJointType::FloatingBase) {
30 const Eigen::VectorXd q_tmp = q;
31 pinocchio::integrate(model_, q_tmp, integration_length*v,
32 const_cast<Eigen::MatrixBase<ConfigVectorType>&>(q));
33 }
34 else {
35 (const_cast<Eigen::MatrixBase<ConfigVectorType>&>(q)).noalias()
36 += integration_length * v;
37 }
38}
39
40
41template <typename ConfigVectorType1, typename TangentVectorType,
42 typename ConfigVectorType2>
43inline void Robot::integrateConfiguration(
44 const Eigen::MatrixBase<ConfigVectorType1>& q,
45 const Eigen::MatrixBase<TangentVectorType>& v,
46 const double integration_length,
47 const Eigen::MatrixBase<ConfigVectorType2>& q_integrated) const {
48 assert(q.size() == dimq_);
49 assert(v.size() == dimv_);
50 assert(q_integrated.size() == dimq_);
51 pinocchio::integrate(
52 model_, q, integration_length*v,
53 const_cast<Eigen::MatrixBase<ConfigVectorType2>&>(q_integrated));
54}
55
56
57template <typename ConfigVectorType, typename TangentVectorType,
58 typename MatrixType1, typename MatrixType2>
59inline void Robot::dIntegrateTransport_dq(
60 const Eigen::MatrixBase<ConfigVectorType>& q,
61 const Eigen::MatrixBase<TangentVectorType>& v,
62 const Eigen::MatrixBase<MatrixType1>& Jin,
63 const Eigen::MatrixBase<MatrixType2>& Jout) const {
64 assert(q.size() == dimq_);
65 assert(v.size() == dimv_);
66 assert(Jin.cols() == dimv_);
67 assert(Jout.rows() == Jin.rows());
68 assert(Jout.cols() == Jin.cols());
69 pinocchio::dIntegrateTransport(
70 model_, q, v, Jin.transpose(),
71 const_cast<Eigen::MatrixBase<MatrixType2>&>(Jout).transpose(),
72 pinocchio::ARG0);
73}
74
75
76template <typename ConfigVectorType, typename TangentVectorType,
77 typename MatrixType1, typename MatrixType2>
78inline void Robot::dIntegrateTransport_dv(
79 const Eigen::MatrixBase<ConfigVectorType>& q,
80 const Eigen::MatrixBase<TangentVectorType>& v,
81 const Eigen::MatrixBase<MatrixType1>& Jin,
82 const Eigen::MatrixBase<MatrixType2>& Jout) const {
83 assert(q.size() == dimq_);
84 assert(v.size() == dimv_);
85 assert(Jin.cols() == dimv_);
86 assert(Jout.rows() == Jin.rows());
87 assert(Jout.cols() == Jin.cols());
88 pinocchio::dIntegrateTransport(
89 model_, q, v, Jin.transpose(),
90 const_cast<Eigen::MatrixBase<MatrixType2>&>(Jout).transpose(),
91 pinocchio::ARG1);
92}
93
94
95template <typename ConfigVectorType1, typename ConfigVectorType2,
96 typename TangentVectorType>
97inline void Robot::subtractConfiguration(
98 const Eigen::MatrixBase<ConfigVectorType1>& qf,
99 const Eigen::MatrixBase<ConfigVectorType2>& q0,
100 const Eigen::MatrixBase<TangentVectorType>& qdiff) const {
101 assert(qf.size() == dimq_);
102 assert(q0.size() == dimq_);
103 assert(qdiff.size() == dimv_);
104 pinocchio::difference(
105 model_, q0, qf,
106 const_cast<Eigen::MatrixBase<TangentVectorType>&>(qdiff));
107}
108
109
110template <typename ConfigVectorType1, typename ConfigVectorType2,
111 typename MatrixType>
112inline void Robot::dSubtractConfiguration_dqf(
113 const Eigen::MatrixBase<ConfigVectorType1>& qf,
114 const Eigen::MatrixBase<ConfigVectorType2>& q0,
115 const Eigen::MatrixBase<MatrixType>& dqdiff_dqf) const {
116 assert(qf.size() == dimq_);
117 assert(q0.size() == dimq_);
118 assert(dqdiff_dqf.rows() == dimv_);
119 assert(dqdiff_dqf.cols() == dimv_);
120 pinocchio::dDifference(model_, q0, qf,
121 const_cast<Eigen::MatrixBase<MatrixType>&>(dqdiff_dqf),
122 pinocchio::ARG1);
123}
124
125
126template <typename ConfigVectorType1, typename ConfigVectorType2,
127 typename MatrixType>
128inline void Robot::dSubtractConfiguration_dq0(
129 const Eigen::MatrixBase<ConfigVectorType1>& qf,
130 const Eigen::MatrixBase<ConfigVectorType2>& q0,
131 const Eigen::MatrixBase<MatrixType>& dqdiff_dq0) const {
132 assert(qf.size() == dimq_);
133 assert(q0.size() == dimq_);
134 assert(dqdiff_dq0.rows() == dimv_);
135 assert(dqdiff_dq0.cols() == dimv_);
136 pinocchio::dDifference(model_, q0, qf,
137 const_cast<Eigen::MatrixBase<MatrixType>&>(dqdiff_dq0),
138 pinocchio::ARG0);
139}
140
141
142template <typename ConfigVectorType1, typename ConfigVectorType2,
143 typename ConfigVectorType3>
144inline void Robot::interpolateConfiguration(
145 const Eigen::MatrixBase<ConfigVectorType1>& q1,
146 const Eigen::MatrixBase<ConfigVectorType2>& q2, const double t,
147 const Eigen::MatrixBase<ConfigVectorType3>& qout) const {
148 assert(q1.size() == dimq_);
149 assert(q2.size() == dimq_);
150 assert(qout.size() == dimq_);
151 assert(t >= 0.0);
152 assert(t <= 1.0);
153 pinocchio::interpolate(model_, q1, q2, t,
154 const_cast<Eigen::MatrixBase<ConfigVectorType3>&>(qout));
155}
156
157
158template <typename ConfigVectorType, typename MatrixType>
159inline void Robot::integrateCoeffWiseJacobian(
160 const Eigen::MatrixBase<ConfigVectorType>& q,
161 const Eigen::MatrixBase<MatrixType>& J) const {
162 assert(q.size() == dimq_);
163 assert(J.rows() == dimq_);
164 assert(J.cols() == dimv_);
165 pinocchio::integrateCoeffWiseJacobian(model_, q,
166 const_cast<Eigen::MatrixBase<MatrixType>&>(J));
167}
168
169
170template <typename ConfigVectorType, typename TangentVectorType1,
171 typename TangentVectorType2>
172inline void Robot::updateKinematics(
173 const Eigen::MatrixBase<ConfigVectorType>& q,
174 const Eigen::MatrixBase<TangentVectorType1>& v,
175 const Eigen::MatrixBase<TangentVectorType2>& a) {
176 assert(q.size() == dimq_);
177 assert(v.size() == dimv_);
178 assert(a.size() == dimv_);
179 pinocchio::forwardKinematics(model_, data_, q, v, a);
180 pinocchio::updateFramePlacements(model_, data_);
181 pinocchio::computeForwardKinematicsDerivatives(model_, data_, q, v, a);
182 pinocchio::jacobianCenterOfMass(model_, data_, false);
183}
184
185
186template <typename ConfigVectorType, typename TangentVectorType>
187inline void Robot::updateKinematics(
188 const Eigen::MatrixBase<ConfigVectorType>& q,
189 const Eigen::MatrixBase<TangentVectorType>& v) {
190 assert(q.size() == dimq_);
191 assert(v.size() == dimv_);
192 pinocchio::forwardKinematics(model_, data_, q, v);
193 pinocchio::updateFramePlacements(model_, data_);
194 pinocchio::computeForwardKinematicsDerivatives(model_, data_, q, v,
195 Eigen::VectorXd::Zero(dimv_));
196 pinocchio::jacobianCenterOfMass(model_, data_, false);
197}
198
199
200template <typename ConfigVectorType>
201inline void Robot::updateKinematics(
202 const Eigen::MatrixBase<ConfigVectorType>& q) {
203 assert(q.size() == dimq_);
204 pinocchio::framesForwardKinematics(model_, data_, q);
205 pinocchio::computeJointJacobians(model_, data_, q);
206 pinocchio::jacobianCenterOfMass(model_, data_, false);
207}
208
209
210template <typename ConfigVectorType, typename TangentVectorType1,
211 typename TangentVectorType2>
212inline void Robot::updateFrameKinematics(
213 const Eigen::MatrixBase<ConfigVectorType>& q,
214 const Eigen::MatrixBase<TangentVectorType1>& v,
215 const Eigen::MatrixBase<TangentVectorType2>& a) {
216 assert(q.size() == dimq_);
217 assert(v.size() == dimv_);
218 assert(a.size() == dimv_);
219 pinocchio::forwardKinematics(model_, data_, q, v, a);
220 pinocchio::updateFramePlacements(model_, data_);
221 pinocchio::centerOfMass(model_, data_, q, v, a, false);
222}
223
224
225template <typename ConfigVectorType, typename TangentVectorType>
226inline void Robot::updateFrameKinematics(
227 const Eigen::MatrixBase<ConfigVectorType>& q,
228 const Eigen::MatrixBase<TangentVectorType>& v) {
229 assert(q.size() == dimq_);
230 assert(v.size() == dimv_);
231 pinocchio::forwardKinematics(model_, data_, q, v);
232 pinocchio::updateFramePlacements(model_, data_);
233 pinocchio::centerOfMass(model_, data_, q, v, false);
234}
235
236
237template <typename ConfigVectorType>
238inline void Robot::updateFrameKinematics(
239 const Eigen::MatrixBase<ConfigVectorType>& q) {
240 assert(q.size() == dimq_);
241 pinocchio::framesForwardKinematics(model_, data_, q);
242 pinocchio::centerOfMass(model_, data_, q, false);
243}
244
245
246template <typename MatrixType>
247inline void Robot::getFrameJacobian(const int frame_id,
248 const Eigen::MatrixBase<MatrixType>& J) {
249 assert(J.rows() == 6);
250 assert(J.cols() == dimv_);
251 pinocchio::getFrameJacobian(model_, data_, frame_id, pinocchio::LOCAL,
252 const_cast<Eigen::MatrixBase<MatrixType>&>(J));
253}
254
255
256template <typename MatrixType>
257inline void Robot::getCoMJacobian(const Eigen::MatrixBase<MatrixType>& J) const {
258 assert(J.rows() == 3);
259 assert(J.cols() == dimv_);
260 const_cast<Eigen::MatrixBase<MatrixType>&>(J) = data_.Jcom;
261}
262
263
264template <typename Vector3dType>
265inline void Robot::transformFromLocalToWorld(
266 const int frame_id, const Eigen::Vector3d& vec_local,
267 const Eigen::MatrixBase<Vector3dType>& vec_world) const {
268 assert(vec_world.size() == 3);
269 const_cast<Eigen::MatrixBase<Vector3dType>&>(vec_world).noalias()
270 = frameRotation(frame_id) * vec_local;
271}
272
273
274template <typename Vector3dType, typename MatrixType>
275inline void Robot::getJacobianTransformFromLocalToWorld(
276 const int frame_id, const Eigen::MatrixBase<Vector3dType>& vec_world,
277 const Eigen::MatrixBase<MatrixType>& J) {
278 assert(vec_world.size() == 3);
279 assert(J.rows() == 6);
280 assert(J.cols() == dimv_);
281 const_cast<Eigen::MatrixBase<MatrixType>&>(J).setZero();
282 getFrameJacobian(frame_id, const_cast<Eigen::MatrixBase<MatrixType>&>(J));
283 for (int i=0; i<dimv_; ++i) {
284 const_cast<Eigen::MatrixBase<MatrixType>&>(J).template topRows<3>().col(i).noalias()
285 = J.template bottomRows<3>().col(i).cross(vec_world.template head<3>());
286 }
287}
288
289
290template <typename VectorType>
291inline void Robot::computeBaumgarteResidual(
292 const ContactStatus& contact_status,
293 const Eigen::MatrixBase<VectorType>& baumgarte_residual) {
294 assert(baumgarte_residual.size() == contact_status.dimf());
295 const int num_point_contacts = point_contacts_.size();
296 const int num_surface_contacts = surface_contacts_.size();
297 int dimf = 0;
298 for (int i=0; i<num_point_contacts; ++i) {
299 if (contact_status.isContactActive(i)) {
300 point_contacts_[i].computeBaumgarteResidual(
301 model_, data_, contact_status.contactPosition(i),
302 (const_cast<Eigen::MatrixBase<VectorType>&>(baumgarte_residual))
303 .template segment<3>(dimf));
304 dimf += 3;
305 }
306 }
307 for (int i=0; i<num_surface_contacts; ++i) {
308 if (contact_status.isContactActive(i+num_point_contacts)) {
309 surface_contacts_[i].computeBaumgarteResidual(
310 model_, data_, contact_status.contactPlacement(i+num_point_contacts),
311 (const_cast<Eigen::MatrixBase<VectorType>&>(baumgarte_residual))
312 .template segment<6>(dimf));
313 dimf += 6;
314 }
315 }
316 assert(dimf == contact_status.dimf());
317}
318
319
320template <typename MatrixType1, typename MatrixType2, typename MatrixType3>
321inline void Robot::computeBaumgarteDerivatives(
322 const ContactStatus& contact_status,
323 const Eigen::MatrixBase<MatrixType1>& baumgarte_partial_dq,
324 const Eigen::MatrixBase<MatrixType2>& baumgarte_partial_dv,
325 const Eigen::MatrixBase<MatrixType3>& baumgarte_partial_da) {
326 assert(baumgarte_partial_dq.rows() == contact_status.dimf());
327 assert(baumgarte_partial_dq.cols() == dimv_);
328 assert(baumgarte_partial_dv.rows() == contact_status.dimf());
329 assert(baumgarte_partial_dv.cols() == dimv_);
330 assert(baumgarte_partial_da.rows() == contact_status.dimf());
331 assert(baumgarte_partial_da.cols() == dimv_);
332 const int num_point_contacts = point_contacts_.size();
333 const int num_surface_contacts = surface_contacts_.size();
334 int dimf = 0;
335 for (int i=0; i<num_point_contacts; ++i) {
336 if (contact_status.isContactActive(i)) {
337 point_contacts_[i].computeBaumgarteDerivatives(
338 model_, data_,
339 (const_cast<Eigen::MatrixBase<MatrixType1>&>(baumgarte_partial_dq))
340 .block(dimf, 0, 3, dimv_),
341 (const_cast<Eigen::MatrixBase<MatrixType2>&>(baumgarte_partial_dv))
342 .block(dimf, 0, 3, dimv_),
343 (const_cast<Eigen::MatrixBase<MatrixType3>&>(baumgarte_partial_da))
344 .block(dimf, 0, 3, dimv_));
345 dimf += 3;
346 }
347 }
348 for (int i=0; i<num_surface_contacts; ++i) {
349 if (contact_status.isContactActive(i+num_point_contacts)) {
350 surface_contacts_[i].computeBaumgarteDerivatives(
351 model_, data_,
352 (const_cast<Eigen::MatrixBase<MatrixType1>&>(baumgarte_partial_dq))
353 .block(dimf, 0, 6, dimv_),
354 (const_cast<Eigen::MatrixBase<MatrixType2>&>(baumgarte_partial_dv))
355 .block(dimf, 0, 6, dimv_),
356 (const_cast<Eigen::MatrixBase<MatrixType3>&>(baumgarte_partial_da))
357 .block(dimf, 0, 6, dimv_));
358 dimf += 6;
359 }
360 }
361 assert(dimf == contact_status.dimf());
362}
363
364
365template <typename VectorType>
366inline void Robot::computeImpactVelocityResidual(
367 const ImpactStatus& impact_status,
368 const Eigen::MatrixBase<VectorType>& velocity_residual) const {
369 assert(velocity_residual.size() == impact_status.dimf());
370 const int num_point_contacts = point_contacts_.size();
371 const int num_surface_contacts = surface_contacts_.size();
372 int dimf = 0;
373 for (int i=0; i<num_point_contacts; ++i) {
374 if (impact_status.isImpactActive(i)) {
375 point_contacts_[i].computeContactVelocityResidual(
376 model_, data_,
377 (const_cast<Eigen::MatrixBase<VectorType>&>(velocity_residual))
378 .template segment<3>(dimf));
379 dimf += 3;
380 }
381 }
382 for (int i=0; i<num_surface_contacts; ++i) {
383 if (impact_status.isImpactActive(i+num_point_contacts)) {
384 surface_contacts_[i].computeContactVelocityResidual(
385 model_, data_,
386 (const_cast<Eigen::MatrixBase<VectorType>&>(velocity_residual))
387 .template segment<6>(dimf));
388 dimf += 6;
389 }
390 }
391 assert(dimf == impact_status.dimf());
392}
393
394
395template <typename MatrixType1, typename MatrixType2>
396inline void Robot::computeImpactVelocityDerivatives(
397 const ImpactStatus& impact_status,
398 const Eigen::MatrixBase<MatrixType1>& velocity_partial_dq,
399 const Eigen::MatrixBase<MatrixType2>& velocity_partial_dv) {
400 assert(velocity_partial_dq.rows() == impact_status.dimf());
401 assert(velocity_partial_dq.cols() == dimv_);
402 assert(velocity_partial_dv.rows() == impact_status.dimf());
403 assert(velocity_partial_dv.cols() == dimv_);
404 const int num_point_contacts = point_contacts_.size();
405 const int num_surface_contacts = surface_contacts_.size();
406 int dimf = 0;
407 for (int i=0; i<num_point_contacts; ++i) {
408 if (impact_status.isImpactActive(i)) {
409 point_contacts_[i].computeContactVelocityDerivatives(
410 model_, data_,
411 (const_cast<Eigen::MatrixBase<MatrixType1>&>(velocity_partial_dq))
412 .block(dimf, 0, 3, dimv_),
413 (const_cast<Eigen::MatrixBase<MatrixType2>&>(velocity_partial_dv))
414 .block(dimf, 0, 3, dimv_));
415 dimf += 3;
416 }
417 }
418 for (int i=0; i<num_surface_contacts; ++i) {
419 if (impact_status.isImpactActive(i+num_point_contacts)) {
420 surface_contacts_[i].computeContactVelocityDerivatives(
421 model_, data_,
422 (const_cast<Eigen::MatrixBase<MatrixType1>&>(velocity_partial_dq))
423 .block(dimf, 0, 6, dimv_),
424 (const_cast<Eigen::MatrixBase<MatrixType2>&>(velocity_partial_dv))
425 .block(dimf, 0, 6, dimv_));
426 dimf += 6;
427 }
428 }
429 assert(dimf == impact_status.dimf());
430}
431
432
433template <typename VectorType>
434inline void Robot::computeContactPositionResidual(
435 const ImpactStatus& impact_status,
436 const Eigen::MatrixBase<VectorType>& position_residual) {
437 assert(position_residual.size() == impact_status.dimf());
438 const int num_point_contacts = point_contacts_.size();
439 const int num_surface_contacts = surface_contacts_.size();
440 int dimf = 0;
441 for (int i=0; i<num_point_contacts; ++i) {
442 if (impact_status.isImpactActive(i)) {
443 point_contacts_[i].computeContactPositionResidual(
444 model_, data_, impact_status.contactPosition(i),
445 (const_cast<Eigen::MatrixBase<VectorType>&>(position_residual))
446 .template segment<3>(dimf));
447 dimf += 3;
448 }
449 }
450 for (int i=0; i<num_surface_contacts; ++i) {
451 if (impact_status.isImpactActive(i+num_point_contacts)) {
452 surface_contacts_[i].computeContactPositionResidual(
453 model_, data_, impact_status.contactPlacement(i+num_point_contacts),
454 (const_cast<Eigen::MatrixBase<VectorType>&>(position_residual))
455 .template segment<6>(dimf));
456 dimf += 6;
457 }
458 }
459 assert(dimf == impact_status.dimf());
460}
461
462
463template <typename MatrixType>
464inline void Robot::computeContactPositionDerivative(
465 const ImpactStatus& impact_status,
466 const Eigen::MatrixBase<MatrixType>& position_partial_dq) {
467 assert(position_partial_dq.rows() == impact_status.dimf());
468 assert(position_partial_dq.cols() == dimv_);
469 const int num_point_contacts = point_contacts_.size();
470 const int num_surface_contacts = surface_contacts_.size();
471 int dimf = 0;
472 for (int i=0; i<num_point_contacts; ++i) {
473 if (impact_status.isImpactActive(i)) {
474 point_contacts_[i].computeContactPositionDerivative(
475 model_, data_,
476 (const_cast<Eigen::MatrixBase<MatrixType>&>(position_partial_dq))
477 .block(dimf, 0, 3, dimv_));
478 dimf += 3;
479 }
480 }
481 for (int i=0; i<num_surface_contacts; ++i) {
482 if (impact_status.isImpactActive(i+num_point_contacts)) {
483 surface_contacts_[i].computeContactPositionDerivative(
484 model_, data_,
485 (const_cast<Eigen::MatrixBase<MatrixType>&>(position_partial_dq))
486 .block(dimf, 0, 6, dimv_));
487 dimf += 6;
488 }
489 }
490 assert(dimf == impact_status.dimf());
491}
492
493
494inline void Robot::setImpactForces(const ImpactStatus& impact_status,
495 const std::vector<Vector6d>& f) {
496 assert(f.size() == max_num_contacts_);
497 const int num_point_contacts = point_contacts_.size();
498 const int num_surface_contacts = surface_contacts_.size();
499 for (int i=0; i<num_point_contacts; ++i) {
500 if (impact_status.isImpactActive(i)) {
501 point_contacts_[i].computeJointForceFromContactForce(
502 f[i].template head<3>(), fjoint_);
503 }
504 else {
505 point_contacts_[i].computeJointForceFromContactForce(
506 Eigen::Vector3d::Zero(), fjoint_);
507 }
508 }
509 for (int i=0; i<num_surface_contacts; ++i) {
510 if (impact_status.isImpactActive(i+num_point_contacts)) {
511 surface_contacts_[i].computeJointForceFromContactWrench(
512 f[i+num_point_contacts], fjoint_);
513 }
514 else {
515 surface_contacts_[i].computeJointForceFromContactWrench(
516 Vector6d::Zero(), fjoint_);
517 }
518 }
519}
520
521
522template <typename ConfigVectorType, typename TangentVectorType1,
523 typename TangentVectorType2, typename TangentVectorType3>
524inline void Robot::RNEA(const Eigen::MatrixBase<ConfigVectorType>& q,
525 const Eigen::MatrixBase<TangentVectorType1>& v,
526 const Eigen::MatrixBase<TangentVectorType2>& a,
527 const Eigen::MatrixBase<TangentVectorType3>& tau) {
528 assert(q.size() == dimq_);
529 assert(v.size() == dimv_);
530 assert(a.size() == dimv_);
531 assert(tau.size() == dimv_);
532 if (max_num_contacts_) {
533 const_cast<Eigen::MatrixBase<TangentVectorType3>&>(tau)
534 = pinocchio::rnea(model_, data_, q, v, a, fjoint_);
535 }
536 else {
537 const_cast<Eigen::MatrixBase<TangentVectorType3>&>(tau)
538 = pinocchio::rnea(model_, data_, q, v, a);
539 }
540 if (properties_.has_generalized_momentum_bias) {
541 const_cast<Eigen::MatrixBase<TangentVectorType3>&>(tau).noalias()
542 -= properties_.generalized_momentum_bias;
543 }
544}
545
546
547template <typename ConfigVectorType, typename TangentVectorType1,
548 typename TangentVectorType2, typename MatrixType1,
549 typename MatrixType2, typename MatrixType3>
550inline void Robot::RNEADerivatives(
551 const Eigen::MatrixBase<ConfigVectorType>& q,
552 const Eigen::MatrixBase<TangentVectorType1>& v,
553 const Eigen::MatrixBase<TangentVectorType2>& a,
554 const Eigen::MatrixBase<MatrixType1>& dRNEA_partial_dq,
555 const Eigen::MatrixBase<MatrixType2>& dRNEA_partial_dv,
556 const Eigen::MatrixBase<MatrixType3>& dRNEA_partial_da) {
557 assert(q.size() == dimq_);
558 assert(v.size() == dimv_);
559 assert(a.size() == dimv_);
560 assert(dRNEA_partial_dq.cols() == dimv_);
561 assert(dRNEA_partial_dq.rows() == dimv_);
562 assert(dRNEA_partial_dv.cols() == dimv_);
563 assert(dRNEA_partial_dv.rows() == dimv_);
564 assert(dRNEA_partial_da.cols() == dimv_);
565 assert(dRNEA_partial_da.rows() == dimv_);
566 if (max_num_contacts_) {
567 pinocchio::computeRNEADerivatives(
568 model_, data_, q, v, a, fjoint_,
569 const_cast<Eigen::MatrixBase<MatrixType1>&>(dRNEA_partial_dq),
570 const_cast<Eigen::MatrixBase<MatrixType2>&>(dRNEA_partial_dv),
571 const_cast<Eigen::MatrixBase<MatrixType3>&>(dRNEA_partial_da));
572 }
573 else {
574 pinocchio::computeRNEADerivatives(
575 model_, data_, q, v, a,
576 const_cast<Eigen::MatrixBase<MatrixType1>&>(dRNEA_partial_dq),
577 const_cast<Eigen::MatrixBase<MatrixType2>&>(dRNEA_partial_dv),
578 const_cast<Eigen::MatrixBase<MatrixType3>&>(dRNEA_partial_da));
579 }
580 (const_cast<Eigen::MatrixBase<MatrixType3>&>(dRNEA_partial_da))
581 .template triangularView<Eigen::StrictlyLower>()
582 = (const_cast<Eigen::MatrixBase<MatrixType3>&>(dRNEA_partial_da)).transpose()
583 .template triangularView<Eigen::StrictlyLower>();
584}
585
586
587template <typename ConfigVectorType, typename TangentVectorType1,
588 typename TangentVectorType2>
589inline void Robot::RNEAImpact(
590 const Eigen::MatrixBase<ConfigVectorType>& q,
591 const Eigen::MatrixBase<TangentVectorType1>& dv,
592 const Eigen::MatrixBase<TangentVectorType2>& res) {
593 assert(q.size() == dimq_);
594 assert(dv.size() == dimv_);
595 assert(res.size() == dimv_);
596 const_cast<Eigen::MatrixBase<TangentVectorType2>&>(res)
597 = pinocchio::rnea(impact_model_, impact_data_, q,
598 Eigen::VectorXd::Zero(dimv_), dv, fjoint_);
599}
600
601
602template <typename ConfigVectorType, typename TangentVectorType,
603 typename MatrixType1, typename MatrixType2>
604inline void Robot::RNEAImpactDerivatives(
605 const Eigen::MatrixBase<ConfigVectorType>& q,
606 const Eigen::MatrixBase<TangentVectorType>& dv,
607 const Eigen::MatrixBase<MatrixType1>& dRNEA_partial_dq,
608 const Eigen::MatrixBase<MatrixType2>& dRNEA_partial_ddv) {
609 assert(q.size() == dimq_);
610 assert(dv.size() == dimv_);
611 assert(dRNEA_partial_dq.cols() == dimv_);
612 assert(dRNEA_partial_dq.rows() == dimv_);
613 assert(dRNEA_partial_ddv.cols() == dimv_);
614 assert(dRNEA_partial_ddv.rows() == dimv_);
615 pinocchio::computeRNEADerivatives(
616 impact_model_, impact_data_, q, Eigen::VectorXd::Zero(dimv_), dv,
617 fjoint_, const_cast<Eigen::MatrixBase<MatrixType1>&>(dRNEA_partial_dq),
618 dimpact_dv_,
619 const_cast<Eigen::MatrixBase<MatrixType2>&>(dRNEA_partial_ddv));
620 (const_cast<Eigen::MatrixBase<MatrixType2>&>(dRNEA_partial_ddv))
621 .template triangularView<Eigen::StrictlyLower>()
622 = (const_cast<Eigen::MatrixBase<MatrixType2>&>(dRNEA_partial_ddv)).transpose()
623 .template triangularView<Eigen::StrictlyLower>();
624}
625
626
627template <typename MatrixType1, typename MatrixType2>
628inline void Robot::computeMinv(const Eigen::MatrixBase<MatrixType1>& M,
629 const Eigen::MatrixBase<MatrixType2>& Minv) {
630 assert(M.rows() == dimv_);
631 assert(M.cols() == dimv_);
632 assert(Minv.rows() == dimv_);
633 assert(Minv.cols() == dimv_);
634 data_.M = M;
635 pinocchio::cholesky::decompose(model_, data_);
636 pinocchio::cholesky::computeMinv(
637 model_, data_, const_cast<Eigen::MatrixBase<MatrixType2>&>(Minv));
638}
639
640
641template <typename MatrixType1, typename MatrixType2, typename MatrixType3>
642inline void Robot::computeMJtJinv(
643 const Eigen::MatrixBase<MatrixType1>& M,
644 const Eigen::MatrixBase<MatrixType2>& J,
645 const Eigen::MatrixBase<MatrixType3>& MJtJinv) {
646 assert(M.rows() == dimv_);
647 assert(M.cols() == dimv_);
648 assert(J.rows() <= max_dimf_);
649 assert(J.cols() == dimv_);
650 assert(MJtJinv.rows() == M.rows()+J.rows());
651 assert(MJtJinv.cols() == M.rows()+J.rows());
652 const int dimf = J.rows();
653 data_.M = M;
654 pinocchio::cholesky::decompose(model_, data_);
655 data_.sDUiJt.leftCols(dimf) = J.transpose();
656 pinocchio::cholesky::Uiv(model_, data_, data_.sDUiJt.leftCols(dimf));
657 for (Eigen::DenseIndex k=0; k<dimv_; ++k) {
658 data_.sDUiJt.leftCols(dimf).row(k) /= std::sqrt(data_.D[k]);
659 }
660 data_.JMinvJt.topLeftCorner(dimf, dimf).noalias()
661 = data_.sDUiJt.leftCols(dimf).transpose() * data_.sDUiJt.leftCols(dimf);
662 if (info_.contact_inv_damping > 0.) {
663 data_.JMinvJt.diagonal().array() += info_.contact_inv_damping;
664 }
665 data_.llt_JMinvJt.compute(data_.JMinvJt.topLeftCorner(dimf, dimf));
666 assert(data_.llt_JMinvJt.info() == Eigen::Success);
667 Eigen::Block<MatrixType3> topLeft
668 = const_cast<Eigen::MatrixBase<MatrixType3>&>(MJtJinv).topLeftCorner(dimv_, dimv_);
669 Eigen::Block<MatrixType3> topRight
670 = const_cast<Eigen::MatrixBase<MatrixType3>&>(MJtJinv).topRightCorner(dimv_, dimf);
671 Eigen::Block<MatrixType3> bottomLeft
672 = const_cast<Eigen::MatrixBase<MatrixType3>&>(MJtJinv).bottomLeftCorner(dimf, dimv_);
673 Eigen::Block<MatrixType3> bottomRight
674 = const_cast<Eigen::MatrixBase<MatrixType3>&>(MJtJinv).bottomRightCorner(dimf, dimf);
675 bottomRight = - pinocchio::Data::MatrixXs::Identity(dimf, dimf);
676 topLeft.setIdentity();
677 data_.llt_JMinvJt.solveInPlace(bottomRight);
678 pinocchio::cholesky::solve(model_, data_, topLeft);
679 bottomLeft.noalias() = J * topLeft;
680 topRight.noalias() = bottomLeft.transpose() * (-bottomRight);
681 topLeft.noalias() -= topRight*bottomLeft;
682 bottomLeft = topRight.transpose();
683 assert(!MJtJinv.hasNaN());
684}
685
686
687template <typename ConfigVectorType>
688inline void Robot::normalizeConfiguration(
689 const Eigen::MatrixBase<ConfigVectorType>& q) const {
690 assert(q.size() == dimq_);
691 if (info_.base_joint_type == BaseJointType::FloatingBase) {
692 if (q.template segment<4>(3).squaredNorm()
693 <= std::numeric_limits<double>::epsilon()) {
694 (const_cast<Eigen::MatrixBase<ConfigVectorType>&> (q)).coeffRef(3) = 1;
695 }
696 pinocchio::normalize(model_,
697 const_cast<Eigen::MatrixBase<ConfigVectorType>&>(q));
698 }
699}
700
701} // namespace robotoc
702
703#endif // ROBOTOC_ROBOT_HXX_
robotoc::ContactStatus
Contact status of robot model.
Definition: contact_status.hpp:32
robotoc::ContactStatus::isContactActive
bool isContactActive(const int contact_index) const
Returns true if a contact is active and false if not.
Definition: contact_status.hxx:101
robotoc::ContactStatus::dimf
int dimf() const
Returns the dimension of the active contact forces.
Definition: contact_status.hxx:125
robotoc::ContactStatus::contactPlacement
const SE3 & contactPlacement(const int contact_index) const
Gets the contact placement.
Definition: contact_status.hxx:332
robotoc::ContactStatus::contactPosition
const Eigen::Vector3d & contactPosition(const int contact_index) const
Gets the contact position.
Definition: contact_status.hxx:346
robotoc::ImpactStatus
Impact status of robot model. Wrapper of ContactStatus to treat impacts.
Definition: impact_status.hpp:21
robotoc::ImpactStatus::dimf
int dimf() const
Returns the dimension of the active impact forces.
Definition: impact_status.hxx:70
robotoc::ImpactStatus::contactPlacement
const SE3 & contactPlacement(const int contact_index) const
Gets the contact placement.
Definition: impact_status.hxx:141
robotoc::ImpactStatus::isImpactActive
bool isImpactActive(const int contact_index) const
Returns true if a contact is active and false if not.
Definition: impact_status.hxx:55
robotoc::ImpactStatus::contactPosition
const Eigen::Vector3d & contactPosition(const int contact_index) const
Gets the contact position.
Definition: impact_status.hxx:147
robotoc::Robot::RNEA
void RNEA(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a, const Eigen::MatrixBase< TangentVectorType3 > &tau)
Computes inverse dynamics, i.e., generalized torques corresponding for given configuration,...
Definition: robot.hxx:524
robotoc::Robot::integrateConfiguration
void integrateConfiguration(const Eigen::MatrixBase< TangentVectorType > &v, const double integration_length, const Eigen::MatrixBase< ConfigVectorType > &q) const
Integrates the generalized velocity, that is, performs q <- q + integration_length * v like computati...
Definition: robot.hxx:23
robotoc::Robot::computeImpactVelocityDerivatives
void computeImpactVelocityDerivatives(const ImpactStatus &impact_status, const Eigen::MatrixBase< MatrixType1 > &velocity_partial_dq, const Eigen::MatrixBase< MatrixType2 > &velocity_partial_dv)
Computes the partial derivatives of the impact velocity constraint. Before calling this function,...
Definition: robot.hxx:396
robotoc::Robot::computeImpactVelocityResidual
void computeImpactVelocityResidual(const ImpactStatus &impact_status, const Eigen::MatrixBase< VectorType > &velocity_residual) const
Computes the residual of the impact velocity constraint. Before calling this function,...
Definition: robot.hxx:366
robotoc::Robot::subtractConfiguration
void subtractConfiguration(const Eigen::MatrixBase< ConfigVectorType1 > &qf, const Eigen::MatrixBase< ConfigVectorType2 > &q0, const Eigen::MatrixBase< TangentVectorType > &qdiff) const
Computes qf - q0 at the tangent space. The result means that the unit velocity from initial configura...
Definition: robot.hxx:97
robotoc::Robot::RNEADerivatives
void RNEADerivatives(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a, const Eigen::MatrixBase< MatrixType1 > &dRNEA_partial_dq, const Eigen::MatrixBase< MatrixType2 > &dRNEA_partial_dv, const Eigen::MatrixBase< MatrixType3 > &dRNEA_partial_da)
Computes the partial dervatives of the function of inverse dynamics with respect to the configuration...
Definition: robot.hxx:550
robotoc::Robot::getJacobianTransformFromLocalToWorld
void getJacobianTransformFromLocalToWorld(const int frame_id, const Eigen::MatrixBase< Vector3dType > &vec_world, const Eigen::MatrixBase< MatrixType > &J)
Jacobian of the transformation mapping of a 3D quantity from local frame to the world frame.
Definition: robot.hxx:275
robotoc::Robot::setImpactForces
void setImpactForces(const ImpactStatus &impact_status, const std::vector< Vector6d > &f)
Set impact forces in this robot model for each active impacts.
Definition: robot.hxx:494
robotoc::Robot::dSubtractConfiguration_dq0
void dSubtractConfiguration_dq0(const Eigen::MatrixBase< ConfigVectorType1 > &qf, const Eigen::MatrixBase< ConfigVectorType2 > &q0, const Eigen::MatrixBase< MatrixType > &dqdiff_dq0) const
Computes the partial derivative of the function of subtraction qf - q0 w.r.t. the initial configurati...
Definition: robot.hxx:128
robotoc::Robot::dIntegrateTransport_dv
void dIntegrateTransport_dv(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< MatrixType1 > &Jin, const Eigen::MatrixBase< MatrixType2 > &Jout) const
Transport the Jacobian w.r.t. the generalized velocity evaluated at the integrated configuration q + ...
Definition: robot.hxx:78
robotoc::Robot::computeMJtJinv
void computeMJtJinv(const Eigen::MatrixBase< MatrixType1 > &M, const Eigen::MatrixBase< MatrixType2 > &J, const Eigen::MatrixBase< MatrixType3 > &MJtJinv)
Computes the inverse of the contact dynamics matrix [[M J^T], [J O]].
Definition: robot.hxx:642
robotoc::Robot::computeMinv
void computeMinv(const Eigen::MatrixBase< MatrixType1 > &M, const Eigen::MatrixBase< MatrixType2 > &Minv)
Computes the inverse of the joint inertia matrix M.
Definition: robot.hxx:628
robotoc::Robot::frameRotation
const Eigen::Matrix3d & frameRotation(const int frame_id) const
Returns the rotation matrix of the frame. Before calling this function, updateKinematics() or updat...
robotoc::Robot::computeContactPositionDerivative
void computeContactPositionDerivative(const ImpactStatus &impact_status, const Eigen::MatrixBase< MatrixType > &position_partial_dq)
Computes the partial derivative of the contact position at the impact. Before calling this function,...
Definition: robot.hxx:464
robotoc::Robot::updateFrameKinematics
void updateFrameKinematics(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Updates the frame kinematics of the robot. The frame placements, velocities, and accelerations are ca...
Definition: robot.hxx:212
robotoc::Robot::dIntegrateTransport_dq
void dIntegrateTransport_dq(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< MatrixType1 > &Jin, const Eigen::MatrixBase< MatrixType2 > &Jout) const
Transport the Jacobian w.r.t. the configuration evaluated at the integrated configuration q + v to q.
Definition: robot.hxx:59
robotoc::Robot::integrateCoeffWiseJacobian
void integrateCoeffWiseJacobian(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< MatrixType > &J) const
Transforms the Jacobian w.r.t. configuration to tangentspace.
Definition: robot.hxx:159
robotoc::Robot::computeBaumgarteResidual
void computeBaumgarteResidual(const ContactStatus &contact_status, const Eigen::MatrixBase< VectorType > &baumgarte_residual)
Computes the residual of the contact constriants represented by Baumgarte's stabilization method....
Definition: robot.hxx:291
robotoc::Robot::transformFromLocalToWorld
void transformFromLocalToWorld(const int frame_id, const Eigen::Vector3d &vec_local, const Eigen::MatrixBase< Vector3dType > &vec_world) const
Transforms 3D quantity from local frame to the world frame.
Definition: robot.hxx:265
robotoc::Robot::computeContactPositionResidual
void computeContactPositionResidual(const ImpactStatus &impact_status, const Eigen::MatrixBase< VectorType > &position_residual)
Computes the residual of the contact position constraint at the impact. Before calling this function,...
Definition: robot.hxx:434
robotoc::Robot::RNEAImpactDerivatives
void RNEAImpactDerivatives(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &dv, const Eigen::MatrixBase< MatrixType1 > &dRNEA_partial_dq, const Eigen::MatrixBase< MatrixType2 > &dRNEA_partial_ddv)
Computes the partial dervatives of the function of impuse dynamics with respect to configuration and ...
Definition: robot.hxx:604
robotoc::Robot::getFrameJacobian
void getFrameJacobian(const int frame_id, const Eigen::MatrixBase< MatrixType > &J)
Computes the Jacobian of the frame position expressed in the local coordinate. Before calling this fu...
Definition: robot.hxx:247
robotoc::Robot::updateKinematics
void updateKinematics(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Updates the kinematics of the robot. The frame placements, frame velocity, frame acceleration,...
Definition: robot.hxx:172
robotoc::Robot::getCoMJacobian
void getCoMJacobian(const Eigen::MatrixBase< MatrixType > &J) const
Gets the Jacobian of the position of the center of mass. Before calling this function,...
Definition: robot.hxx:257
robotoc::Robot::RNEAImpact
void RNEAImpact(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &dv, const Eigen::MatrixBase< TangentVectorType2 > &res)
Computes the residual of the impact dynamics for given configuration and impact change in the general...
Definition: robot.hxx:589
robotoc::Robot::computeBaumgarteDerivatives
void computeBaumgarteDerivatives(const ContactStatus &contact_status, const Eigen::MatrixBase< MatrixType1 > &baumgarte_partial_dq, const Eigen::MatrixBase< MatrixType2 > &baumgarte_partial_dv, const Eigen::MatrixBase< MatrixType3 > &baumgarte_partial_da)
Computes the partial derivatives of the contact constriants represented by the Baumgarte's stabilizat...
Definition: robot.hxx:321
robotoc::Robot::normalizeConfiguration
void normalizeConfiguration(const Eigen::MatrixBase< ConfigVectorType > &q) const
Normalizes a configuration vector.
Definition: robot.hxx:688
robotoc::Robot::dSubtractConfiguration_dqf
void dSubtractConfiguration_dqf(const Eigen::MatrixBase< ConfigVectorType1 > &qf, const Eigen::MatrixBase< ConfigVectorType2 > &q0, const Eigen::MatrixBase< MatrixType > &dqdiff_dqf) const
Computes the partial derivative of the function of subtraction qf - q0 w.r.t. the terminal configurat...
Definition: robot.hxx:112
robotoc::Robot::interpolateConfiguration
void interpolateConfiguration(const Eigen::MatrixBase< ConfigVectorType1 > &q1, const Eigen::MatrixBase< ConfigVectorType2 > &q2, const double t, const Eigen::MatrixBase< ConfigVectorType3 > &qout) const
Computes an interpolation of two configurations by (1.0 - t) * s1 + t * s2.
Definition: robot.hxx:144
robotoc
Definition: constraint_component_base.hpp:17
robotoc::RobotModelInfo::contact_inv_damping
double contact_inv_damping
Damping paramter in matrix inversion of the contact-consistent forward dynamics. 1e-12 works well for...
Definition: robot_model_info.hpp:95
robotoc::RobotModelInfo::base_joint_type
BaseJointType base_joint_type
Type of the base joint. Default is BaseJointType::FixedBase.
Definition: robot_model_info.hpp:78
robotoc::RobotProperties::generalized_momentum_bias
Eigen::VectorXd generalized_momentum_bias
Definition: robot_properties.hpp:31
robotoc::RobotProperties::has_generalized_momentum_bias
bool has_generalized_momentum_bias
Definition: robot_properties.hpp:32