9 #ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_ 10 #define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_ 14 #include "crocoddyl/multibody/fwd.hpp" 15 #include "crocoddyl/core/diff-action-base.hpp" 16 #include "crocoddyl/core/costs/cost-sum.hpp" 17 #include "crocoddyl/multibody/states/multibody.hpp" 18 #include "crocoddyl/core/actuation-base.hpp" 19 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp" 20 #include "crocoddyl/multibody/data/contacts.hpp" 24 template <
typename _Scalar>
27 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
29 typedef _Scalar Scalar;
38 typedef typename MathBase::VectorXs VectorXs;
39 typedef typename MathBase::MatrixXs MatrixXs;
42 boost::shared_ptr<ActuationModelAbstract> actuation,
43 boost::shared_ptr<ContactModelMultiple> contacts,
44 boost::shared_ptr<CostModelSum> costs,
45 const Scalar JMinvJt_damping = Scalar(0.),
46 const bool enable_force =
false);
49 virtual void calc(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
50 const Eigen::Ref<const VectorXs>& u);
51 virtual void calcDiff(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
52 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u);
53 virtual boost::shared_ptr<DifferentialActionDataAbstract>
createData();
54 virtual bool checkData(
const boost::shared_ptr<DifferentialActionDataAbstract>& data);
55 virtual void quasiStatic(
const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
56 const Eigen::Ref<const VectorXs>& x,
const std::size_t maxiter = 100,
57 const Scalar tol = Scalar(1e-9));
59 const boost::shared_ptr<ActuationModelAbstract>& get_actuation()
const;
60 const boost::shared_ptr<ContactModelMultiple>& get_contacts()
const;
61 const boost::shared_ptr<CostModelSum>& get_costs()
const;
62 pinocchio::ModelTpl<Scalar>& get_pinocchio()
const;
63 const VectorXs& get_armature()
const;
64 const Scalar get_damping_factor()
const;
66 void set_armature(
const VectorXs& armature);
67 void set_damping_factor(
const Scalar damping);
72 template <
class Scalar>
73 friend std::ostream& operator<<(std::ostream& os, const DifferentialActionModelContactFwdDynamicsTpl<Scalar>& model);
85 boost::shared_ptr<ActuationModelAbstract> actuation_;
86 boost::shared_ptr<ContactModelMultiple> contacts_;
87 boost::shared_ptr<CostModelSum> costs_;
88 pinocchio::ModelTpl<Scalar>& pinocchio_;
91 Scalar JMinvJt_damping_;
95 template <
typename _Scalar>
97 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
98 typedef _Scalar Scalar;
101 typedef typename MathBase::VectorXs VectorXs;
102 typedef typename MathBase::MatrixXs MatrixXs;
104 template <
template <
typename Scalar>
class Model>
107 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
108 multibody(&pinocchio, model->get_actuation()->createData(), model->get_contacts()->createData(&pinocchio)),
109 costs(model->get_costs()->createData(&multibody)),
110 Kinv(model->get_state()->get_nv() + model->get_contacts()->get_nc_total(),
111 model->get_state()->get_nv() + model->get_contacts()->get_nc_total()),
112 df_dx(model->get_contacts()->get_nc_total(), model->get_state()->get_ndx()),
113 df_du(model->get_contacts()->get_nc_total(), model->get_nu()),
114 tmp_xstatic(model->get_state()->get_nx()),
115 tmp_Jstatic(model->get_state()->get_nv(), model->get_nu() + model->get_contacts()->get_nc_total()) {
116 costs->shareMemory(
this);
120 tmp_xstatic.setZero();
121 tmp_Jstatic.setZero();
122 pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
123 pinocchio.lambda_c.setZero();
126 pinocchio::DataTpl<Scalar> pinocchio;
128 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
132 VectorXs tmp_xstatic;
133 MatrixXs tmp_Jstatic;
152 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx> 154 #endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
VectorXs u_lb_
Lower control limits.
Abstract class for differential action model.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
State multibody representation.
std::size_t nu_
Control dimension.
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.
VectorXs u_ub_
Upper control limits.