Crocoddyl
 
Loading...
Searching...
No Matches
impulse-6d.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_6D_HPP_
11#define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_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, ImpulseModel6DTpl)
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 MatrixXs;
34 typedef typename MathBase::Matrix3s Matrix3s;
35
44 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
45 const pinocchio::ReferenceFrame type = pinocchio::ReferenceFrame::LOCAL);
46 virtual ~ImpulseModel6DTpl() = 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 ImpulseData6DTpl : public ImpulseDataAbstractTpl<_Scalar> {
108 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
109 typedef _Scalar Scalar;
112 typedef typename MathBase::Matrix3s Matrix3s;
113 typedef typename MathBase::Matrix6xs Matrix6xs;
114 typedef typename MathBase::MatrixXs MatrixXs;
115 typedef typename pinocchio::SE3Tpl<Scalar> SE3;
116 typedef typename pinocchio::MotionTpl<Scalar> Motion;
117 typedef typename pinocchio::ForceTpl<Scalar> Force;
118
119 template <template <typename Scalar> class Model>
120 ImpulseData6DTpl(Model<Scalar>* const model,
121 pinocchio::DataTpl<Scalar>* const data)
122 : Base(model, data),
123 lwaMl(SE3::Identity()),
124 v0(Motion::Zero()),
125 f_local(Force::Zero()),
126 dv0_local_dq(6, model->get_state()->get_nv()),
127 fJf(6, model->get_state()->get_nv()),
128 v_partial_dq(6, model->get_state()->get_nv()),
129 v_partial_dv(6, model->get_state()->get_nv()),
130 fJf_df(6, model->get_state()->get_nv()) {
131 frame = model->get_id();
132 jMf =
133 model->get_state()->get_pinocchio()->frames[model->get_id()].placement;
134 fXj = jMf.inverse().toActionMatrix();
135 fJf.setZero();
136 v_partial_dq.setZero();
137 v_partial_dv.setZero();
138 vv_skew.setZero();
139 vw_skew.setZero();
140 vv_world_skew.setZero();
141 vw_world_skew.setZero();
142 fv_skew.setZero();
143 fw_skew.setZero();
144 fJf_df.setZero();
145 }
146 virtual ~ImpulseData6DTpl() = default;
147
148 using Base::df_dx;
149 using Base::dv0_dq;
150 using Base::f;
151 using Base::frame;
152 using Base::fXj;
153 using Base::Jc;
154 using Base::jMf;
155 using Base::pinocchio;
156
157 SE3 lwaMl;
158 Motion v0;
159 Force f_local;
160 Matrix6xs dv0_local_dq;
161 Matrix6xs fJf;
162 Matrix6xs v_partial_dq;
163 Matrix6xs v_partial_dv;
164 Matrix3s vv_skew;
165 Matrix3s vw_skew;
166 Matrix3s vv_world_skew;
167 Matrix3s vw_world_skew;
168 Matrix3s fv_skew;
169 Matrix3s fw_skew;
170 MatrixXs fJf_df;
171};
172
173} // namespace crocoddyl
174
175/* --- Details -------------------------------------------------------------- */
176/* --- Details -------------------------------------------------------------- */
177/* --- Details -------------------------------------------------------------- */
178#include "crocoddyl/multibody/impulses/impulse-6d.hxx"
179
180CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ImpulseModel6DTpl)
181CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ImpulseData6DTpl)
182
183#endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
virtual std::shared_ptr< ImpulseDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data) override
Create the 3d 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 3d impulse Jacobian.
ImpulseModel6DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const pinocchio::ReferenceFrame type=pinocchio::ReferenceFrame::LOCAL)
Initialize the 6d impulse model.
virtual void calcDiff(const std::shared_ptr< ImpulseDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the 3d impulse holonomic constraint.
ImpulseModel6DTpl< NewScalar > cast() const
Cast the impulse-6d model to a different scalar type.
virtual void print(std::ostream &os) const override
Print relevant information of the 6d 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.