crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
contact-6d.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-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_6D_HPP_
10#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
11
12#include <pinocchio/spatial/motion.hpp>
13#include <pinocchio/multibody/data.hpp>
14
15#include "crocoddyl/multibody/fwd.hpp"
16#include "crocoddyl/multibody/contact-base.hpp"
17#include "crocoddyl/core/utils/deprecate.hpp"
18
19#include "crocoddyl/multibody/frames-deprecated.hpp"
20
21namespace crocoddyl {
22
23template <typename _Scalar>
25 public:
26 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27
28 typedef _Scalar Scalar;
34 typedef pinocchio::SE3Tpl<Scalar> SE3;
35 typedef typename MathBase::Vector2s Vector2s;
36 typedef typename MathBase::Vector3s Vector3s;
37 typedef typename MathBase::VectorXs VectorXs;
38
48 ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const SE3& pref,
49 const std::size_t nu, const Vector2s& gains = Vector2s::Zero());
50
61 ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const SE3& pref,
62 const Vector2s& gains = Vector2s::Zero());
63 DEPRECATED("Use constructor which is not based on FramePlacement.",
64 ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const FramePlacementTpl<Scalar>& Mref,
65 const std::size_t nu, const Vector2s& gains = Vector2s::Zero());)
66 DEPRECATED("Use constructor which is not based on FramePlacement.",
67 ContactModel6DTpl(boost::shared_ptr<StateMultibody> state, const FramePlacementTpl<Scalar>& Mref,
68 const Vector2s& gains = Vector2s::Zero());)
69 virtual ~ContactModel6DTpl();
70
78 virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
79
87 virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
88
95 virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
96
100 virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
101
105 const SE3& get_reference() const;
106 DEPRECATED("Use get_reference() or get_id()", FramePlacementTpl<Scalar> get_Mref() const;)
107
111 const Vector2s& get_gains() const;
112
116 void set_reference(const SE3& reference);
117
123 virtual void print(std::ostream& os) const;
124
125 protected:
126 using Base::id_;
127 using Base::nc_;
128 using Base::nu_;
129 using Base::state_;
130
131 private:
132 SE3 pref_;
133 Vector2s gains_;
134};
135
136template <typename _Scalar>
137struct ContactData6DTpl : public ContactDataAbstractTpl<_Scalar> {
138 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
139
140 typedef _Scalar Scalar;
143 typedef typename MathBase::Matrix6xs Matrix6xs;
144 typedef typename MathBase::Matrix6s Matrix6s;
145
146 template <template <typename Scalar> class Model>
147 ContactData6DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
148 : Base(model, data),
149 rMf(pinocchio::SE3Tpl<Scalar>::Identity()),
150 v_partial_dq(6, model->get_state()->get_nv()),
151 a_partial_dq(6, model->get_state()->get_nv()),
152 a_partial_dv(6, model->get_state()->get_nv()),
153 a_partial_da(6, model->get_state()->get_nv()) {
154 frame = model->get_id();
155 jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
156 fXj = jMf.inverse().toActionMatrix();
157 v_partial_dq.setZero();
158 a_partial_dq.setZero();
159 a_partial_dv.setZero();
160 a_partial_da.setZero();
161 rMf_Jlog6.setZero();
162 }
163
164 using Base::a0;
165 using Base::da0_dx;
166 using Base::df_du;
167 using Base::df_dx;
168 using Base::f;
169 using Base::frame;
170 using Base::fXj;
171 using Base::Jc;
172 using Base::jMf;
173 using Base::pinocchio;
174
175 pinocchio::SE3Tpl<Scalar> rMf;
176 pinocchio::MotionTpl<Scalar> v;
177 pinocchio::MotionTpl<Scalar> a;
178 Matrix6xs v_partial_dq;
179 Matrix6xs a_partial_dq;
180 Matrix6xs a_partial_dv;
181 Matrix6xs a_partial_da;
182 Matrix6s rMf_Jlog6;
183};
184
185} // namespace crocoddyl
186/* --- Details -------------------------------------------------------------- */
187/* --- Details -------------------------------------------------------------- */
188/* --- Details -------------------------------------------------------------- */
189#include "crocoddyl/multibody/contacts/contact-6d.hxx"
190
191#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
ContactModel6DTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const SE3 &pref, const std::size_t nu, const Vector2s &gains=Vector2s::Zero())
Initialize the 6d contact model.
DEPRECATED("Use get_reference() or get_id()", FramePlacementTpl< Scalar > get_Mref() const ;) const Vector2s &get_gains() const
Return the Baumgarte stabilization gains.
virtual void print(std::ostream &os) const
Print relevant information of the 6d contact model.
virtual boost::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create the 6d contact data.
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 3d contact Jacobian and drift.
virtual void calcDiff(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the 6d contact holonomic constraint.
const SE3 & get_reference() const
Return the reference frame placement.
void set_reference(const SE3 &reference)
Modify the reference frame placement.
ContactModel6DTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const SE3 &pref, const Vector2s &gains=Vector2s::Zero())
Initialize the 6d contact model.
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