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_