crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
contact-base.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_CONTACT_BASE_HPP_
10#define CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
11
12#include "crocoddyl/multibody/fwd.hpp"
13#include "crocoddyl/core/mathbase.hpp"
14#include "crocoddyl/multibody/states/multibody.hpp"
15#include "crocoddyl/multibody/force-base.hpp"
16
17namespace crocoddyl {
18
19template <typename _Scalar>
21 public:
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23
24 typedef _Scalar Scalar;
28 typedef typename MathBase::VectorXs VectorXs;
29 typedef typename MathBase::MatrixXs MatrixXs;
30
31 ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nc, const std::size_t nu);
32 ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nc);
34
35 virtual void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
36 virtual void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x) = 0;
37
38 virtual void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) = 0;
39 void updateForceDiff(const boost::shared_ptr<ContactDataAbstract>& data, const MatrixXs& df_dx,
40 const MatrixXs& df_du) const;
41 void setZeroForce(const boost::shared_ptr<ContactDataAbstract>& data) const;
42 void setZeroForceDiff(const boost::shared_ptr<ContactDataAbstract>& data) const;
43
44 virtual boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
45
46 const boost::shared_ptr<StateMultibody>& get_state() const;
47 std::size_t get_nc() const;
48 std::size_t get_nu() const;
49
53 pinocchio::FrameIndex get_id() const;
54
58 void set_id(const pinocchio::FrameIndex id);
59
63 template <class Scalar>
64 friend std::ostream& operator<<(std::ostream& os, const ContactModelAbstractTpl<Scalar>& model);
65
71 virtual void print(std::ostream& os) const;
72
73 protected:
74 boost::shared_ptr<StateMultibody> state_;
75 std::size_t nc_;
76 std::size_t nu_;
77 pinocchio::FrameIndex id_;
78};
79
80template <typename _Scalar>
82 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83
84 typedef _Scalar Scalar;
87 typedef typename MathBase::VectorXs VectorXs;
88 typedef typename MathBase::MatrixXs MatrixXs;
89
90 template <template <typename Scalar> class Model>
91 ContactDataAbstractTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
92 : Base(model, data),
93 fXj(jMf.inverse().toActionMatrix()),
94 a0(model->get_nc()),
95 da0_dx(model->get_nc(), model->get_state()->get_ndx()) {
96 a0.setZero();
97 da0_dx.setZero();
98 }
99 virtual ~ContactDataAbstractTpl() {}
100
101 using Base::df_du;
102 using Base::df_dx;
103 using Base::f;
104 using Base::frame;
105 using Base::Jc;
106 using Base::jMf;
107 using Base::pinocchio;
108 typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
109 VectorXs a0;
110 MatrixXs da0_dx;
111};
112
113} // namespace crocoddyl
114
115/* --- Details -------------------------------------------------------------- */
116/* --- Details -------------------------------------------------------------- */
117/* --- Details -------------------------------------------------------------- */
118#include "crocoddyl/multibody/contact-base.hxx"
119
120#endif // CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
friend std::ostream & operator<<(std::ostream &os, const ContactModelAbstractTpl< Scalar > &model)
Print information on the contact model.
virtual void print(std::ostream &os) const
Print relevant information of the contact model.
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
pinocchio::FrameIndex id_
Reference frame id of the contact.
State multibody representation.
Definition: multibody.hpp:31
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