crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
contact-2d.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2020-2021, LAAS-CNRS, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
10#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
11
12#include <pinocchio/spatial/motion.hpp>
13#include <pinocchio/multibody/data.hpp>
14#include <pinocchio/algorithm/frames.hpp>
15#include <pinocchio/algorithm/kinematics-derivatives.hpp>
16
17#include "crocoddyl/multibody/fwd.hpp"
18#include "crocoddyl/core/utils/exception.hpp"
19#include "crocoddyl/multibody/contact-base.hpp"
20#include "crocoddyl/core/utils/deprecate.hpp"
21
22#include "crocoddyl/multibody/frames-deprecated.hpp"
23
24namespace crocoddyl {
25
26template <typename _Scalar>
28 public:
29 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30
31 typedef _Scalar Scalar;
37 typedef typename MathBase::Vector2s Vector2s;
38 typedef typename MathBase::Vector3s Vector3s;
39 typedef typename MathBase::VectorXs VectorXs;
40 typedef typename MathBase::Matrix3s Matrix3s;
41
51 ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Vector2s& xref,
52 const std::size_t nu, const Vector2s& gains = Vector2s::Zero());
53
64 ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Vector2s& xref,
65 const Vector2s& gains = Vector2s::Zero());
66 DEPRECATED("Use constructor which is not based on FrameTranslation.",
67 ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslationTpl<Scalar>& xref,
68 const std::size_t nu, const Vector2s& gains = Vector2s::Zero());)
69 DEPRECATED("Use constructor which is not based on FrameTranslation.",
70 ContactModel2DTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslationTpl<Scalar>& xref,
71 const Vector2s& gains = Vector2s::Zero());)
72 virtual ~ContactModel2DTpl();
73
81 virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
82
90 virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
91
98 virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
99
103 virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
104
108 const Vector2s& get_reference() const;
109
110 DEPRECATED("Use get_reference() or get_id()", FrameTranslationTpl<Scalar> get_xref() const;)
111
115 const Vector2s& get_gains() const;
116
120 void set_reference(const Vector2s& reference);
121
127 virtual void print(std::ostream& os) const;
128
129 protected:
130 using Base::id_;
131 using Base::nc_;
132 using Base::nu_;
133 using Base::state_;
134
135 private:
136 Vector2s xref_;
137 Vector2s gains_;
138};
139
140template <typename _Scalar>
141struct ContactData2DTpl : public ContactDataAbstractTpl<_Scalar> {
142 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143
144 typedef _Scalar Scalar;
147 typedef typename MathBase::Matrix2s Matrix2s;
148 typedef typename MathBase::Matrix3s Matrix3s;
149 typedef typename MathBase::Matrix6xs Matrix6xs;
150 typedef typename MathBase::Vector3s Vector3s;
151
152 template <template <typename Scalar> class Model>
153 ContactData2DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
154 : Base(model, data),
155 fJf(6, model->get_state()->get_nv()),
156 v_partial_dq(6, model->get_state()->get_nv()),
157 a_partial_dq(6, model->get_state()->get_nv()),
158 a_partial_dv(6, model->get_state()->get_nv()),
159 a_partial_da(6, model->get_state()->get_nv()),
160 fXjdv_dq(6, model->get_state()->get_nv()),
161 fXjda_dq(6, model->get_state()->get_nv()),
162 fXjda_dv(6, model->get_state()->get_nv()) {
163 frame = model->get_id();
164 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
165 fXj = jMf.inverse().toActionMatrix();
166 fJf.setZero();
167 v_partial_dq.setZero();
168 a_partial_dq.setZero();
169 a_partial_dv.setZero();
170 a_partial_da.setZero();
171 fXjdv_dq.setZero();
172 fXjda_dq.setZero();
173 fXjda_dv.setZero();
174 vv.setZero();
175 vw.setZero();
176 vv_skew.setZero();
177 vw_skew.setZero();
178 oRf.setZero();
179 }
180
181 using Base::a0;
182 using Base::da0_dx;
183 using Base::df_du;
184 using Base::df_dx;
185 using Base::f;
186 using Base::frame;
187 using Base::fXj;
188 using Base::Jc;
189 using Base::jMf;
190 using Base::pinocchio;
191
192 pinocchio::MotionTpl<Scalar> v;
193 pinocchio::MotionTpl<Scalar> a;
194 Matrix6xs fJf;
195 Matrix6xs v_partial_dq;
196 Matrix6xs a_partial_dq;
197 Matrix6xs a_partial_dv;
198 Matrix6xs a_partial_da;
199 Matrix6xs fXjdv_dq;
200 Matrix6xs fXjda_dq;
201 Matrix6xs fXjda_dv;
202 Vector3s vv;
203 Vector3s vw;
204 Matrix3s vv_skew;
205 Matrix3s vw_skew;
206 Matrix2s oRf;
207};
208
209} // namespace crocoddyl
210
211/* --- Details -------------------------------------------------------------- */
212/* --- Details -------------------------------------------------------------- */
213/* --- Details -------------------------------------------------------------- */
214#include "crocoddyl/multibody/contacts/contact-2d.hxx"
215
216#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the 2d contact model.
virtual boost::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create the 2d contact data.
const Vector2s & get_reference() const
Return the reference frame translation.
virtual void updateForce(const boost::shared_ptr< ContactDataAbstract > &data, const VectorXs &force)
Convert the force into a stack of spatial forces.
virtual void calc(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the 2d contact Jacobian and drift.
virtual void calcDiff(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the 2d contact holonomic constraint.
ContactModel2DTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector2s &xref, const Vector2s &gains=Vector2s::Zero())
Initialize the 2d contact model.
ContactModel2DTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Vector2s &xref, const std::size_t nu, const Vector2s &gains=Vector2s::Zero())
Initialize the 2d contact model.
DEPRECATED("Use get_reference() or get_id()", FrameTranslationTpl< Scalar > get_xref() const ;) const Vector2s &get_gains() const
Create the 2d contact data.
void set_reference(const Vector2s &reference)
Modify the reference frame translation.
pinocchio::FrameIndex id_
Reference frame id of the contact.
State multibody representation.
Definition: multibody.hpp:31
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:45
pinocchio::SE3Tpl< Scalar > jMf
Local frame placement of the contact frame.
Definition: force-base.hpp:46
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:45
pinocchio::ForceTpl< Scalar > f
Definition: force-base.hpp:48
MatrixXs df_du
Jacobian of the contact forces.
Definition: force-base.hpp:51
MatrixXs Jc
Contact Jacobian.
Definition: force-base.hpp:47
pinocchio::SE3Tpl< Scalar > jMf
Local frame placement of the contact frame.
Definition: force-base.hpp:46
MatrixXs df_dx
Jacobian of the contact forces.
Definition: force-base.hpp:50
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: force-base.hpp:44