9#ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_
10#define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_INVDYN_HPP_
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"
43template <
typename _Scalar>
47 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 typedef _Scalar Scalar;
64 typedef typename MathBase::VectorXs VectorXs;
65 typedef typename MathBase::MatrixXs MatrixXs;
79 std::shared_ptr<StateMultibody> state,
80 std::shared_ptr<ActuationModelAbstract> actuation,
81 std::shared_ptr<ContactModelMultiple> contacts,
82 std::shared_ptr<CostModelSum> costs);
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);
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;
120 virtual void calc(
const std::shared_ptr<DifferentialActionDataAbstract>& data,
121 const Eigen::Ref<const VectorXs>& x)
override;
137 const std::shared_ptr<DifferentialActionDataAbstract>& data,
138 const Eigen::Ref<const VectorXs>& x,
139 const Eigen::Ref<const VectorXs>& u)
override;
147 const std::shared_ptr<DifferentialActionDataAbstract>& data,
148 const Eigen::Ref<const VectorXs>& x)
override;
155 virtual std::shared_ptr<DifferentialActionDataAbstract>
createData()
override;
166 template <
typename NewScalar>
174 const std::shared_ptr<DifferentialActionDataAbstract>& data)
override;
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;
198 virtual std::size_t
get_ng()
const override;
203 virtual std::size_t
get_nh()
const override;
254 virtual void print(std::ostream& os)
const override;
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_;
288 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
293 typedef _Scalar Scalar;
299 typedef typename MathBase::VectorXs VectorXs;
309 const std::size_t nu,
const std::size_t nc)
310 :
Base(state, state->get_nv() - nu, state->get_nv() + nc, true, true,
323 virtual void calc(
const std::shared_ptr<ResidualDataAbstract>& data,
324 const Eigen::Ref<const VectorXs>&,
325 const Eigen::Ref<const VectorXs>&)
override {
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) {
343 virtual void calc(
const std::shared_ptr<ResidualDataAbstract>& data,
344 const Eigen::Ref<const VectorXs>&)
override {
355 virtual void calcDiff(
const std::shared_ptr<ResidualDataAbstract>& data,
356 const Eigen::Ref<const VectorXs>&,
357 const Eigen::Ref<const VectorXs>&)
override {
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;
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) {
370 d->
Rx.row(nrow) = d->dtau_dx.row(k);
371 d->
Ru.row(nrow) = d->dtau_du.row(k);
382 virtual void calcDiff(
const std::shared_ptr<ResidualDataAbstract>& data,
383 const Eigen::Ref<const VectorXs>&)
override {
395 return std::allocate_shared<typename Data::ResidualDataActuation>(
396 Eigen::aligned_allocator<typename Data::ResidualDataActuation>(),
411 template <
typename NewScalar>
413 NewScalar>::ResidualModelActuation
416 NewScalar>::ResidualModelActuation ReturnType;
418 ReturnType ret(std::static_pointer_cast<StateType>(
419 state_->template cast<NewScalar>()),
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_
462 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
467 typedef _Scalar Scalar;
473 typedef typename MathBase::VectorXs VectorXs;
474 typedef typename MathBase::MatrixXs MatrixXs;
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),
500 void calc(
const std::shared_ptr<ResidualDataAbstract>& data,
501 const Eigen::Ref<const VectorXs>&,
502 const Eigen::Ref<const VectorXs>&)
override {
512 virtual void calc(
const std::shared_ptr<ResidualDataAbstract>& data,
513 const Eigen::Ref<const VectorXs>&)
override {
524 void calcDiff(
const std::shared_ptr<ResidualDataAbstract>& data,
525 const Eigen::Ref<const VectorXs>&,
526 const Eigen::Ref<const VectorXs>&)
override {
538 virtual void calcDiff(
const std::shared_ptr<ResidualDataAbstract>& data,
539 const Eigen::Ref<const VectorXs>&)
override {
551 return std::allocate_shared<typename Data::ResidualDataContact>(
552 Eigen::aligned_allocator<typename Data::ResidualDataContact>(),
this,
567 template <
typename NewScalar>
569 NewScalar>::ResidualModelContact
572 NewScalar>::ResidualModelContact ReturnType;
574 ReturnType ret(std::static_pointer_cast<StateType>(
575 state_->template cast<NewScalar>()),
586 virtual void print(std::ostream& os)
const override {
587 os <<
"ResidualModelContact {frame=" << frame_name_ <<
", nr=" <<
nr_
597 pinocchio::FrameIndex id_;
598 std::string frame_name_;
602template <
typename _Scalar>
605 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
606 typedef _Scalar Scalar;
616 typedef typename MathBase::VectorXs VectorXs;
617 typedef typename MathBase::MatrixXs MatrixXs;
619 template <
template <
typename Scalar>
class Model>
621 Model<Scalar>*
const model)
623 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
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)),
631 model->get_contacts()->get_nc()),
633 model->get_actuation()->get_nu() +
634 model->get_contacts()->get_nc()) {
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());
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();
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);
658 model->get_contacts()->updateForceDiff(
multibody.contacts, df_dx, df_du);
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];
669 costs->shareMemory(
this);
700 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
702 typedef _Scalar Scalar;
710 typedef typename MathBase::MatrixXs MatrixXs;
712 template <
template <
typename Scalar>
class Model>
716 dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
717 dtau_du(model->get_state()->get_nv(), model->get_nu()) {
723 "Invalid argument: the shared data should be derived from "
724 "DataCollectorActMultibodyInContact");
747 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
749 typedef _Scalar Scalar;
757 template <
template <
typename Scalar>
class Model>
760 :
Base(model, data) {
765 "Invalid argument: the shared data should be derived from "
766 "DataCollectorMultibodyInContact");
768 typename ContactModelMultiple::ContactDataContainer::iterator it, end;
769 for (it = d->contacts->contacts.begin(),
770 end = d->contacts->contacts.end();
772 if (
id == it->second->frame) {
793#include <crocoddyl/multibody/actions/contact-invdyn.hxx>
795CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
797CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
Abstract class for the actuation-mapping model.
Manage the individual constraint models.
Summation of individual cost models.
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 nu_
Control dimension.
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.
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.
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 .
VectorXs xout
evolution state
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.
VectorXs r
Residual vector.