10#ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
11#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
13#include "crocoddyl/core/residual-base.hpp"
14#include "crocoddyl/multibody/contact-base.hpp"
15#include "crocoddyl/multibody/contacts/contact-3d.hpp"
16#include "crocoddyl/multibody/contacts/contact-6d.hpp"
17#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
18#include "crocoddyl/multibody/cop-support.hpp"
19#include "crocoddyl/multibody/data/contacts.hpp"
20#include "crocoddyl/multibody/data/impulses.hpp"
21#include "crocoddyl/multibody/fwd.hpp"
22#include "crocoddyl/multibody/impulse-base.hpp"
23#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
24#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
25#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
26#include "crocoddyl/multibody/states/multibody.hpp"
65template <
typename _Scalar>
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72 typedef _Scalar Scalar;
80 typedef typename MathBase::VectorXs VectorXs;
81 typedef typename MathBase::MatrixXs MatrixXs;
82 typedef typename MathBase::Matrix46s Matrix46;
98 const pinocchio::FrameIndex
id,
100 const std::size_t nu,
101 const bool fwddyn =
true);
114 const pinocchio::FrameIndex
id,
130 virtual void calc(
const std::shared_ptr<ResidualDataAbstract>& data,
131 const Eigen::Ref<const VectorXs>& x,
132 const Eigen::Ref<const VectorXs>& u)
override;
144 virtual void calc(
const std::shared_ptr<ResidualDataAbstract>& data,
145 const Eigen::Ref<const VectorXs>& x)
override;
159 virtual void calcDiff(
const std::shared_ptr<ResidualDataAbstract>& data,
160 const Eigen::Ref<const VectorXs>& x,
161 const Eigen::Ref<const VectorXs>& u)
override;
174 virtual void calcDiff(
const std::shared_ptr<ResidualDataAbstract>& data,
175 const Eigen::Ref<const VectorXs>& x)
override;
207 template <
typename NewScalar>
230 void set_id(pinocchio::FrameIndex
id);)
235 void set_reference(
const CoPSupport& reference);
242 virtual void print(std::ostream& os)
const override;
251 bool update_jacobians_;
253 pinocchio::FrameIndex id_;
257template <
typename _Scalar>
260 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
262 typedef _Scalar Scalar;
267 typedef typename MathBase::MatrixXs MatrixXs;
269 template <
template <
typename Scalar>
class Model>
272 :
Base(model, data) {
274 bool is_contact =
true;
279 if (d1 == NULL && d2 == NULL) {
281 "Invalid argument: the shared data should be derived from "
282 "DataCollectorContact or DataCollectorImpulse");
289 const pinocchio::FrameIndex
id = model->get_id();
290 const std::shared_ptr<StateMultibody>& state =
291 std::static_pointer_cast<StateMultibody>(model->get_state());
292 std::string frame_name = state->get_pinocchio()->frames[id].name;
293 bool found_contact =
false;
296 Scalar>::ContactDataContainer::iterator it =
297 d1->contacts->contacts.begin();
298 it != d1->contacts->contacts.end(); ++it) {
299 if (it->second->frame ==
id) {
303 found_contact =
true;
306 "Domain error: there isn't defined at least a 6d contact for " +
313 found_contact =
true;
318 "Domain error: there isn't defined at least a 6d contact for " +
325 Scalar>::ImpulseDataContainer::iterator it =
326 d2->impulses->impulses.begin();
327 it != d2->impulses->impulses.end(); ++it) {
328 if (it->second->frame ==
id) {
332 found_contact =
true;
335 "Domain error: there isn't defined at least a 6d contact for " +
342 found_contact =
true;
347 "Domain error: there isn't defined at least a 6d contact for " +
353 if (!found_contact) {
354 throw_pretty(
"Domain error: there isn't defined contact data for " +
361 std::shared_ptr<ForceDataAbstractTpl<Scalar> >
contact;
373#include "crocoddyl/multibody/residuals/contact-cop-position.hxx"
375CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
377CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
This class encapsulates a center of pressure support of a 6d contact.
Define a stack of impulse models.
Abstract class for residual models.
std::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
State multibody representation.
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.
VectorXs r
Residual vector.