crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
contact.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2020, University of Edinburgh, LAAS-CNRS
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
10#define CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
11
12#include <boost/function.hpp>
13#include "crocoddyl/multibody/fwd.hpp"
14#include "crocoddyl/multibody/contact-base.hpp"
15
16namespace crocoddyl {
17
18template <typename _Scalar>
20 public:
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22
23 typedef _Scalar Scalar;
28 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
29 typedef boost::function<void(const VectorXs&, const VectorXs&)> ReevaluationFunction;
30
36 explicit ContactModelNumDiffTpl(const boost::shared_ptr<Base>& model);
37
42
46 void calc(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
47
51 void calcDiff(const boost::shared_ptr<ContactDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
52
56 void updateForce(const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force);
57
64 boost::shared_ptr<ContactDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
65
71 const boost::shared_ptr<Base>& get_model() const;
72
78 const Scalar get_disturbance() const;
79
85 void set_disturbance(const Scalar disturbance);
86
93 void set_reevals(const std::vector<ReevaluationFunction>& reevals);
94
95 protected:
96 using Base::nc_;
97 using Base::nu_;
98 using Base::state_;
99
101 boost::shared_ptr<Base> model_;
102
105
107 std::vector<ReevaluationFunction> reevals_;
108
109 private:
121 void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
122};
123
124template <typename _Scalar>
126 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
127
128 typedef _Scalar Scalar;
131 typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
132
133 template <template <typename Scalar> class Model>
134 explicit ContactDataNumDiffTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
135 : Base(model, data), dx(model->get_state()->get_ndx()), xp(model->get_state()->get_nx()) {
136 dx.setZero();
137 xp.setZero();
138
139 const std::size_t ndx = model->get_model()->get_state()->get_ndx();
140 data_0 = model->get_model()->createData(data);
141 for (std::size_t i = 0; i < ndx; ++i) {
142 data_x.push_back(model->get_model()->createData(data));
143 }
144 }
145
146 virtual ~ContactDataNumDiffTpl() {}
147
148 using Base::a0;
149 using Base::da0_dx;
150 using Base::f;
151 using Base::pinocchio;
152
153 VectorXs dx;
154 VectorXs xp;
155 boost::shared_ptr<Base> data_0;
156 std::vector<boost::shared_ptr<Base> > data_x;
157};
158
159} // namespace crocoddyl
160
161/* --- Details -------------------------------------------------------------- */
162/* --- Details -------------------------------------------------------------- */
163/* --- Details -------------------------------------------------------------- */
164#include "crocoddyl/multibody/numdiff/contact.hxx"
165
166#endif // CROCODDYL_MULTIBODY_NUMDIFF_CONTACT_HPP_
Scalar disturbance_
Numerical disturbance used in the numerical differentiation.
Definition: contact.hpp:104
const boost::shared_ptr< Base > & get_model() const
Get the model_ object.
void calc(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
void updateForce(const boost::shared_ptr< ContactDataAbstract > &data, const VectorXs &force)
const Scalar get_disturbance() const
Get the disturbance_ object.
void set_reevals(const std::vector< ReevaluationFunction > &reevals)
Register functions that take a pinocchio model, a pinocchio data, a state and a control....
void calcDiff(const boost::shared_ptr< ContactDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
void set_disturbance(const Scalar disturbance)
Set the disturbance_ object.
boost::shared_ptr< ContactDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data)
Create a Data object.
boost::shared_ptr< Base > model_
Model of the Contact.
Definition: contact.hpp:101
ContactModelNumDiffTpl(const boost::shared_ptr< Base > &model)
Construct a new ContactModelNumDiff object from a ContactModelAbstract.
std::vector< ReevaluationFunction > reevals_
Functions that needs execution before calc or calcDiff.
Definition: contact.hpp:107
virtual ~ContactModelNumDiffTpl()
Default destructor of the ContactModelNumDiff object.
pinocchio::ForceTpl< Scalar > f
Definition: force-base.hpp:48
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: force-base.hpp:44
boost::shared_ptr< Base > data_0
The data at the approximation point.
Definition: contact.hpp:155
VectorXs xp
The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
Definition: contact.hpp:154
std::vector< boost::shared_ptr< Base > > data_x
The temporary data associated with the state variation.
Definition: contact.hpp:156
VectorXs dx
State disturbance.
Definition: contact.hpp:153