Crocoddyl
 
Loading...
Searching...
No Matches
contact-invdyn.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021-2025, Heriot-Watt University, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_
10#define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_
11
12#include "crocoddyl/core/actuation-base.hpp"
13#include "crocoddyl/core/constraints/constraint-manager.hpp"
14#include "crocoddyl/core/costs/cost-sum.hpp"
15#include "crocoddyl/core/diff-action-base.hpp"
16#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
17#include "crocoddyl/multibody/data/contacts.hpp"
18#include "crocoddyl/multibody/fwd.hpp"
19#include "crocoddyl/multibody/states/multibody.hpp"
20
21namespace crocoddyl {
22
43template <typename _Scalar>
45 : public DifferentialActionModelAbstractTpl<_Scalar> {
46 public:
47 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 CROCODDYL_DERIVED_CAST(DifferentialActionModelBase,
50
51 typedef _Scalar Scalar;
64 typedef typename MathBase::VectorXs VectorXs;
65 typedef typename MathBase::MatrixXs MatrixXs;
66
79 std::shared_ptr<StateMultibody> state,
80 std::shared_ptr<ActuationModelAbstract> actuation,
81 std::shared_ptr<ContactModelMultiple> contacts,
82 std::shared_ptr<CostModelSum> costs);
83
94 std::shared_ptr<StateMultibody> state,
95 std::shared_ptr<ActuationModelAbstract> actuation,
96 std::shared_ptr<ContactModelMultiple> contacts,
97 std::shared_ptr<CostModelSum> costs,
98 std::shared_ptr<ConstraintModelManager> constraints);
100
111 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
112 const Eigen::Ref<const VectorXs>& x,
113 const Eigen::Ref<const VectorXs>& u) override;
114
120 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
121 const Eigen::Ref<const VectorXs>& x) override;
122
136 virtual void calcDiff(
137 const std::shared_ptr<DifferentialActionDataAbstract>& data,
138 const Eigen::Ref<const VectorXs>& x,
139 const Eigen::Ref<const VectorXs>& u) override;
140
146 virtual void calcDiff(
147 const std::shared_ptr<DifferentialActionDataAbstract>& data,
148 const Eigen::Ref<const VectorXs>& x) override;
149
155 virtual std::shared_ptr<DifferentialActionDataAbstract> createData() override;
156
166 template <typename NewScalar>
168
173 virtual bool checkData(
174 const std::shared_ptr<DifferentialActionDataAbstract>& data) override;
175
189 virtual void quasiStatic(
190 const std::shared_ptr<DifferentialActionDataAbstract>& data,
191 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
192 const std::size_t maxiter = 100,
193 const Scalar tol = Scalar(1e-9)) override;
194
198 virtual std::size_t get_ng() const override;
199
203 virtual std::size_t get_nh() const override;
204
208 virtual std::size_t get_ng_T() const override;
209
213 virtual std::size_t get_nh_T() const override;
214
218 virtual const VectorXs& get_g_lb() const override;
219
223 virtual const VectorXs& get_g_ub() const override;
224
228 const std::shared_ptr<ActuationModelAbstract>& get_actuation() const;
229
233 const std::shared_ptr<ContactModelMultiple>& get_contacts() const;
234
238 const std::shared_ptr<CostModelSum>& get_costs() const;
239
243 const std::shared_ptr<ConstraintModelManager>& get_constraints() const;
244
248 pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
249
254 virtual void print(std::ostream& os) const override;
255
256 protected:
257 using Base::g_lb_;
258 using Base::g_ub_;
259 using Base::ng_;
260 using Base::nh_;
261 using Base::nu_;
262 using Base::state_;
263
264 private:
265 void init(const std::shared_ptr<StateMultibody>& state);
266 std::shared_ptr<ActuationModelAbstract> actuation_;
267 std::shared_ptr<ContactModelMultiple> contacts_;
268 std::shared_ptr<CostModelSum> costs_;
269 std::shared_ptr<ConstraintModelManager> constraints_;
270 pinocchio::ModelTpl<Scalar>* pinocchio_;
271
272 public:
287 public:
288 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
289 CROCODDYL_INNER_DERIVED_CAST(ResidualModelBase,
292
293 typedef _Scalar Scalar;
299 typedef typename MathBase::VectorXs VectorXs;
300
308 ResidualModelActuation(std::shared_ptr<StateMultibody> state,
309 const std::size_t nu, const std::size_t nc)
310 : Base(state, state->get_nv() - nu, state->get_nv() + nc, true, true,
311 true),
312 na_(nu),
313 nc_(nc) {}
314 virtual ~ResidualModelActuation() = default;
315
323 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
324 const Eigen::Ref<const VectorXs>&,
325 const Eigen::Ref<const VectorXs>&) override {
326 typename Data::ResidualDataActuation* d =
327 static_cast<typename Data::ResidualDataActuation*>(data.get());
328 // Update the under-actuation set and residual
329 std::size_t nrow = 0;
330 for (std::size_t k = 0;
331 k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
332 if (!d->actuation->tau_set[k]) {
333 data->r(nrow) = d->pinocchio->tau(k);
334 nrow += 1;
335 }
336 }
337 }
338
343 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
344 const Eigen::Ref<const VectorXs>&) override {
345 data->r.setZero();
346 }
347
355 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
356 const Eigen::Ref<const VectorXs>&,
357 const Eigen::Ref<const VectorXs>&) override {
358 typename Data::ResidualDataActuation* d =
359 static_cast<typename Data::ResidualDataActuation*>(data.get());
360 std::size_t nrow = 0;
361 const std::size_t nv = state_->get_nv();
362 d->dtau_dx.leftCols(nv) = d->pinocchio->dtau_dq;
363 d->dtau_dx.rightCols(nv) = d->pinocchio->dtau_dv;
364 d->dtau_dx -= d->actuation->dtau_dx;
365 d->dtau_du.leftCols(nv) = d->pinocchio->M;
366 d->dtau_du.rightCols(nc_) = -d->contact->Jc.transpose();
367 for (std::size_t k = 0;
368 k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
369 if (!d->actuation->tau_set[k]) {
370 d->Rx.row(nrow) = d->dtau_dx.row(k);
371 d->Ru.row(nrow) = d->dtau_du.row(k);
372 nrow += 1;
373 }
374 }
375 }
376
382 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
383 const Eigen::Ref<const VectorXs>&) override {
384 data->Rx.setZero();
385 data->Ru.setZero();
386 }
387
393 virtual std::shared_ptr<ResidualDataAbstract> createData(
394 DataCollectorAbstract* const data) override {
395 return std::allocate_shared<typename Data::ResidualDataActuation>(
396 Eigen::aligned_allocator<typename Data::ResidualDataActuation>(),
397 this, data);
398 }
399
411 template <typename NewScalar>
413 NewScalar>::ResidualModelActuation
414 cast() const {
416 NewScalar>::ResidualModelActuation ReturnType;
417 typedef StateMultibodyTpl<NewScalar> StateType;
418 ReturnType ret(std::static_pointer_cast<StateType>(
419 state_->template cast<NewScalar>()),
420 na_, nc_);
421 return ret;
422 }
423
429 virtual void print(std::ostream& os) const override {
430 os << "ResidualModelActuation {nx=" << state_->get_nx()
431 << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << ", na=" << na_
432 << "}";
433 }
434
435 protected:
436 using Base::nu_;
437 using Base::state_;
438
439 private:
440 std::size_t na_;
441 std::size_t nc_;
442 };
443
444 public:
461 public:
462 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
463 CROCODDYL_INNER_DERIVED_CAST(ResidualModelBase,
466
467 typedef _Scalar Scalar;
473 typedef typename MathBase::VectorXs VectorXs;
474 typedef typename MathBase::MatrixXs MatrixXs;
475
484 ResidualModelContact(std::shared_ptr<StateMultibody> state,
485 const pinocchio::FrameIndex id, const std::size_t nr,
486 const std::size_t nc)
487 : Base(state, nr, state->get_nv() + nc, true, true, true),
488 id_(id),
489 frame_name_(state->get_pinocchio()->frames[id].name),
490 nc_(nc) {}
491 virtual ~ResidualModelContact() = default;
492
500 void calc(const std::shared_ptr<ResidualDataAbstract>& data,
501 const Eigen::Ref<const VectorXs>&,
502 const Eigen::Ref<const VectorXs>&) override {
503 typename Data::ResidualDataContact* d =
504 static_cast<typename Data::ResidualDataContact*>(data.get());
505 d->r = d->contact->a0;
506 }
507
512 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
513 const Eigen::Ref<const VectorXs>&) override {
514 data->r.setZero();
515 }
516
524 void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
525 const Eigen::Ref<const VectorXs>&,
526 const Eigen::Ref<const VectorXs>&) override {
527 typename Data::ResidualDataContact* d =
528 static_cast<typename Data::ResidualDataContact*>(data.get());
529 d->Rx = d->contact->da0_dx;
530 d->Ru.leftCols(state_->get_nv()) = d->contact->Jc;
531 }
532
538 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
539 const Eigen::Ref<const VectorXs>&) override {
540 data->Rx.setZero();
541 data->Ru.setZero();
542 }
543
549 virtual std::shared_ptr<ResidualDataAbstract> createData(
550 DataCollectorAbstract* const data) override {
551 return std::allocate_shared<typename Data::ResidualDataContact>(
552 Eigen::aligned_allocator<typename Data::ResidualDataContact>(), this,
553 data, id_);
554 }
555
567 template <typename NewScalar>
569 NewScalar>::ResidualModelContact
570 cast() const {
572 NewScalar>::ResidualModelContact ReturnType;
573 typedef StateMultibodyTpl<NewScalar> StateType;
574 ReturnType ret(std::static_pointer_cast<StateType>(
575 state_->template cast<NewScalar>()),
576 id_, nr_, nc_);
577 return ret;
578 }
579
586 virtual void print(std::ostream& os) const override {
587 os << "ResidualModelContact {frame=" << frame_name_ << ", nr=" << nr_
588 << "}";
589 }
590
591 protected:
592 using Base::nr_;
593 using Base::nu_;
594 using Base::state_;
595
596 private:
597 pinocchio::FrameIndex id_;
598 std::string frame_name_;
599 std::size_t nc_;
600 };
601};
602template <typename _Scalar>
604 : public DifferentialActionDataAbstractTpl<_Scalar> {
605 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
606 typedef _Scalar Scalar;
616 typedef typename MathBase::VectorXs VectorXs;
617 typedef typename MathBase::MatrixXs MatrixXs;
618
619 template <template <typename Scalar> class Model>
621 Model<Scalar>* const model)
622 : Base(model),
623 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
624 multibody(
625 &pinocchio, model->get_actuation()->createData(),
626 std::make_shared<JointDataAbstract>(
627 model->get_state(), model->get_actuation(), model->get_nu()),
628 model->get_contacts()->createData(&pinocchio)),
629 tmp_xstatic(model->get_state()->get_nx()),
630 tmp_rstatic(model->get_actuation()->get_nu() +
631 model->get_contacts()->get_nc()),
632 tmp_Jstatic(model->get_state()->get_nv(),
633 model->get_actuation()->get_nu() +
634 model->get_contacts()->get_nc()) {
635 // Set constant values for Fu, df_dx, and df_du
636 const std::size_t nv = model->get_state()->get_nv();
637 const std::size_t nc = model->get_contacts()->get_nc_total();
638 Fu.leftCols(nv).diagonal().setOnes();
639 multibody.joint->da_du.leftCols(nv).diagonal().setOnes();
640 MatrixXs df_dx = MatrixXs::Zero(nc, model->get_state()->get_ndx());
641 MatrixXs df_du = MatrixXs::Zero(nc, model->get_nu());
642 std::size_t fid = 0;
643 for (typename ContactModelMultiple::ContactModelContainer::const_iterator
644 it = model->get_contacts()->get_contacts().begin();
645 it != model->get_contacts()->get_contacts().end(); ++it) {
646 const std::size_t nc = it->second->contact->get_nc();
647 df_du.block(fid, nv + fid, nc, nc).diagonal().setOnes();
648 fid += nc;
649 }
650 std::vector<bool> contact_status;
651 for (typename ContactModelMultiple::ContactModelContainer::const_iterator
652 it = model->get_contacts()->get_contacts().begin();
653 it != model->get_contacts()->get_contacts().end(); ++it) {
654 const std::shared_ptr<ContactItem>& m_i = it->second;
655 contact_status.push_back(m_i->active);
656 m_i->active = true;
657 }
658 model->get_contacts()->updateForceDiff(multibody.contacts, df_dx, df_du);
659 std::size_t cid = 0;
660 for (typename ContactModelMultiple::ContactModelContainer::const_iterator
661 it = model->get_contacts()->get_contacts().begin();
662 it != model->get_contacts()->get_contacts().end(); ++it) {
663 const std::shared_ptr<ContactItem>& m_i = it->second;
664 m_i->active = contact_status[cid];
665 cid++;
666 }
667 costs = model->get_costs()->createData(&multibody);
668 constraints = model->get_constraints()->createData(&multibody);
669 costs->shareMemory(this);
670 constraints->shareMemory(this);
671
672 tmp_xstatic.setZero();
673 tmp_rstatic.setZero();
674 tmp_Jstatic.setZero();
675 }
677
678 pinocchio::DataTpl<Scalar> pinocchio;
680 std::shared_ptr<CostDataSum> costs;
681 std::shared_ptr<ConstraintDataManager> constraints;
682 VectorXs
684 VectorXs
686 MatrixXs tmp_Jstatic;
687
688 using Base::cost;
689 using Base::Fu;
690 using Base::Fx;
691 using Base::Lu;
692 using Base::Luu;
693 using Base::Lx;
694 using Base::Lxu;
695 using Base::Lxx;
696 using Base::r;
697 using Base::xout;
698
700 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
701
702 typedef _Scalar Scalar;
710 typedef typename MathBase::MatrixXs MatrixXs;
711
712 template <template <typename Scalar> class Model>
713 ResidualDataActuation(Model<Scalar>* const model,
714 DataCollectorAbstract* const data)
715 : Base(model, data),
716 dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
717 dtau_du(model->get_state()->get_nv(), model->get_nu()) {
718 // Check that proper shared data has been passed
721 if (d == NULL) {
722 throw_pretty(
723 "Invalid argument: the shared data should be derived from "
724 "DataCollectorActMultibodyInContact");
725 }
726 // Avoids data casting at runtime
727 pinocchio = d->pinocchio;
728 actuation = d->actuation;
729 contact = d->contacts;
730 dtau_dx.setZero();
731 dtau_du.setZero();
732 }
733 virtual ~ResidualDataActuation() = default;
734
735 pinocchio::DataTpl<Scalar>* pinocchio;
736 std::shared_ptr<ActuationDataAbstract> actuation;
737 std::shared_ptr<ContactDataMultiple> contact;
738 MatrixXs dtau_dx;
739 MatrixXs dtau_du;
740 using Base::r;
741 using Base::Ru;
742 using Base::Rx;
743 using Base::shared;
744 };
745
747 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
748
749 typedef _Scalar Scalar;
756
757 template <template <typename Scalar> class Model>
758 ResidualDataContact(Model<Scalar>* const model,
759 DataCollectorAbstract* const data, const std::size_t id)
760 : Base(model, data) {
763 if (d == NULL) {
764 throw_pretty(
765 "Invalid argument: the shared data should be derived from "
766 "DataCollectorMultibodyInContact");
767 }
768 typename ContactModelMultiple::ContactDataContainer::iterator it, end;
769 for (it = d->contacts->contacts.begin(),
770 end = d->contacts->contacts.end();
771 it != end; ++it) {
772 if (id == it->second->frame) { // TODO(cmastalli): use model->get_id()
773 // and avoid to pass id in constructor
774 contact = it->second.get();
775 break;
776 }
777 }
778 }
779 virtual ~ResidualDataContact() = default;
780
782 using Base::r;
783 using Base::Ru;
784 using Base::Rx;
785 using Base::shared;
786 };
787};
788} // namespace crocoddyl
789
790/* --- Details -------------------------------------------------------------- */
791/* --- Details -------------------------------------------------------------- */
792/* --- Details -------------------------------------------------------------- */
793#include <crocoddyl/multibody/actions/contact-invdyn.hxx>
794
795CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
797CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
799
800#endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_
Abstract class for the actuation-mapping model.
Manage the individual constraint models.
Define a stack of contact models.
Summation of individual cost models.
Definition cost-sum.hpp:75
Abstract class for differential action model.
std::shared_ptr< StateAbstract > state_
Model of the state.
VectorXs g_ub_
Lower bound of the inequality constraints.
std::size_t nh_
Number of equality constraints.
VectorXs g_lb_
Lower bound of the inequality constraints.
std::size_t ng_
Number of inequality constraints.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &) override
ResidualModelActuation(std::shared_ptr< StateMultibody > state, const std::size_t nu, const std::size_t nc)
Initialize the actuation residual model.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &) override
Compute the residual vector for nodes that depends only on the state.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the derivatives of the actuation residual.
DifferentialActionModelContactInvDynamicsTpl< NewScalar >::ResidualModelActuation cast() const
Cast the actuation-residual model to a different scalar type.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the actuation residual.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the actuation residual data.
virtual void print(std::ostream &os) const override
Print relevant information of the actuation residual model.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &) override
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &) override
Compute the residual vector for nodes that depends only on the state.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the contact-acceleration residual data.
ResidualModelContact(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const std::size_t nr, const std::size_t nc)
Initialize the contact-acceleration residual model.
void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the derivatives of the contact-acceleration residual.
DifferentialActionModelContactInvDynamicsTpl< NewScalar >::ResidualModelContact cast() const
Cast the contact-residual model to a different scalar type.
void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the contact-acceleration residual.
virtual void print(std::ostream &os) const override
Print relevant information of the contact-acceleration residual model.
Differential action model for contact inverse dynamics in multibody systems.
virtual bool checkData(const std::shared_ptr< DifferentialActionDataAbstract > &data) override
Checks that a specific data belongs to the contact inverse-dynamics model.
DifferentialActionModelContactInvDynamicsTpl(std::shared_ptr< StateMultibody > state, std::shared_ptr< ActuationModelAbstract > actuation, std::shared_ptr< ContactModelMultiple > contacts, std::shared_ptr< CostModelSum > costs)
Initialize the contact inverse-dynamics action model.
virtual const VectorXs & get_g_lb() const override
Return the lower bound of the inequality constraints.
const std::shared_ptr< ConstraintModelManager > & get_constraints() const
Return the constraint model manager.
virtual void calcDiff(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
virtual const VectorXs & get_g_ub() const override
Return the upper bound of the inequality constraints.
virtual void calc(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the system acceleration, cost value and constraint residuals.
virtual std::shared_ptr< DifferentialActionDataAbstract > createData() override
Create the contact inverse-dynamics data.
virtual std::size_t get_ng() const override
Return the number of inequality constraints.
DifferentialActionModelContactInvDynamicsTpl(std::shared_ptr< StateMultibody > state, std::shared_ptr< ActuationModelAbstract > actuation, std::shared_ptr< ContactModelMultiple > contacts, std::shared_ptr< CostModelSum > costs, std::shared_ptr< ConstraintModelManager > constraints)
Initialize the contact inverse-dynamics action model.
DifferentialActionModelContactInvDynamicsTpl< NewScalar > cast() const
Cast the contact-invdyn model to a different scalar type.
virtual std::size_t get_nh_T() const override
Return the number of equality terminal constraints.
virtual std::size_t get_nh() const override
Return the number of equality constraints.
const std::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
virtual std::size_t get_ng_T() const override
Return the number of equality terminal constraints.
virtual void calc(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
const std::shared_ptr< ActuationModelAbstract > & get_actuation() const
Return the actuation model.
virtual void quasiStatic(const std::shared_ptr< DifferentialActionDataAbstract > &data, Eigen::Ref< VectorXs > u, const Eigen::Ref< const VectorXs > &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9)) override
Computes the quasic static commands.
const std::shared_ptr< ContactModelMultiple > & get_contacts() const
Return the contact model.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
virtual void print(std::ostream &os) const override
Print relevant information of the contact inverse-dynamics model.
virtual void calcDiff(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the dynamics, cost and constraint functions.
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
std::size_t nr_
Residual vector dimension.
State multibody representation.
Definition multibody.hpp:34
Define the multi-contact data.
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs Lx
Jacobian of the cost w.r.t. the state .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .
VectorXs Lu
Jacobian of the cost w.r.t. the control .
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
std::shared_ptr< ActuationDataAbstract > actuation
Actuation data.
DataCollectorAbstract * shared
Shared data allocated by the action model.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
ContactDataAbstractTpl< Scalar > * contact
Contact force data.
DataCollectorAbstract * shared
Shared data allocated by the action model.
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
DataCollectorJointActMultibodyInContact multibody
Multibody data.
std::shared_ptr< ConstraintDataManager > constraints
Constraints data.
VectorXs tmp_xstatic
State point used for computing the quasi-static input.
VectorXs tmp_rstatic
Factorization used for computing the quasi-static input.
pinocchio::DataTpl< Scalar > pinocchio
Pinocchio data.
std::shared_ptr< CostDataSum > costs
Costs data.
MatrixXs tmp_Jstatic
Jacobian used for computing the quasi-static input.
MatrixXs Jc
Contact Jacobian.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.