crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
impulse-fwddyn.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
10#define CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
11
12#include <stdexcept>
13
14#include <pinocchio/algorithm/compute-all-terms.hpp>
15#include <pinocchio/algorithm/frames.hpp>
16#include <pinocchio/algorithm/contact-dynamics.hpp>
17#include <pinocchio/algorithm/centroidal.hpp>
18#include <pinocchio/algorithm/rnea-derivatives.hpp>
19#include <pinocchio/algorithm/kinematics-derivatives.hpp>
20
21#include "crocoddyl/multibody/fwd.hpp"
22#include "crocoddyl/core/utils/exception.hpp"
23#include "crocoddyl/core/action-base.hpp"
24#include "crocoddyl/core/costs/cost-sum.hpp"
25#include "crocoddyl/multibody/states/multibody.hpp"
26#include "crocoddyl/multibody/actuations/floating-base.hpp"
27#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
28#include "crocoddyl/multibody/data/impulses.hpp"
29#include "crocoddyl/multibody/actions/impulse-fwddyn.hpp"
30
31namespace crocoddyl {
32
61template <typename _Scalar>
63 public:
64 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65
66 typedef _Scalar Scalar;
74 typedef typename MathBase::VectorXs VectorXs;
75 typedef typename MathBase::MatrixXs MatrixXs;
76
91 ActionModelImpulseFwdDynamicsTpl(boost::shared_ptr<StateMultibody> state,
92 boost::shared_ptr<ImpulseModelMultiple> impulses,
93 boost::shared_ptr<CostModelSum> costs, const Scalar r_coeff = Scalar(0.),
94 const Scalar JMinvJt_damping = Scalar(0.), const bool enable_force = false);
96
106 virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
107 const Eigen::Ref<const VectorXs>& u);
108
116 virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
117 const Eigen::Ref<const VectorXs>& u);
118
124 virtual boost::shared_ptr<ActionDataAbstract> createData();
125
129 virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
130
134 const boost::shared_ptr<ImpulseModelMultiple>& get_impulses() const;
135
139 const boost::shared_ptr<CostModelSum>& get_costs() const;
140
144 pinocchio::ModelTpl<Scalar>& get_pinocchio() const;
145
149 const VectorXs& get_armature() const;
150
154 const Scalar get_restitution_coefficient() const;
155
159 const Scalar get_damping_factor() const;
160
164 void set_armature(const VectorXs& armature);
165
169 void set_restitution_coefficient(const Scalar r_coeff);
170
174 void set_damping_factor(const Scalar damping);
175
181 virtual void print(std::ostream& os) const;
182
183 protected:
184 using Base::state_;
185
186 private:
187 boost::shared_ptr<ImpulseModelMultiple> impulses_;
188 boost::shared_ptr<CostModelSum> costs_;
189 pinocchio::ModelTpl<Scalar>& pinocchio_;
190 bool with_armature_;
191 VectorXs armature_;
192 Scalar r_coeff_;
193 Scalar JMinvJt_damping_;
194 bool enable_force_;
195 pinocchio::MotionTpl<Scalar> gravity_;
196};
197
198template <typename _Scalar>
200 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
201 typedef _Scalar Scalar;
204 typedef typename MathBase::VectorXs VectorXs;
205 typedef typename MathBase::MatrixXs MatrixXs;
206
207 template <template <typename Scalar> class Model>
208 explicit ActionDataImpulseFwdDynamicsTpl(Model<Scalar>* const model)
209 : Base(model),
210 pinocchio(pinocchio::DataTpl<Scalar>(model->get_pinocchio())),
211 multibody(&pinocchio, model->get_impulses()->createData(&pinocchio)),
212 costs(model->get_costs()->createData(&multibody)),
213 vnone(model->get_state()->get_nv()),
214 Kinv(model->get_state()->get_nv() + model->get_impulses()->get_nc_total(),
215 model->get_state()->get_nv() + model->get_impulses()->get_nc_total()),
216 df_dx(model->get_impulses()->get_nc_total(), model->get_state()->get_ndx()),
217 dgrav_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
218 costs->shareMemory(this);
219 vnone.setZero();
220 Kinv.setZero();
221 df_dx.setZero();
222 dgrav_dq.setZero();
223 }
224
225 pinocchio::DataTpl<Scalar> pinocchio;
227 boost::shared_ptr<CostDataSumTpl<Scalar> > costs;
228 VectorXs vnone;
229 MatrixXs Kinv;
230 MatrixXs df_dx;
231 MatrixXs dgrav_dq;
232};
233
234} // namespace crocoddyl
235
236/* --- Details -------------------------------------------------------------- */
237/* --- Details -------------------------------------------------------------- */
238/* --- Details -------------------------------------------------------------- */
239#include <crocoddyl/multibody/actions/impulse-fwddyn.hxx>
240
241#endif // CROCODDYL_MULTIBODY_ACTIONS_IMPULSE_FWDDYN_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
boost::shared_ptr< StateAbstract > state_
Model of the state.
Action model for impulse forward dynamics in multibody systems.
void set_restitution_coefficient(const Scalar r_coeff)
Modify the restituion coefficient.
virtual void print(std::ostream &os) const
Print relevant information of the impulse forward-dynamics model.
const VectorXs & get_armature() const
Return the armature vector.
void set_damping_factor(const Scalar damping)
Modify the damping factor used in the operational space inertia matrix.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the system acceleration, and cost value.
const boost::shared_ptr< ImpulseModelMultiple > & get_impulses() const
Return the impulse model.
const Scalar get_restitution_coefficient() const
Return the restituion coefficient.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the impulse dynamics, and cost function.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the impulse forward-dynamics data.
void set_armature(const VectorXs &armature)
Modify the armature vector.
const boost::shared_ptr< CostModelSum > & get_costs() const
Return the cost model.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Check that the given data belongs to the impulse forward-dynamics data.
const Scalar get_damping_factor() const
Return the damping factor used in the operational space inertia matrix.
pinocchio::ModelTpl< Scalar > & get_pinocchio() const
Return the Pinocchio model.
ActionModelImpulseFwdDynamicsTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ImpulseModelMultiple > impulses, boost::shared_ptr< CostModelSum > costs, const Scalar r_coeff=Scalar(0.), const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false)
Initialize the impulse forward-dynamics action model.
Summation of individual cost models.
Definition: cost-sum.hpp:65
Define a stack of impulse models.
State multibody representation.
Definition: multibody.hpp:31