crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
multiple-impulses.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
10#define CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
11
12#include <string>
13#include <map>
14#include <set>
15#include <utility>
16
17#include "crocoddyl/multibody/fwd.hpp"
18#include "crocoddyl/core/utils/exception.hpp"
19#include "crocoddyl/multibody/impulse-base.hpp"
20
21namespace crocoddyl {
22
23template <typename _Scalar>
25 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26
27 typedef _Scalar Scalar;
29
31 ImpulseItemTpl(const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse, const bool active = true)
32 : name(name), impulse(impulse), active(active) {}
33
37 friend std::ostream& operator<<(std::ostream& os, const ImpulseItemTpl<Scalar>& model) {
38 os << "{" << *model.impulse << "}";
39 return os;
40 }
41
42 std::string name;
43 boost::shared_ptr<ImpulseModelAbstract> impulse;
44 bool active;
45};
46
54template <typename _Scalar>
56 public:
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58
59 typedef _Scalar Scalar;
65
67
68 typedef typename MathBase::Vector2s Vector2s;
69 typedef typename MathBase::Vector3s Vector3s;
70 typedef typename MathBase::VectorXs VectorXs;
71 typedef typename MathBase::MatrixXs MatrixXs;
72
73 typedef std::map<std::string, boost::shared_ptr<ImpulseItem> > ImpulseModelContainer;
74 typedef std::map<std::string, boost::shared_ptr<ImpulseDataAbstract> > ImpulseDataContainer;
75 typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
76
82 explicit ImpulseModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
84
94 void addImpulse(const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse, const bool active = true);
95
101 void removeImpulse(const std::string& name);
102
109 void changeImpulseStatus(const std::string& name, const bool active);
110
117 void calc(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
118
125 void calcDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
126
133 void updateVelocity(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& vnext) const;
134
142 void updateForce(const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& impulse);
143
151 void updateVelocityDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& dvnext_dx) const;
152
160 void updateForceDiff(const boost::shared_ptr<ImpulseDataMultiple>& data, const MatrixXs& df_dx) const;
161
168 boost::shared_ptr<ImpulseDataMultiple> createData(pinocchio::DataTpl<Scalar>* const data);
169
173 const boost::shared_ptr<StateMultibody>& get_state() const;
174
178 const ImpulseModelContainer& get_impulses() const;
179
183 std::size_t get_nc() const;
184 DEPRECATED("Use get_nc().", std::size_t get_ni() const;)
185
189 std::size_t get_nc_total() const;
190 DEPRECATED("Use get_nc_total().", std::size_t get_ni_total() const;)
191
195 const std::set<std::string>& get_active_set() const;
196
200 const std::set<std::string>& get_inactive_set() const;
201
202 DEPRECATED("get_active() is deprecated and will be replaced with get_active_set()",
203 const std::vector<std::string>& get_active() {
204 active_.clear();
205 active_.reserve(active_set_.size());
206 for (const auto& contact : active_set_) {
207 active_.push_back(contact);
208 }
209 return active_;
210 };)
211
212 DEPRECATED("get_inactive() is deprecated and will be replaced with get_inactive_set()",
213 const std::vector<std::string>& get_inactive() {
214 inactive_.clear();
215 inactive_.reserve(inactive_set_.size());
216 for (const auto& contact : inactive_set_) {
217 inactive_.push_back(contact);
218 }
219 return inactive_;
220 };)
221
225 bool getImpulseStatus(const std::string& name) const;
226
230 template <class Scalar>
231 friend std::ostream& operator<<(std::ostream& os, const ImpulseModelMultipleTpl<Scalar>& model);
232
233 private:
234 boost::shared_ptr<StateMultibody> state_;
235 ImpulseModelContainer impulses_;
236 std::size_t nc_;
237 std::size_t nc_total_;
238 std::set<std::string> active_set_;
239 std::set<std::string> inactive_set_;
240
241 // Vector variants. These are to maintain the API compatibility for the deprecated syntax.
242 // These will be removed in future versions along with get_active() / get_inactive()
243 std::vector<std::string> active_;
244 std::vector<std::string> inactive_;
245};
246
252template <typename _Scalar>
254 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
255
256 typedef _Scalar Scalar;
260 typedef typename MathBase::VectorXs VectorXs;
261 typedef typename MathBase::MatrixXs MatrixXs;
262
269 template <template <typename Scalar> class Model>
270 ImpulseDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
271 : Jc(model->get_nc_total(), model->get_state()->get_nv()),
272 dv0_dq(model->get_nc_total(), model->get_state()->get_nv()),
273 vnext(model->get_state()->get_nv()),
274 dvnext_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
275 fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
276 Jc.setZero();
277 dv0_dq.setZero();
278 vnext.setZero();
279 dvnext_dx.setZero();
280 for (typename ImpulseModelMultiple::ImpulseModelContainer::const_iterator it = model->get_impulses().begin();
281 it != model->get_impulses().end(); ++it) {
282 const boost::shared_ptr<ImpulseItem>& item = it->second;
283 impulses.insert(std::make_pair(item->name, item->impulse->createData(data)));
284 }
285 }
286
287 MatrixXs Jc;
289 MatrixXs
293 VectorXs vnext;
295 MatrixXs dvnext_dx;
297 typename ImpulseModelMultiple::ImpulseDataContainer impulses;
298 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
300};
301
302} // namespace crocoddyl
303
304/* --- Details -------------------------------------------------------------- */
305/* --- Details -------------------------------------------------------------- */
306/* --- Details -------------------------------------------------------------- */
307#include "crocoddyl/multibody/impulses/multiple-impulses.hxx"
308
309#endif // CROCODDYL_MULTIBODY_IMPULSES_MULTIPLE_IMPULSES_HPP_
Define a stack of impulse models.
void updateForceDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &df_dx) const
Update the Jacobian of the spatial impulse defined in frame coordinate.
std::size_t get_nc() const
Return the dimension of active impulses.
boost::shared_ptr< ImpulseDataMultiple > createData(pinocchio::DataTpl< Scalar > *const data)
Create the multi-impulse data.
void addImpulse(const std::string &name, boost::shared_ptr< ImpulseModelAbstract > impulse, const bool active=true)
Add impulse item.
void calcDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the contact holonomic constraint.
void updateVelocity(const boost::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &vnext) const
Update the system velocity after impulse.
DEPRECATED("Use get_nc().", std::size_t get_ni() const ;) std DEPRECATED("Use get_nc_total().", std::size_t get_ni_total() const ;) const std const std::set< std::string > & get_inactive_set() const
Return the dimension of all impulses.
void updateForce(const boost::shared_ptr< ImpulseDataMultiple > &data, const VectorXs &impulse)
Update the spatial impulse defined in frame coordinate.
void calc(const boost::shared_ptr< ImpulseDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total contact Jacobian and contact acceleration.
const boost::shared_ptr< StateMultibody > & get_state() const
Return the multibody state.
void updateVelocityDiff(const boost::shared_ptr< ImpulseDataMultiple > &data, const MatrixXs &dvnext_dx) const
Update the Jacobian of the system velocity after impulse.
ImpulseModelMultipleTpl(boost::shared_ptr< StateMultibody > state)
Initialize the multi-impulse model.
bool getImpulseStatus(const std::string &name) const
Return the status of a given impulse name.
void changeImpulseStatus(const std::string &name, const bool active)
Change the impulse status.
void removeImpulse(const std::string &name)
Remove impulse item.
const ImpulseModelContainer & get_impulses() const
Return the impulse models.
friend std::ostream & operator<<(std::ostream &os, const ImpulseModelMultipleTpl< Scalar > &model)
Print information on the impulse models.
State multibody representation.
Definition: multibody.hpp:31
Define the multi-impulse data.
ImpulseDataMultipleTpl(Model< Scalar > *const model, pinocchio::DataTpl< Scalar > *const data)
Initialized a multi-impulse data.
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > > fext
External spatial forces in body coordinates.
ImpulseModelMultiple::ImpulseDataContainer impulses
Stack of impulse data.
friend std::ostream & operator<<(std::ostream &os, const ImpulseItemTpl< Scalar > &model)
Print information on the impulse item.