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" 58 template <
typename _Scalar>
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 typedef _Scalar Scalar;
72 typedef typename MathBase::VectorXs VectorXs;
73 typedef typename MathBase::MatrixXs MatrixXs;
89 boost::shared_ptr<ActuationModelAbstract> actuation,
90 boost::shared_ptr<ContactModelMultiple> contacts,
91 boost::shared_ptr<CostModelSum> costs,
92 const Scalar JMinvJt_damping = Scalar(0.),
93 const bool enable_force =
false);
105 virtual void calc(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
106 const Eigen::Ref<const VectorXs>& u);
118 virtual void calc(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
119 const Eigen::Ref<const VectorXs>& x);
128 virtual void calcDiff(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
129 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u);
141 virtual void calcDiff(
const boost::shared_ptr<DifferentialActionDataAbstract>& data,
142 const Eigen::Ref<const VectorXs>& x);
149 virtual boost::shared_ptr<DifferentialActionDataAbstract>
createData();
154 virtual bool checkData(
const boost::shared_ptr<DifferentialActionDataAbstract>& data);
159 virtual void quasiStatic(
const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
160 const Eigen::Ref<const VectorXs>& x,
const std::size_t maxiter = 100,
161 const Scalar tol = Scalar(1e-9));
166 const boost::shared_ptr<ActuationModelAbstract>&
get_actuation()
const;
171 const boost::shared_ptr<ContactModelMultiple>&
get_contacts()
const;
176 const boost::shared_ptr<CostModelSum>&
get_costs()
const;
208 virtual void print(std::ostream& os)
const;
215 boost::shared_ptr<ActuationModelAbstract> actuation_;
216 boost::shared_ptr<ContactModelMultiple> contacts_;
217 boost::shared_ptr<CostModelSum> costs_;
218 pinocchio::ModelTpl<Scalar>& pinocchio_;
221 Scalar JMinvJt_damping_;
225 template <
typename _Scalar>
227 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
228 typedef _Scalar Scalar;
231 typedef typename MathBase::VectorXs VectorXs;
232 typedef typename MathBase::MatrixXs MatrixXs;
234 template <
template <
typename Scalar>
class Model>
237 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
238 multibody(&pinocchio, model->get_actuation()->createData(), model->get_contacts()->createData(&pinocchio)),
239 costs(model->get_costs()->createData(&multibody)),
240 Kinv(model->get_state()->get_nv() + model->get_contacts()->get_nc_total(),
241 model->get_state()->get_nv() + model->get_contacts()->get_nc_total()),
242 df_dx(model->get_contacts()->get_nc_total(), model->get_state()->get_ndx()),
243 df_du(model->get_contacts()->get_nc_total(), model->get_nu()),
244 tmp_xstatic(model->get_state()->get_nx()),
245 tmp_Jstatic(model->get_state()->get_nv(), model->get_nu() + model->get_contacts()->get_nc_total()) {
246 costs->shareMemory(
this);
250 tmp_xstatic.setZero();
251 tmp_Jstatic.setZero();
252 pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
253 pinocchio.lambda_c.setZero();
256 pinocchio::DataTpl<Scalar> pinocchio;
258 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
262 VectorXs tmp_xstatic;
263 MatrixXs tmp_Jstatic;
282 #include <crocoddyl/multibody/actions/contact-fwddyn.hxx> 284 #endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_ VectorXs Lx
Jacobian of the cost function.
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Lxx
Hessian of the cost function.
Abstract class for differential action model.
MatrixXs Luu
Hessian of the cost function.
State multibody representation.
std::size_t nu_
Control dimension.
MatrixXs Lxu
Hessian of the cost function.
boost::shared_ptr< StateAbstract > state_
Model of the state.
MatrixXs Fu
Jacobian of the dynamics.
VectorXs xout
evolution state
Abstract class for the actuation-mapping model.
VectorXs Lu
Jacobian of the cost function.