Crocoddyl
 
Loading...
Searching...
No Matches
impulse-fwddyn.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// University of Oxford, Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
11#define CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
12
13#include "crocoddyl/core/action-base.hpp"
14#include "crocoddyl/core/constraints/constraint-manager.hpp"
15#include "crocoddyl/core/costs/cost-sum.hpp"
16#include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
17#include "crocoddyl/multibody/actuations/floating-base.hpp"
18#include "crocoddyl/multibody/data/impulses.hpp"
19#include "crocoddyl/multibody/fwd.hpp"
20#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
21#include "crocoddyl/multibody/states/multibody.hpp"
22
23namespace crocoddyl {
24
58template <typename _Scalar>
60 : public ActionModelAbstractTpl<_Scalar> {
61 public:
62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64
65 typedef _Scalar Scalar;
74 typedef typename MathBase::VectorXs VectorXs;
75 typedef typename MathBase::MatrixXs MatrixXs;
76
95 std::shared_ptr<StateMultibody> state,
96 std::shared_ptr<ImpulseModelMultiple> impulses,
97 std::shared_ptr<CostModelSum> costs, const Scalar r_coeff = Scalar(0.),
98 const Scalar JMinvJt_damping = Scalar(0.),
99 const bool enable_force = false);
100
120 std::shared_ptr<StateMultibody> state,
121 std::shared_ptr<ImpulseModelMultiple> impulses,
122 std::shared_ptr<CostModelSum> costs,
123 std::shared_ptr<ConstraintModelManager> constraints,
124 const Scalar r_coeff = Scalar(0.),
125 const Scalar JMinvJt_damping = Scalar(0.),
126 const bool enable_force = false);
127 virtual ~ActionModelImpulseFwdDynamicsTpl() = default;
128
138 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
139 const Eigen::Ref<const VectorXs>& x,
140 const Eigen::Ref<const VectorXs>& u) override;
141
154 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
155 const Eigen::Ref<const VectorXs>& x) override;
156
164 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
165 const Eigen::Ref<const VectorXs>& x,
166 const Eigen::Ref<const VectorXs>& u) override;
167
179 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
180 const Eigen::Ref<const VectorXs>& x) override;
181
187 virtual std::shared_ptr<ActionDataAbstract> createData() override;
188
198 template <typename NewScalar>
200
205 virtual bool checkData(
206 const std::shared_ptr<ActionDataAbstract>& data) override;
207
211 virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
212 Eigen::Ref<VectorXs> u,
213 const Eigen::Ref<const VectorXs>& x,
214 const std::size_t maxiter = 100,
215 const Scalar tol = Scalar(1e-9)) override;
216
220 virtual std::size_t get_ng() const override;
221
225 virtual std::size_t get_nh() const override;
226
230 virtual std::size_t get_ng_T() const override;
231
235 virtual std::size_t get_nh_T() const override;
236
240 virtual const VectorXs& get_g_lb() const override;
241
245 virtual const VectorXs& get_g_ub() const override;
246
250 const std::shared_ptr<ImpulseModelMultiple>& get_impulses() const;
251
255 const std::shared_ptr<CostModelSum>& get_costs() const;
256
260 const std::shared_ptr<ConstraintModelManager>& get_constraints() const;
261
265 pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
266
270 const VectorXs& get_armature() const;
271
275 const Scalar get_restitution_coefficient() const;
276
281 const Scalar get_damping_factor() const;
282
286 void set_armature(const VectorXs& armature);
287
291 void set_restitution_coefficient(const Scalar r_coeff);
292
297 void set_damping_factor(const Scalar damping);
298
304 virtual void print(std::ostream& os) const override;
305
306 protected:
307 using Base::g_lb_;
308 using Base::g_ub_;
309 using Base::state_;
310
311 private:
312 void init();
313 void initCalc(Data* data, const Eigen::Ref<const VectorXs>& x);
314 void initCalcDiff(Data* data, const Eigen::Ref<const VectorXs>& x);
315 std::shared_ptr<ImpulseModelMultiple> impulses_;
316 std::shared_ptr<CostModelSum> costs_;
317 std::shared_ptr<ConstraintModelManager> constraints_;
318 pinocchio::ModelTpl<Scalar>* pinocchio_;
319 bool with_armature_;
320 VectorXs armature_;
321 Scalar r_coeff_;
322 Scalar JMinvJt_damping_;
324 bool enable_force_;
326 pinocchio::MotionTpl<Scalar> gravity_;
327};
328
329template <typename _Scalar>
331 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
332 typedef _Scalar Scalar;
335 typedef typename MathBase::VectorXs VectorXs;
336 typedef typename MathBase::MatrixXs MatrixXs;
337
338 template <template <typename Scalar> class Model>
339 explicit ActionDataImpulseFwdDynamicsTpl(Model<Scalar>* const model)
340 : Base(model),
341 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
342 multibody(&pinocchio, model->get_impulses()->createData(&pinocchio)),
343 costs(model->get_costs()->createData(&multibody)),
344 vnone(model->get_state()->get_nv()),
345 Kinv(model->get_state()->get_nv() +
346 model->get_impulses()->get_nc_total(),
347 model->get_state()->get_nv() +
348 model->get_impulses()->get_nc_total()),
349 df_dx(model->get_impulses()->get_nc_total(),
350 model->get_state()->get_ndx()),
351 dgrav_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
352 costs->shareMemory(this);
353 if (model->get_constraints() != nullptr) {
354 constraints = model->get_constraints()->createData(&multibody);
355 constraints->shareMemory(this);
356 }
357 vnone.setZero();
358 Kinv.setZero();
359 df_dx.setZero();
360 dgrav_dq.setZero();
361 }
362 virtual ~ActionDataImpulseFwdDynamicsTpl() = default;
363
364 pinocchio::DataTpl<Scalar> pinocchio;
366 std::shared_ptr<CostDataSumTpl<Scalar> > costs;
367 std::shared_ptr<ConstraintDataManagerTpl<Scalar> > constraints;
368 VectorXs vnone;
369 MatrixXs Kinv;
370 MatrixXs df_dx;
371 MatrixXs dgrav_dq;
372};
373
374} // namespace crocoddyl
375
376/* --- Details -------------------------------------------------------------- */
377/* --- Details -------------------------------------------------------------- */
378/* --- Details -------------------------------------------------------------- */
379#include <crocoddyl/multibody/actions/impulse-fwddyn.hxx>
380
381CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
383CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
385
386#endif // CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
Abstract class for action model.
std::shared_ptr< StateAbstract > state_
Model of the state.
VectorXs g_ub_
Lower bound of the inequality constraints.
VectorXs g_lb_
Lower bound of the inequality constraints.
Action model for impulse forward dynamics in multibody systems.
const std::shared_ptr< ImpulseModelMultiple > & get_impulses() const
Return the impulse model.
virtual const VectorXs & get_g_lb() const override
Return the lower bound of the inequality constraints.
const std::shared_ptr< ConstraintModelManager > & get_constraints() const
Return the constraint model manager.
virtual void calc(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the system acceleration, and cost value.
virtual const VectorXs & get_g_ub() const override
Return the upper bound of the inequality constraints.
virtual std::shared_ptr< ActionDataAbstract > createData() override
Create the impulse forward-dynamics data.
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the cost functions with respect to the state only.
void set_restitution_coefficient(const Scalar r_coeff)
Modify the restituion coefficient.
virtual bool checkData(const std::shared_ptr< ActionDataAbstract > &data) override
Check that the given data belongs to the impulse forward-dynamics data.
ActionModelImpulseFwdDynamicsTpl(std::shared_ptr< StateMultibody > state, std::shared_ptr< ImpulseModelMultiple > impulses, std::shared_ptr< CostModelSum > costs, const Scalar r_coeff=Scalar(0.), const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false)
Initialize the impulse forward-dynamics action model.
virtual std::size_t get_ng() const override
Return the number of inequality constraints.
virtual void quasiStatic(const std::shared_ptr< ActionDataAbstract > &data, Eigen::Ref< VectorXs > u, const Eigen::Ref< const VectorXs > &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9)) override
Computes the quasic static commands.
const VectorXs & get_armature() const
Return the armature vector.
virtual std::size_t get_nh_T() const override
Return the number of equality terminal constraints.
virtual std::size_t get_nh() const override
Return the number of equality constraints.
void set_damping_factor(const Scalar damping)
Modify the damping factor used in the operational space inertia matrix.
const std::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
virtual void calc(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the total cost value for nodes that depends only on the state.
const Scalar get_restitution_coefficient() const
Return the restituion coefficient.
virtual std::size_t get_ng_T() const override
Return the number of equality terminal constraints.
void set_armature(const VectorXs &armature)
Modify the armature vector.
const Scalar get_damping_factor() const
Return the damping factor used in the operational space inertia matrix.
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the impulse dynamics, and cost function.
ActionModelImpulseFwdDynamicsTpl(std::shared_ptr< StateMultibody > state, std::shared_ptr< ImpulseModelMultiple > impulses, std::shared_ptr< CostModelSum > costs, std::shared_ptr< ConstraintModelManager > constraints, const Scalar r_coeff=Scalar(0.), const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false)
Initialize the impulse forward-dynamics action model.
ActionModelImpulseFwdDynamicsTpl< NewScalar > cast() const
Cast the impulse-fwddyn model to a different scalar type.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
virtual void print(std::ostream &os) const override
Print relevant information of the impulse forward-dynamics model.
Manage the individual constraint models.
Summation of individual cost models.
Definition cost-sum.hpp:75
Define a stack of impulse models.
State multibody representation.
Definition multibody.hpp:34