crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
multiple-contacts.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_CONTACTS_MULTIPLE_CONTACTS_HPP_
10#define CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_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/contact-base.hpp"
20
21namespace crocoddyl {
22
23template <typename _Scalar>
25 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26
27 typedef _Scalar Scalar;
29
31 ContactItemTpl(const std::string& name, boost::shared_ptr<ContactModelAbstract> contact, const bool active = true)
32 : name(name), contact(contact), active(active) {}
33
37 friend std::ostream& operator<<(std::ostream& os, const ContactItemTpl<Scalar>& model) {
38 os << "{" << *model.contact << "}";
39 return os;
40 }
41
42 std::string name;
43 boost::shared_ptr<ContactModelAbstract> contact;
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<ContactItem> > ContactModelContainer;
74 typedef std::map<std::string, boost::shared_ptr<ContactDataAbstract> > ContactDataContainer;
75 typedef typename pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >::iterator ForceIterator;
76
83 ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state, const std::size_t nu);
84
90 ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state);
92
102 void addContact(const std::string& name, boost::shared_ptr<ContactModelAbstract> contact, const bool active = true);
103
109 void removeContact(const std::string& name);
110
117 void changeContactStatus(const std::string& name, const bool active);
118
125 void calc(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
126
133 void calcDiff(const boost::shared_ptr<ContactDataMultiple>& data, const Eigen::Ref<const VectorXs>& x);
134
141 void updateAcceleration(const boost::shared_ptr<ContactDataMultiple>& data, const VectorXs& dv) const;
142
150 void updateForce(const boost::shared_ptr<ContactDataMultiple>& data, const VectorXs& force);
151
159 void updateAccelerationDiff(const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& ddv_dx) const;
160
170 void updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& df_dx,
171 const MatrixXs& df_du) const;
172
179 boost::shared_ptr<ContactDataMultiple> createData(pinocchio::DataTpl<Scalar>* const data);
180
184 const boost::shared_ptr<StateMultibody>& get_state() const;
185
189 const ContactModelContainer& get_contacts() const;
190
194 std::size_t get_nc() const;
195
199 std::size_t get_nc_total() const;
200
204 std::size_t get_nu() const;
205
209 const std::set<std::string>& get_active_set() const;
210
214 const std::set<std::string>& get_inactive_set() const;
215
216 DEPRECATED("get_active() is deprecated and will be replaced with get_active_set()",
217 const std::vector<std::string>& get_active() {
218 active_.clear();
219 active_.reserve(active_set_.size());
220 for (const auto& contact : active_set_) {
221 active_.push_back(contact);
222 }
223 return active_;
224 };)
225
226 DEPRECATED("get_inactive() is deprecated and will be replaced with get_inactive_set()",
227 const std::vector<std::string>& get_inactive() {
228 inactive_.clear();
229 inactive_.reserve(inactive_set_.size());
230 for (const auto& contact : inactive_set_) {
231 inactive_.push_back(contact);
232 }
233 return inactive_;
234 };)
235
239 bool getContactStatus(const std::string& name) const;
240
244 template <class Scalar>
245 friend std::ostream& operator<<(std::ostream& os, const ContactModelMultipleTpl<Scalar>& model);
246
247 private:
248 boost::shared_ptr<StateMultibody> state_;
249 ContactModelContainer contacts_;
250 std::size_t nc_;
251 std::size_t nc_total_;
252 std::size_t nu_;
253 std::set<std::string> active_set_;
254 std::set<std::string> inactive_set_;
255
256 // Vector variants. These are to maintain the API compatibility for the deprecated syntax.
257 // These will be removed in future versions along with get_active() / get_inactive()
258 std::vector<std::string> active_;
259 std::vector<std::string> inactive_;
260};
261
267template <typename _Scalar>
269 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
270
271 typedef _Scalar Scalar;
275 typedef typename MathBase::VectorXs VectorXs;
276 typedef typename MathBase::MatrixXs MatrixXs;
277
284 template <template <typename Scalar> class Model>
285 ContactDataMultipleTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
286 : Jc(model->get_nc_total(), model->get_state()->get_nv()),
287 a0(model->get_nc_total()),
288 da0_dx(model->get_nc_total(), model->get_state()->get_ndx()),
289 dv(model->get_state()->get_nv()),
290 ddv_dx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
291 fext(model->get_state()->get_pinocchio()->njoints, pinocchio::ForceTpl<Scalar>::Zero()) {
292 Jc.setZero();
293 a0.setZero();
294 da0_dx.setZero();
295 dv.setZero();
296 ddv_dx.setZero();
297 for (typename ContactModelMultiple::ContactModelContainer::const_iterator it = model->get_contacts().begin();
298 it != model->get_contacts().end(); ++it) {
299 const boost::shared_ptr<ContactItem>& item = it->second;
300 contacts.insert(std::make_pair(item->name, item->contact->createData(data)));
301 }
302 }
303
304 MatrixXs Jc;
306 VectorXs a0;
309 MatrixXs
313 VectorXs
315 MatrixXs ddv_dx;
317 typename ContactModelMultiple::ContactDataContainer contacts;
318 pinocchio::container::aligned_vector<pinocchio::ForceTpl<Scalar> >
320};
321
322} // namespace crocoddyl
323
324/* --- Details -------------------------------------------------------------- */
325/* --- Details -------------------------------------------------------------- */
326/* --- Details -------------------------------------------------------------- */
327#include "crocoddyl/multibody/contacts/multiple-contacts.hxx"
328
329#endif // CROCODDYL_MULTIBODY_CONTACTS_MULTIPLE_CONTACTS_HPP_
Define a stack of contact models.
ContactModelMultipleTpl(boost::shared_ptr< StateMultibody > state, const std::size_t nu)
Initialize the multi-contact model.
friend std::ostream & operator<<(std::ostream &os, const ContactModelMultipleTpl< Scalar > &model)
Print information on the contact models.
std::size_t get_nc() const
Return the dimension of active contacts.
const std::set< std::string > & get_active_set() const
Return the names of the set of active contacts.
const std::set< std::string > & get_inactive_set() const
Return the names of the set of inactive contacts.
const ContactModelContainer & get_contacts() const
Return the contact models.
void addContact(const std::string &name, boost::shared_ptr< ContactModelAbstract > contact, const bool active=true)
Add contact item.
const boost::shared_ptr< StateMultibody > & get_state() const
Return the multibody state.
void updateForceDiff(const boost::shared_ptr< ContactDataMultiple > &data, const MatrixXs &df_dx, const MatrixXs &df_du) const
Update the Jacobian of the spatial force defined in frame coordinate.
void changeContactStatus(const std::string &name, const bool active)
Change the contact status.
void removeContact(const std::string &name)
Remove contact item.
bool getContactStatus(const std::string &name) const
Return the status of a given contact name.
void updateAccelerationDiff(const boost::shared_ptr< ContactDataMultiple > &data, const MatrixXs &ddv_dx) const
Update the Jacobian of the constrained system acceleration.
std::size_t get_nc_total() const
Return the dimension of all contacts.
void calc(const boost::shared_ptr< ContactDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the contact Jacobian and contact acceleration.
void updateAcceleration(const boost::shared_ptr< ContactDataMultiple > &data, const VectorXs &dv) const
Update the constrained system acceleration.
void calcDiff(const boost::shared_ptr< ContactDataMultiple > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the contact holonomic constraint.
boost::shared_ptr< ContactDataMultiple > createData(pinocchio::DataTpl< Scalar > *const data)
Create the multi-contact data.
ContactModelMultipleTpl(boost::shared_ptr< StateMultibody > state)
Initialize the multi-contact model.
void updateForce(const boost::shared_ptr< ContactDataMultiple > &data, const VectorXs &force)
Update the spatial force defined in frame coordinate.
std::size_t get_nu() const
Return the dimension of control vector.
State multibody representation.
Definition: multibody.hpp:31
Define the multi-contact data.
VectorXs dv
Constrained system acceleration in generalized coordinates .
ContactModelMultiple::ContactDataContainer contacts
Stack of contact data.
ContactDataMultipleTpl(Model< Scalar > *const model, pinocchio::DataTpl< Scalar > *const data)
Initialized a multi-contact data.
pinocchio::container::aligned_vector< pinocchio::ForceTpl< Scalar > > fext
External spatial forces in body coordinates.
friend std::ostream & operator<<(std::ostream &os, const ContactItemTpl< Scalar > &model)
Print information on the contact item.