crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
contact-cop-position.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2020-2021, University of Duisburg-Essen, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
10#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
11
12#include "crocoddyl/multibody/fwd.hpp"
13#include "crocoddyl/core/residual-base.hpp"
14#include "crocoddyl/multibody/states/multibody.hpp"
15#include "crocoddyl/multibody/contact-base.hpp"
16#include "crocoddyl/multibody/impulse-base.hpp"
17#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
18#include "crocoddyl/multibody/contacts/contact-3d.hpp"
19#include "crocoddyl/multibody/contacts/contact-6d.hpp"
20#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
21#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
22#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
23#include "crocoddyl/multibody/data/contacts.hpp"
24#include "crocoddyl/multibody/data/impulses.hpp"
25#include "crocoddyl/multibody/cop-support.hpp"
26#include "crocoddyl/core/utils/exception.hpp"
27
28namespace crocoddyl {
29
62template <typename _Scalar>
64 public:
65 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66
67 typedef _Scalar Scalar;
75 typedef typename MathBase::VectorXs VectorXs;
76 typedef typename MathBase::MatrixXs MatrixXs;
77 typedef typename MathBase::Matrix46s Matrix46;
78
87 ResidualModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
88 const CoPSupport& cref, const std::size_t nu);
89
99 ResidualModelContactCoPPositionTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
100 const CoPSupport& cref);
102
114 virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
115 const Eigen::Ref<const VectorXs>& u);
116
126 virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
127
139 virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
140 const Eigen::Ref<const VectorXs>& u);
141
151 virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
152
162 virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
163
167 pinocchio::FrameIndex get_id() const;
168
172 const CoPSupport& get_reference() const;
173
177 void set_id(pinocchio::FrameIndex id);
178
182 void set_reference(const CoPSupport& reference);
183
189 virtual void print(std::ostream& os) const;
190
191 protected:
192 using Base::nu_;
193 using Base::state_;
194 using Base::unone_;
195
196 private:
197 pinocchio::FrameIndex id_;
198 CoPSupport cref_;
199};
200
201template <typename _Scalar>
203 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
204
205 typedef _Scalar Scalar;
211 typedef typename MathBase::MatrixXs MatrixXs;
212
213 template <template <typename Scalar> class Model>
214 ResidualDataContactCoPPositionTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
215 : Base(model, data) {
216 // Check that proper shared data has been passed
217 bool is_contact = true;
220 if (d1 == NULL && d2 == NULL) {
221 throw_pretty(
222 "Invalid argument: the shared data should be derived from DataCollectorContact or DataCollectorImpulse");
223 }
224 if (d2 != NULL) {
225 is_contact = false;
226 }
227
228 // Avoids data casting at runtime
229 const pinocchio::FrameIndex id = model->get_id();
230 const boost::shared_ptr<StateMultibody>& state = boost::static_pointer_cast<StateMultibody>(model->get_state());
231 std::string frame_name = state->get_pinocchio()->frames[id].name;
232 bool found_contact = false;
233 if (is_contact) {
234 for (typename ContactModelMultiple::ContactDataContainer::iterator it = d1->contacts->contacts.begin();
235 it != d1->contacts->contacts.end(); ++it) {
236 if (it->second->frame == id) {
237 ContactData3DTpl<Scalar>* d3d = dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
238 if (d3d != NULL) {
239 found_contact = true;
240 contact = it->second;
241 throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
242 break;
243 }
244 ContactData6DTpl<Scalar>* d6d = dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
245 if (d6d != NULL) {
246 found_contact = true;
247 contact = it->second;
248 break;
249 }
250 throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
251 break;
252 }
253 }
254 } else {
255 for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it = d2->impulses->impulses.begin();
256 it != d2->impulses->impulses.end(); ++it) {
257 if (it->second->frame == id) {
258 ImpulseData3DTpl<Scalar>* d3d = dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
259 if (d3d != NULL) {
260 found_contact = true;
261 contact = it->second;
262 throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
263 break;
264 }
265 ImpulseData6DTpl<Scalar>* d6d = dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
266 if (d6d != NULL) {
267 found_contact = true;
268 contact = it->second;
269 break;
270 }
271 throw_pretty("Domain error: there isn't defined at least a 6d contact for " + frame_name);
272 break;
273 }
274 }
275 }
276 if (!found_contact) {
277 throw_pretty("Domain error: there isn't defined contact data for " + frame_name);
278 }
279 }
280
281 pinocchio::DataTpl<Scalar>* pinocchio;
282 boost::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
283 using Base::r;
284 using Base::Ru;
285 using Base::Rx;
286 using Base::shared;
287};
288
289} // namespace crocoddyl
290
291/* --- Details -------------------------------------------------------------- */
292/* --- Details -------------------------------------------------------------- */
293/* --- Details -------------------------------------------------------------- */
294#include "crocoddyl/multibody/residuals/contact-cop-position.hxx"
295
296#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
This class encapsulates a center of pressure support of a 6d contact.
Definition: cop-support.hpp:25
Abstract class for residual models.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
VectorXs unone_
No control vector.
Center of pressure residual function.
void set_reference(const CoPSupport &reference)
Modify the reference support region of CoP.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the residual vector for nodes that depends only on the state.
void set_id(pinocchio::FrameIndex id)
Modify the reference frame id.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the contact CoP residual data.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the Jacobian of the residual functions with respect to the state only.
virtual void print(std::ostream &os) const
Print relevant information of the cop-position residual.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact CoP residual.
const CoPSupport & get_reference() const
Return the reference support region of CoP.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the contact CoP residual.
ResidualModelContactCoPPositionTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const CoPSupport &cref, const std::size_t nu)
Initialize the contact CoP residual model.
ResidualModelContactCoPPositionTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const CoPSupport &cref)
Initialize the contact CoP residual model.
State multibody representation.
Definition: multibody.hpp:31
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.
boost::shared_ptr< ForceDataAbstractTpl< Scalar > > contact
Contact force.
DataCollectorAbstract * shared
Shared data allocated by the action model.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.