Crocoddyl
 
Loading...
Searching...
No Matches
impulse-3d.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_IMPULSES_IMPULSE_3D_HPP_
11#define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
12
13#include "crocoddyl/multibody/fwd.hpp"
14#include "crocoddyl/multibody/impulse-base.hpp"
15
16namespace crocoddyl {
17
18template <typename _Scalar>
20 public:
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 CROCODDYL_DERIVED_CAST(ImpulseModelBase, ImpulseModel3DTpl)
23
24 typedef _Scalar Scalar;
30 typedef typename MathBase::Vector2s Vector2s;
31 typedef typename MathBase::Vector3s Vector3s;
32 typedef typename MathBase::VectorXs VectorXs;
33 typedef typename MathBase::MatrixXs Matrix3s;
34 typedef typename MathBase::MatrixXs MatrixXs;
35
44 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
45 const pinocchio::ReferenceFrame type = pinocchio::ReferenceFrame::LOCAL);
46 virtual ~ImpulseModel3DTpl() = default;
47
54 virtual void calc(const std::shared_ptr<ImpulseDataAbstract>& data,
55 const Eigen::Ref<const VectorXs>& x) override;
56
63 virtual void calcDiff(const std::shared_ptr<ImpulseDataAbstract>& data,
64 const Eigen::Ref<const VectorXs>& x) override;
65
72 virtual void updateForce(const std::shared_ptr<ImpulseDataAbstract>& data,
73 const VectorXs& force) override;
74
78 virtual std::shared_ptr<ImpulseDataAbstract> createData(
79 pinocchio::DataTpl<Scalar>* const data) override;
80
90 template <typename NewScalar>
92
98 virtual void print(std::ostream& os) const override;
99
100 protected:
101 using Base::id_;
102 using Base::state_;
103 using Base::type_;
104};
105
106template <typename _Scalar>
107struct ImpulseData3DTpl : public ImpulseDataAbstractTpl<_Scalar> {
108 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
109
110 typedef _Scalar Scalar;
113 typedef typename MathBase::Vector3s Vector3s;
114 typedef typename MathBase::Matrix3s Matrix3s;
115 typedef typename MathBase::Matrix3xs Matrix3xs;
116 typedef typename MathBase::Matrix6xs Matrix6xs;
117 typedef typename pinocchio::ForceTpl<Scalar> Force;
118
119 template <template <typename Scalar> class Model>
120 ImpulseData3DTpl(Model<Scalar>* const model,
121 pinocchio::DataTpl<Scalar>* const data)
122 : Base(model, data),
123 f_local(Force::Zero()),
124 dv0_local_dq(3, model->get_state()->get_nv()),
125 fJf(6, model->get_state()->get_nv()),
126 v_partial_dq(6, model->get_state()->get_nv()),
127 v_partial_dv(6, model->get_state()->get_nv()),
128 fJf_df(3, model->get_state()->get_nv()) {
129 frame = model->get_id();
130 jMf =
131 model->get_state()->get_pinocchio()->frames[model->get_id()].placement;
132 fXj = jMf.inverse().toActionMatrix();
133 v0.setZero();
134 dv0_local_dq.setZero();
135 fJf.setZero();
136 v_partial_dq.setZero();
137 v_partial_dv.setZero();
138 v0_skew.setZero();
139 v0_world_skew.setZero();
140 f_skew.setZero();
141 fJf_df.setZero();
142 }
143 virtual ~ImpulseData3DTpl() = default;
144
145 using Base::df_dx;
146 using Base::dv0_dq;
147 using Base::f;
148 using Base::frame;
149 using Base::fXj;
150 using Base::Jc;
151 using Base::jMf;
152 using Base::pinocchio;
153
154 Vector3s v0;
155 Force f_local;
156 Matrix3xs dv0_local_dq;
157 Matrix6xs fJf;
158 Matrix6xs v_partial_dq;
159 Matrix6xs v_partial_dv;
160 Matrix3s v0_skew;
161 Matrix3s v0_world_skew;
162 Matrix3s f_skew;
163 Matrix3xs fJf_df;
164};
165
166} // namespace crocoddyl
167
168/* --- Details -------------------------------------------------------------- */
169/* --- Details -------------------------------------------------------------- */
170/* --- Details -------------------------------------------------------------- */
171#include "crocoddyl/multibody/impulses/impulse-3d.hxx"
172
173CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ImpulseModel3DTpl)
174CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ImpulseData3DTpl)
175
176#endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
ImpulseModel3DTpl< NewScalar > cast() const
Cast the impulse-3d model to a different scalar type.
virtual std::shared_ptr< ImpulseDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data) override
Create the 6d impulse data.
virtual void updateForce(const std::shared_ptr< ImpulseDataAbstract > &data, const VectorXs &force) override
Convert the force into a stack of spatial forces.
virtual void calc(const std::shared_ptr< ImpulseDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the 6d impulse Jacobian.
ImpulseModel3DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const pinocchio::ReferenceFrame type=pinocchio::ReferenceFrame::LOCAL)
Initialize the 3d impulse model.
virtual void calcDiff(const std::shared_ptr< ImpulseDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the 6d impulse holonomic constraint.
virtual void print(std::ostream &os) const override
Print relevant information of the 3d impulse model.
pinocchio::ReferenceFrame type_
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.
MatrixXs Jc
Contact Jacobian.
Force f
Contact force expressed in the coordinate defined by type.