Crocoddyl
 
Loading...
Searching...
No Matches
impulse-6d.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2023, 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 <pinocchio/multibody/data.hpp>
14#include <pinocchio/spatial/motion.hpp>
15
16#include "crocoddyl/multibody/fwd.hpp"
17#include "crocoddyl/multibody/impulse-base.hpp"
18
19namespace crocoddyl {
20
21template <typename _Scalar>
23 public:
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25
26 typedef _Scalar Scalar;
32 typedef typename MathBase::Vector2s Vector2s;
33 typedef typename MathBase::Vector3s Vector3s;
34 typedef typename MathBase::VectorXs VectorXs;
35 typedef typename MathBase::MatrixXs MatrixXs;
36 typedef typename MathBase::Matrix3s Matrix3s;
37
46 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
47 const pinocchio::ReferenceFrame type = pinocchio::ReferenceFrame::LOCAL);
48 virtual ~ImpulseModel6DTpl();
49
56 virtual void calc(const std::shared_ptr<ImpulseDataAbstract>& data,
57 const Eigen::Ref<const VectorXs>& x);
58
65 virtual void calcDiff(const std::shared_ptr<ImpulseDataAbstract>& data,
66 const Eigen::Ref<const VectorXs>& x);
67
74 virtual void updateForce(const std::shared_ptr<ImpulseDataAbstract>& data,
75 const VectorXs& force);
76
80 virtual std::shared_ptr<ImpulseDataAbstract> createData(
81 pinocchio::DataTpl<Scalar>* const data);
82
88 virtual void print(std::ostream& os) const;
89
90 protected:
91 using Base::id_;
92 using Base::state_;
93 using Base::type_;
94};
95
96template <typename _Scalar>
97struct ImpulseData6DTpl : public ImpulseDataAbstractTpl<_Scalar> {
98 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99 typedef _Scalar Scalar;
102 typedef typename MathBase::Matrix3s Matrix3s;
103 typedef typename MathBase::Matrix6xs Matrix6xs;
104 typedef typename MathBase::MatrixXs MatrixXs;
105 typedef typename pinocchio::SE3Tpl<Scalar> SE3;
106 typedef typename pinocchio::MotionTpl<Scalar> Motion;
107 typedef typename pinocchio::ForceTpl<Scalar> Force;
108
109 template <template <typename Scalar> class Model>
110 ImpulseData6DTpl(Model<Scalar>* const model,
111 pinocchio::DataTpl<Scalar>* const data)
112 : Base(model, data),
113 lwaMl(SE3::Identity()),
114 v0(Motion::Zero()),
115 f_local(Force::Zero()),
116 dv0_local_dq(6, model->get_state()->get_nv()),
117 fJf(6, model->get_state()->get_nv()),
118 v_partial_dq(6, model->get_state()->get_nv()),
119 v_partial_dv(6, model->get_state()->get_nv()),
120 fJf_df(6, model->get_state()->get_nv()) {
121 frame = model->get_id();
122 jMf =
123 model->get_state()->get_pinocchio()->frames[model->get_id()].placement;
124 fXj = jMf.inverse().toActionMatrix();
125 fJf.setZero();
126 v_partial_dq.setZero();
127 v_partial_dv.setZero();
128 vv_skew.setZero();
129 vw_skew.setZero();
130 vv_world_skew.setZero();
131 vw_world_skew.setZero();
132 fv_skew.setZero();
133 fw_skew.setZero();
134 fJf_df.setZero();
135 }
136
137 using Base::df_dx;
138 using Base::dv0_dq;
139 using Base::f;
140 using Base::frame;
141 using Base::fXj;
142 using Base::Jc;
143 using Base::jMf;
144 using Base::pinocchio;
145
146 SE3 lwaMl;
147 Motion v0;
148 Force f_local;
149 Matrix6xs dv0_local_dq;
150 Matrix6xs fJf;
151 Matrix6xs v_partial_dq;
152 Matrix6xs v_partial_dv;
153 Matrix3s vv_skew;
154 Matrix3s vw_skew;
155 Matrix3s vv_world_skew;
156 Matrix3s vw_world_skew;
157 Matrix3s fv_skew;
158 Matrix3s fw_skew;
159 MatrixXs fJf_df;
160};
161
162} // namespace crocoddyl
163
164/* --- Details -------------------------------------------------------------- */
165/* --- Details -------------------------------------------------------------- */
166/* --- Details -------------------------------------------------------------- */
167#include "crocoddyl/multibody/impulses/impulse-6d.hxx"
168
169#endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
virtual void calcDiff(const std::shared_ptr< ImpulseDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the 3d impulse holonomic constraint.
virtual std::shared_ptr< ImpulseDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create the 3d impulse data.
virtual void print(std::ostream &os) const
Print relevant information of the 6d impulse model.
virtual void updateForce(const std::shared_ptr< ImpulseDataAbstract > &data, const VectorXs &force)
Convert the force into a stack of spatial forces.
ImpulseModel6DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const pinocchio::ReferenceFrame type=pinocchio::ReferenceFrame::LOCAL)
Initialize the 6d impulse model.
virtual void calc(const std::shared_ptr< ImpulseDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the 3d impulse Jacobian.
pinocchio::ReferenceFrame type_
Type of contact.
pinocchio::FrameIndex id_
Reference frame id of the contact.
State multibody representation.
Definition multibody.hpp:35
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.