Crocoddyl
 
Loading...
Searching...
No Matches
free-invdyn.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2021-2025, Heriot-Watt University, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_
10#define CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_
11
12#include "crocoddyl/core/actuation-base.hpp"
13#include "crocoddyl/core/constraints/constraint-manager.hpp"
14#include "crocoddyl/core/costs/cost-sum.hpp"
15#include "crocoddyl/core/diff-action-base.hpp"
16#include "crocoddyl/core/residual-base.hpp"
17#include "crocoddyl/multibody/data/multibody.hpp"
18#include "crocoddyl/multibody/fwd.hpp"
19#include "crocoddyl/multibody/states/multibody.hpp"
20
21namespace crocoddyl {
22
38template <typename _Scalar>
40 : public DifferentialActionModelAbstractTpl<_Scalar> {
41 public:
42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 CROCODDYL_DERIVED_CAST(DifferentialActionModelBase,
45
46 typedef _Scalar Scalar;
57 typedef typename MathBase::VectorXs VectorXs;
58
70 std::shared_ptr<StateMultibody> state,
71 std::shared_ptr<ActuationModelAbstract> actuation,
72 std::shared_ptr<CostModelSum> costs);
73
83 std::shared_ptr<StateMultibody> state,
84 std::shared_ptr<ActuationModelAbstract> actuation,
85 std::shared_ptr<CostModelSum> costs,
86 std::shared_ptr<ConstraintModelManager> constraints);
88
99 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
100 const Eigen::Ref<const VectorXs>& x,
101 const Eigen::Ref<const VectorXs>& u) override;
102
108 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
109 const Eigen::Ref<const VectorXs>& x) override;
110
124 virtual void calcDiff(
125 const std::shared_ptr<DifferentialActionDataAbstract>& data,
126 const Eigen::Ref<const VectorXs>& x,
127 const Eigen::Ref<const VectorXs>& u) override;
128
134 virtual void calcDiff(
135 const std::shared_ptr<DifferentialActionDataAbstract>& data,
136 const Eigen::Ref<const VectorXs>& x) override;
137
143 virtual std::shared_ptr<DifferentialActionDataAbstract> createData() override;
144
154 template <typename NewScalar>
156
161 virtual bool checkData(
162 const std::shared_ptr<DifferentialActionDataAbstract>& data) override;
163
177 virtual void quasiStatic(
178 const std::shared_ptr<DifferentialActionDataAbstract>& data,
179 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
180 const std::size_t maxiter = 100,
181 const Scalar tol = Scalar(1e-9)) override;
182
186 virtual std::size_t get_ng() const override;
187
191 virtual std::size_t get_nh() const override;
192
196 virtual std::size_t get_ng_T() const override;
197
201 virtual std::size_t get_nh_T() const override;
202
206 virtual const VectorXs& get_g_lb() const override;
207
211 virtual const VectorXs& get_g_ub() const override;
212
216 const std::shared_ptr<ActuationModelAbstract>& get_actuation() const;
217
221 const std::shared_ptr<CostModelSum>& get_costs() const;
222
226 const std::shared_ptr<ConstraintModelManager>& get_constraints() const;
227
231 pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
232
238 virtual void print(std::ostream& os) const override;
239
240 protected:
241 using Base::g_lb_;
242 using Base::g_ub_;
243 using Base::ng_;
244 using Base::nh_;
245 using Base::nu_;
246 using Base::state_;
247
248 private:
249 void init(const std::shared_ptr<StateMultibody>& state);
250 std::shared_ptr<ActuationModelAbstract> actuation_;
251 std::shared_ptr<CostModelSum> costs_;
252 std::shared_ptr<ConstraintModelManager> constraints_;
253 pinocchio::ModelTpl<Scalar>* pinocchio_;
254
255 public:
269 public:
270 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
271 CROCODDYL_INNER_DERIVED_CAST(ResidualModelBase,
274
275 typedef _Scalar Scalar;
282 typedef typename MathBase::VectorXs VectorXs;
283 typedef typename MathBase::MatrixXs MatrixXs;
284
291 ResidualModelActuation(std::shared_ptr<StateMultibody> state,
292 const std::size_t nu)
293 : Base(state, state->get_nv() - nu, state->get_nv(), true, true, true),
294 na_(nu) {}
295 virtual ~ResidualModelActuation() = default;
296
304 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
305 const Eigen::Ref<const VectorXs>&,
306 const Eigen::Ref<const VectorXs>&) override {
307 typename Data::ResidualDataActuation* d =
308 static_cast<typename Data::ResidualDataActuation*>(data.get());
309 // Update the under-actuation set and residual
310 std::size_t nrow = 0;
311 for (std::size_t k = 0;
312 k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
313 if (!d->actuation->tau_set[k]) {
314 data->r(nrow) = d->pinocchio->tau(k);
315 nrow += 1;
316 }
317 }
318 }
319
324 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
325 const Eigen::Ref<const VectorXs>&) override {
326 data->r.setZero();
327 }
328
336 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
337 const Eigen::Ref<const VectorXs>&,
338 const Eigen::Ref<const VectorXs>&) override {
339 typename Data::ResidualDataActuation* d =
340 static_cast<typename Data::ResidualDataActuation*>(data.get());
341 std::size_t nrow = 0;
342 const std::size_t nv = state_->get_nv();
343 d->dtau_dx.leftCols(nv) = d->pinocchio->dtau_dq;
344 d->dtau_dx.rightCols(nv) = d->pinocchio->dtau_dv;
345 d->dtau_dx -= d->actuation->dtau_dx;
346 for (std::size_t k = 0;
347 k < static_cast<std::size_t>(d->actuation->tau_set.size()); ++k) {
348 if (!d->actuation->tau_set[k]) {
349 d->Rx.row(nrow) = d->dtau_dx.row(k);
350 d->Ru.row(nrow) = d->pinocchio->M.row(k);
351 nrow += 1;
352 }
353 }
354 }
355
361 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
362 const Eigen::Ref<const VectorXs>&) override {
363 data->Rx.setZero();
364 data->Ru.setZero();
365 }
366
372 virtual std::shared_ptr<ResidualDataAbstract> createData(
373 DataCollectorAbstract* const data) override {
374 return std::allocate_shared<typename Data::ResidualDataActuation>(
375 Eigen::aligned_allocator<typename Data::ResidualDataActuation>(),
376 this, data);
377 }
378
390 template <typename NewScalar>
392 NewScalar>::ResidualModelActuation
393 cast() const {
395 NewScalar>::ResidualModelActuation ReturnType;
396 typedef StateMultibodyTpl<NewScalar> StateType;
397 ReturnType ret(std::static_pointer_cast<StateType>(
398 state_->template cast<NewScalar>()),
399 na_);
400 return ret;
401 }
402
408 virtual void print(std::ostream& os) const override {
409 os << "ResidualModelActuation {nx=" << state_->get_nx()
410 << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << ", na=" << na_
411 << "}";
412 }
413
414 protected:
415 std::size_t na_;
416 using Base::nu_;
417 using Base::state_;
418 };
419};
420
421template <typename _Scalar>
423 : public DifferentialActionDataAbstractTpl<_Scalar> {
424 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
425 typedef _Scalar Scalar;
433 typedef typename MathBase::VectorXs VectorXs;
434
435 template <template <typename Scalar> class Model>
436 explicit DifferentialActionDataFreeInvDynamicsTpl(Model<Scalar>* const model)
437 : Base(model),
438 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
439 multibody(
440 &pinocchio, model->get_actuation()->createData(),
441 std::make_shared<JointDataAbstract>(
442 model->get_state(), model->get_actuation(), model->get_nu())),
443 costs(model->get_costs()->createData(&multibody)),
444 constraints(model->get_constraints()->createData(&multibody)),
445 tmp_xstatic(model->get_state()->get_nx()) {
446 const std::size_t nv = model->get_state()->get_nv();
447 Fu.leftCols(nv).diagonal().setOnes();
448 multibody.joint->da_du.leftCols(nv).diagonal().setOnes();
449 costs->shareMemory(this);
450 constraints->shareMemory(this);
451 tmp_xstatic.setZero();
452 }
454
455 pinocchio::DataTpl<Scalar> pinocchio;
457 std::shared_ptr<CostDataSum> costs;
458 std::shared_ptr<ConstraintDataManager> constraints;
459 VectorXs
461 using Base::cost;
462 using Base::Fu;
463 using Base::Fx;
464 using Base::Lu;
465 using Base::Luu;
466 using Base::Lx;
467 using Base::Lxu;
468 using Base::Lxx;
469 using Base::r;
470 using Base::xout;
471
473 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
474
475 typedef _Scalar Scalar;
481 typedef typename MathBase::MatrixXs MatrixXs;
482
483 template <template <typename Scalar> class Model>
484 ResidualDataActuation(Model<Scalar>* const model,
485 DataCollectorAbstract* const data)
486 : Base(model, data),
487 dtau_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()) {
488 dtau_dx.setZero();
489 // Check that proper shared data has been passed
491 dynamic_cast<DataCollectorActMultibody*>(shared);
492 if (d == NULL) {
493 throw_pretty(
494 "Invalid argument: the shared data should be derived from "
495 "DataCollectorActMultibody");
496 }
497
498 // Avoids data casting at runtime
499 pinocchio = d->pinocchio;
500 actuation = d->actuation;
501 }
502
503 pinocchio::DataTpl<Scalar>* pinocchio;
504 std::shared_ptr<ActuationDataAbstract> actuation;
505 MatrixXs dtau_dx;
506 using Base::r;
507 using Base::Ru;
508 using Base::Rx;
509 using Base::shared;
510 };
511};
512
513} // namespace crocoddyl
514
515/* --- Details -------------------------------------------------------------- */
516/* --- Details -------------------------------------------------------------- */
517/* --- Details -------------------------------------------------------------- */
518#include <crocoddyl/multibody/actions/free-invdyn.hxx>
519
520CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
522CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
524
525#endif // CROCODDYL_MULTIBODY_ACTIONS_FREE_INVDYN_HPP_
Abstract class for the actuation-mapping model.
Manage the individual constraint models.
Residual-based constraint.
Definition residual.hpp:47
Summation of individual cost models.
Definition cost-sum.hpp:75
Abstract class for differential action model.
std::shared_ptr< StateAbstract > state_
Model of the state.
VectorXs g_ub_
Lower bound of the inequality constraints.
std::size_t nh_
Number of equality constraints.
VectorXs g_lb_
Lower bound of the inequality constraints.
std::size_t ng_
Number of inequality constraints.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &) override
ResidualModelActuation(std::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the actuation residual model.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &) override
Compute the residual vector for nodes that depends only on the state.
DifferentialActionModelFreeInvDynamicsTpl< NewScalar >::ResidualModelActuation cast() const
Cast the actuation-residual model to a different scalar type.
virtual void calcDiff(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the derivatives of the actuation residual.
virtual void calc(const std::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &, const Eigen::Ref< const VectorXs > &) override
Compute the actuation residual.
virtual std::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data) override
Create the actuation residual data.
virtual void print(std::ostream &os) const override
Print relevant information of the actuation residual model.
Differential action model for free inverse dynamics in multibody systems.
virtual bool checkData(const std::shared_ptr< DifferentialActionDataAbstract > &data) override
Checks that a specific data belongs to the free inverse-dynamics 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 calcDiff(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
virtual const VectorXs & get_g_ub() const override
Return the upper bound of the inequality constraints.
virtual void calc(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the system acceleration, cost value and constraint residuals.
virtual std::shared_ptr< DifferentialActionDataAbstract > createData() override
Create the free inverse-dynamics data.
virtual std::size_t get_ng() const override
Return the number of inequality constraints.
virtual std::size_t get_nh_T() const override
Return the number of equality terminal constraints.
DifferentialActionModelFreeInvDynamicsTpl(std::shared_ptr< StateMultibody > state, std::shared_ptr< ActuationModelAbstract > actuation, std::shared_ptr< CostModelSum > costs)
Initialize the free inverse-dynamics action model.
virtual std::size_t get_nh() const override
Return the number of equality constraints.
const std::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
DifferentialActionModelFreeInvDynamicsTpl(std::shared_ptr< StateMultibody > state, std::shared_ptr< ActuationModelAbstract > actuation, std::shared_ptr< CostModelSum > costs, std::shared_ptr< ConstraintModelManager > constraints)
Initialize the free inverse-dynamics action model.
virtual std::size_t get_ng_T() const override
Return the number of equality terminal constraints.
virtual void calc(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
const std::shared_ptr< ActuationModelAbstract > & get_actuation() const
Return the actuation model.
virtual void quasiStatic(const std::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)) override
Computes the quasic static commands.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
DifferentialActionModelFreeInvDynamicsTpl< NewScalar > cast() const
Cast the free-invdyn model to a different scalar type.
virtual void print(std::ostream &os) const override
Print relevant information of the free inverse-dynamics model.
virtual void calcDiff(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the dynamics, cost and constraint functions.
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
State multibody representation.
Definition multibody.hpp:34
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs Lx
Jacobian of the cost w.r.t. the state .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .
VectorXs Lu
Jacobian of the cost w.r.t. the control .
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
std::shared_ptr< ActuationDataAbstract > actuation
Actuation data.
DataCollectorAbstract * shared
Shared data allocated by the action model.
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
std::shared_ptr< ConstraintDataManager > constraints
Constraints data.
VectorXs tmp_xstatic
State point used for computing the quasi-static input.
DataCollectorJointActMultibody multibody
Multibody data.
pinocchio::DataTpl< Scalar > pinocchio
Pinocchio data.
std::shared_ptr< CostDataSum > costs
Costs data.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.