Crocoddyl
 
Loading...
Searching...
No Matches
contact.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
11#define CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
12
13#include <boost/function.hpp>
14
15#include "crocoddyl/multibody/contact-base.hpp"
16#include "crocoddyl/multibody/fwd.hpp"
17
18namespace crocoddyl {
19
20template <typename _Scalar>
22 public:
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModelNumDiffTpl)
25
26 typedef _Scalar Scalar;
31 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
32 typedef boost::function<void(const VectorXs&, const VectorXs&)>
33 ReevaluationFunction;
34
41 explicit ContactModelNumDiffTpl(const std::shared_ptr<Base>& model);
42
46 virtual ~ContactModelNumDiffTpl() = default;
47
51 void calc(const std::shared_ptr<ContactDataAbstract>& data,
52 const Eigen::Ref<const VectorXs>& x) override;
53
57 void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
58 const Eigen::Ref<const VectorXs>& x) override;
59
63 void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
64 const VectorXs& force) override;
65
72 std::shared_ptr<ContactDataAbstract> createData(
73 pinocchio::DataTpl<Scalar>* const data) override;
74
75 template <typename NewScalar>
77
81 const std::shared_ptr<Base>& get_model() const;
82
87 const Scalar get_disturbance() const;
88
93 void set_disturbance(const Scalar disturbance);
94
102 void set_reevals(const std::vector<ReevaluationFunction>& reevals);
103
104 protected:
105 using Base::nc_;
106 using Base::nu_;
107 using Base::state_;
108
109 std::shared_ptr<Base> model_;
110 Scalar e_jac_;
112 std::vector<ReevaluationFunction>
114
115 private:
127 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
128};
129
130template <typename _Scalar>
132 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
133
134 typedef _Scalar Scalar;
137 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
138
139 template <template <typename Scalar> class Model>
140 explicit ContactDataNumDiffTpl(Model<Scalar>* const model,
141 pinocchio::DataTpl<Scalar>* const data)
142 : Base(model, data),
143 dx(model->get_state()->get_ndx()),
144 xp(model->get_state()->get_nx()) {
145 dx.setZero();
146 xp.setZero();
147
148 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
149 data_0 = model->get_model()->createData(data);
150 for (std::size_t i = 0; i < ndx; ++i) {
151 data_x.push_back(model->get_model()->createData(data));
152 }
153 }
154
155 virtual ~ContactDataNumDiffTpl() {}
156
157 using Base::a0;
158 using Base::da0_dx;
159 using Base::f;
160 using Base::pinocchio;
161
162 Scalar x_norm;
163 Scalar
165 VectorXs dx;
166 VectorXs xp;
168 std::shared_ptr<Base> data_0;
169 std::vector<std::shared_ptr<Base> >
171};
172
173} // namespace crocoddyl
174
175/* --- Details -------------------------------------------------------------- */
176/* --- Details -------------------------------------------------------------- */
177/* --- Details -------------------------------------------------------------- */
178#include "crocoddyl/multibody/numdiff/contact.hxx"
179
180#endif // CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
std::shared_ptr< Base > model_
contact model to differentiate
Definition contact.hpp:109
ContactModelNumDiffTpl(const std::shared_ptr< Base > &model)
Construct a new ContactModelNumDiff object from a ContactModelAbstract.
const std::shared_ptr< Base > & get_model() const
Return the acton model that we use to numerical differentiate.
const Scalar get_disturbance() const
Return the disturbance constant used in the numerical differentiation routine.
void set_reevals(const std::vector< ReevaluationFunction > &reevals)
Register functions that take a pinocchio model, a pinocchio data, a state and a control....
void updateForce(const std::shared_ptr< ContactDataAbstract > &data, const VectorXs &force) override
Convert the force into a stack of spatial forces.
void set_disturbance(const Scalar disturbance)
Modify the disturbance constant used in the numerical differentiation routine.
std::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data) override
Create a Data object.
virtual ~ContactModelNumDiffTpl()=default
Default destructor of the ContactModelNumDiff object.
void calc(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the contact Jacobian and acceleration drift.
std::vector< ReevaluationFunction > reevals_
functions that need execution before calc or calcDiff
Definition contact.hpp:113
void calcDiff(const std::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the acceleration-based contact.
Scalar x_norm
Norm of the state vector.
Definition contact.hpp:162
std::vector< std::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition contact.hpp:170
Scalar xh_jac
Disturbance value used for computing .
Definition contact.hpp:164
std::shared_ptr< Base > data_0
The data at the approximation point.
Definition contact.hpp:168
VectorXs dx
State disturbance.
Definition contact.hpp:165
PinocchioData * pinocchio
Pinocchio data.
Force f
Contact force expressed in the coordinate defined by type.