crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
contact-fwddyn.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, CTU, INRIA, University of Oxford
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
10#define CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
11
12#include <stdexcept>
13
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"
21
22namespace crocoddyl {
23
58template <typename _Scalar>
60 public:
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62
63 typedef _Scalar Scalar;
72 typedef typename MathBase::VectorXs VectorXs;
73 typedef typename MathBase::MatrixXs MatrixXs;
74
88 DifferentialActionModelContactFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
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);
95
105 virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
106 const Eigen::Ref<const VectorXs>& u);
107
118 virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
119 const Eigen::Ref<const VectorXs>& x);
120
128 virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
129 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
130
141 virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
142 const Eigen::Ref<const VectorXs>& x);
143
149 virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
150
154 virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
155
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));
162
166 const boost::shared_ptr<ActuationModelAbstract>& get_actuation() const;
167
171 const boost::shared_ptr<ContactModelMultiple>& get_contacts() const;
172
176 const boost::shared_ptr<CostModelSum>& get_costs() const;
177
181 pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
182
186 const VectorXs& get_armature() const;
187
191 const Scalar get_damping_factor() const;
192
196 void set_armature(const VectorXs& armature);
197
201 void set_damping_factor(const Scalar damping);
202
208 virtual void print(std::ostream& os) const;
209
210 protected:
211 using Base::nu_;
212 using Base::state_;
213
214 private:
215 boost::shared_ptr<ActuationModelAbstract> actuation_;
216 boost::shared_ptr<ContactModelMultiple> contacts_;
217 boost::shared_ptr<CostModelSum> costs_;
218 pinocchio::ModelTpl<Scalar>& pinocchio_;
219 bool with_armature_;
220 VectorXs armature_;
221 Scalar JMinvJt_damping_;
222 bool enable_force_;
223};
224
225template <typename _Scalar>
227 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
228 typedef _Scalar Scalar;
231 typedef typename MathBase::VectorXs VectorXs;
232 typedef typename MathBase::MatrixXs MatrixXs;
233
234 template <template <typename Scalar> class Model>
235 explicit DifferentialActionDataContactFwdDynamicsTpl(Model<Scalar>* const model)
236 : Base(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);
247 Kinv.setZero();
248 df_dx.setZero();
249 df_du.setZero();
250 tmp_xstatic.setZero();
251 tmp_Jstatic.setZero();
252 pinocchio.lambda_c.resize(model->get_contacts()->get_nc_total());
253 pinocchio.lambda_c.setZero();
254 }
255
256 pinocchio::DataTpl<Scalar> pinocchio;
258 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
259 MatrixXs Kinv;
260 MatrixXs df_dx;
261 MatrixXs df_du;
262 VectorXs tmp_xstatic;
263 MatrixXs tmp_Jstatic;
264
265 using Base::cost;
266 using Base::Fu;
267 using Base::Fx;
268 using Base::Lu;
269 using Base::Luu;
270 using Base::Lx;
271 using Base::Lxu;
272 using Base::Lxx;
273 using Base::r;
274 using Base::xout;
275};
276
277} // namespace crocoddyl
278
279/* --- Details -------------------------------------------------------------- */
280/* --- Details -------------------------------------------------------------- */
281/* --- Details -------------------------------------------------------------- */
282#include <crocoddyl/multibody/actions/contact-fwddyn.hxx>
283
284#endif // CROCODDYL_MULTIBODY_ACTIONS_CONTACT_FWDDYN_HPP_
Abstract class for the actuation-mapping model.
Define a stack of contact models.
Summation of individual cost models.
Definition: cost-sum.hpp:65
Abstract class for differential action model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
Differential action model for contact forward dynamics in multibody systems.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on the state.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the contact forward-dynamics data.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the contact dynamics, and cost function.
virtual void print(std::ostream &os) const
Print relevant information of the contact forward-dynamics model.
const VectorXs & get_armature() const
Return the armature vector.
void set_damping_factor(const Scalar damping)
Modify the damping factor used in operational space inertia matrix.
void set_armature(const VectorXs &armature)
Modify the armature vector.
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the system acceleration, and cost value.
const Scalar get_damping_factor() const
Return the damping factor used in operational space inertia matrix.
DifferentialActionModelContactFwdDynamicsTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActuationModelAbstract > actuation, boost::shared_ptr< ContactModelMultiple > contacts, boost::shared_ptr< CostModelSum > costs, const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false)
Initialize the contact forward-dynamics action model.
const boost::shared_ptr< ContactModelMultiple > & get_contacts() const
Return the contact model.
virtual void quasiStatic(const boost::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))
Computes the quasic static commands.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Check that the given data belongs to the contact forward-dynamics data.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the cost functions with respect to the state only.
const boost::shared_ptr< ActuationModelAbstract > & get_actuation() const
Return the actuation model.
State multibody representation.
Definition: multibody.hpp:31
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.