Crocoddyl
 
Loading...
Searching...
No Matches
impulse-base.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
11#define CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
12
13#include "crocoddyl/core/utils/deprecate.hpp"
14#include "crocoddyl/multibody/force-base.hpp"
15#include "crocoddyl/multibody/fwd.hpp"
16#include "crocoddyl/multibody/states/multibody.hpp"
17
18namespace crocoddyl {
19
21 public:
22 virtual ~ImpulseModelBase() = default;
23
24 CROCODDYL_BASE_CAST(ImpulseModelBase, ImpulseModelAbstractTpl)
25};
26
27template <typename _Scalar>
29 public:
30 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31
32 typedef _Scalar Scalar;
36 typedef typename MathBase::VectorXs VectorXs;
37 typedef typename MathBase::MatrixXs MatrixXs;
38
39 ImpulseModelAbstractTpl(std::shared_ptr<StateMultibody> state,
40 const pinocchio::ReferenceFrame type,
41 const std::size_t nc);
42
43 DEPRECATED(
44 "Use constructor that passes the type type of contact, this assumes is "
45 "pinocchio::LOCAL",
46 ImpulseModelAbstractTpl(std::shared_ptr<StateMultibody> state,
47 const std::size_t nc);)
48 virtual ~ImpulseModelAbstractTpl() = default;
49
50 virtual void calc(const std::shared_ptr<ImpulseDataAbstract>& data,
51 const Eigen::Ref<const VectorXs>& x) = 0;
52 virtual void calcDiff(const std::shared_ptr<ImpulseDataAbstract>& data,
53 const Eigen::Ref<const VectorXs>& x) = 0;
54
55 virtual void updateForce(const std::shared_ptr<ImpulseDataAbstract>& data,
56 const VectorXs& force) = 0;
57 void updateForceDiff(const std::shared_ptr<ImpulseDataAbstract>& data,
58 const MatrixXs& df_dx) const;
59 void setZeroForce(const std::shared_ptr<ImpulseDataAbstract>& data) const;
60 void setZeroForceDiff(const std::shared_ptr<ImpulseDataAbstract>& data) const;
61
62 virtual std::shared_ptr<ImpulseDataAbstract> createData(
63 pinocchio::DataTpl<Scalar>* const data);
64
65 const std::shared_ptr<StateMultibody>& get_state() const;
66 std::size_t get_nc() const;
67 DEPRECATED("Use get_nc().", std::size_t get_ni() const;)
68 std::size_t get_nu() const;
69
73 pinocchio::FrameIndex get_id() const;
74
78 void set_id(const pinocchio::FrameIndex id);
79
83 void set_type(const pinocchio::ReferenceFrame type);
84
88 pinocchio::ReferenceFrame get_type() const;
89
93 template <class Scalar>
94 friend std::ostream& operator<<(std::ostream& os,
96
102 virtual void print(std::ostream& os) const;
103
104 protected:
105 std::shared_ptr<StateMultibody> state_;
106 std::size_t nc_;
107 pinocchio::FrameIndex id_;
108 pinocchio::ReferenceFrame type_;
109 ImpulseModelAbstractTpl() : state_(nullptr), nc_(0), id_(0) {};
110};
111
112template <typename _Scalar>
114 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
115
116 typedef _Scalar Scalar;
119 typedef typename MathBase::VectorXs VectorXs;
120 typedef typename MathBase::MatrixXs MatrixXs;
121 typedef typename pinocchio::SE3Tpl<Scalar> SE3;
122
123 template <template <typename Scalar> class Model>
124 ImpulseDataAbstractTpl(Model<Scalar>* const model,
125 pinocchio::DataTpl<Scalar>* const data)
126 : Base(model, data),
127 dv0_dq(model->get_nc(), model->get_state()->get_nv()),
128 dtau_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
129 dv0_dq.setZero();
130 dtau_dq.setZero();
131 }
132 virtual ~ImpulseDataAbstractTpl() = default;
133
134 using Base::df_dx;
135 using Base::f;
136 using Base::frame;
137 using Base::Jc;
138 using Base::jMf;
139 using Base::pinocchio;
140
141 typename SE3::ActionMatrixType fXj;
142 MatrixXs dv0_dq;
143 MatrixXs dtau_dq;
144};
145
146} // namespace crocoddyl
147
148/* --- Details -------------------------------------------------------------- */
149/* --- Details -------------------------------------------------------------- */
150/* --- Details -------------------------------------------------------------- */
151#include "crocoddyl/multibody/impulse-base.hxx"
152
153CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ImpulseModelAbstractTpl)
154CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ImpulseDataAbstractTpl)
155
156#endif // CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the impulse model.
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.
pinocchio::ReferenceFrame type_
Type of contact.
friend std::ostream & operator<<(std::ostream &os, const ImpulseModelAbstractTpl< Scalar > &model)
Print information on the impulse model.
void set_type(const pinocchio::ReferenceFrame type)
Modify the type of contact.
DEPRECATED("Use get_nc().", std::size_t get_ni() const ;) std pinocchio::FrameIndex get_id() const
Return the reference frame id.
pinocchio::ReferenceFrame get_type() const
Return the type of contact.
pinocchio::FrameIndex id_
Reference frame id of the contact.
State multibody representation.
Definition multibody.hpp:34
PinocchioData * pinocchio
Pinocchio data.
pinocchio::FrameIndex frame
Frame index of the contact frame.
SE3 jMf
Local frame placement of the contact frame.
Force f
Contact force expressed in the coordinate defined by type.