9 #ifndef SOBEC_RESIDUAL_CENTER_OF_FRICTION_HPP_ 10 #define SOBEC_RESIDUAL_CENTER_OF_FRICTION_HPP_ 12 #include <crocoddyl/core/residual-base.hpp> 13 #include <crocoddyl/core/utils/exception.hpp> 14 #include <crocoddyl/multibody/data/multibody.hpp> 15 #include <crocoddyl/multibody/fwd.hpp> 16 #include <crocoddyl/multibody/states/multibody.hpp> 17 #include <pinocchio/algorithm/jacobian.hpp> 18 #include <pinocchio/multibody/fwd.hpp> 19 #include <pinocchio/spatial/motion.hpp> 21 #include "crocoddyl/multibody/contacts/contact-6d.hpp" 22 #include "crocoddyl/multibody/contacts/multiple-contacts.hpp" 23 #include "crocoddyl/multibody/data/contacts.hpp" 40 template <
typename _Scalar>
42 :
public ResidualModelAbstractTpl<_Scalar> {
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 typedef ResidualModelAbstractTpl<Scalar>
Base;
57 typedef typename pinocchio::ForceTpl<Scalar>
Force;
71 const pinocchio::FrameIndex contact_id,
72 const std::size_t nu);
83 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract> &data,
84 const Eigen::Ref<const VectorXs> &
x,
85 const Eigen::Ref<const VectorXs> &u);
94 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract> &data,
95 const Eigen::Ref<const VectorXs> &
x,
96 const Eigen::Ref<const VectorXs> &u);
98 virtual boost::shared_ptr<ResidualDataAbstract> createData(
114 pinocchio::FrameIndex contact_id_;
117 template <
typename _Scalar>
119 :
public ResidualDataAbstractTpl<_Scalar> {
120 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124 typedef ResidualDataAbstractTpl<Scalar>
Base;
130 template <
template <
typename Scalar>
class Model>
133 :
Base(model, data) {
135 DataCollectorContactTpl<Scalar> *d =
136 dynamic_cast<DataCollectorContactTpl<Scalar> *
>(this->shared);
139 "Invalid argument: the shared data should be derived from " 140 "DataCollectorContact");
142 const pinocchio::FrameIndex
id = model->get_contact_id();
143 const boost::shared_ptr<StateMultibody> &state =
145 std::string frame_name = state->get_pinocchio()->frames[id].name;
146 bool found_contact =
false;
147 for (
auto &it : d->contacts->contacts) {
148 if (it.second->frame ==
id) {
149 ContactData6DTpl<Scalar> *d6d =
150 dynamic_cast<ContactData6DTpl<Scalar> *
>(it.second.get());
152 found_contact =
true;
153 this->contact = it.second;
157 "Domain error: there isn't defined at least a 6d contact for " +
161 if (!found_contact) {
162 throw_pretty(
"Domain error: there isn't defined contact data for " +
166 boost::shared_ptr<ForceDataAbstractTpl<Scalar> >
contact;
180 #endif // SOBEC_RESIDUAL_CENTER_OF_FRICTION_HPP_ void set_contact_id(const pinocchio::FrameIndex id)
Set the reference contact id.
Definition: residual-center-of-friction.hpp:106
pinocchio::FrameIndex get_contact_id() const
Return the reference contact id.
Definition: residual-center-of-friction.hpp:104
StateMultibodyTpl< Scalar > StateMultibody
Definition: residual-center-of-friction.hpp:51
ResidualModelAbstractTpl< Scalar > Base
Definition: residual-center-of-friction.hpp:48
MathBase::Matrix6xs Matrix6xs
Definition: residual-center-of-friction.hpp:128
boost::shared_ptr< ForceDataAbstractTpl< Scalar > > contact
Definition: residual-center-of-friction.hpp:166
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-center-of-friction.hpp:126
pinocchio::ForceTpl< Scalar > Force
Definition: residual-center-of-friction.hpp:57
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: residual-center-of-friction.hpp:46
MathBaseTpl< Scalar > MathBase
Definition: residual-center-of-friction.hpp:47
ResidualDataCenterOfFrictionTpl(Model< Scalar > *const model, DataCollectorAbstract *const data)
Definition: residual-center-of-friction.hpp:131
ResidualDataAbstractTpl< Scalar > Base
Definition: residual-center-of-friction.hpp:124
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-center-of-friction.hpp:52
StateMultibodyTpl< Scalar > StateMultibody
Definition: residual-center-of-friction.hpp:125
MathBase::MatrixXs MatrixXs
Definition: residual-center-of-friction.hpp:56
ResidualDataAbstractTpl< Scalar > ResidualDataAbstract
Definition: residual-center-of-friction.hpp:50
Definition: activation-quad-ref.hpp:19
ResidualDataCenterOfFrictionTpl< Scalar > Data
Definition: residual-center-of-friction.hpp:49
MathBase::Vector3s Vector3s
Definition: residual-center-of-friction.hpp:55
Center of friction residual.
Definition: residual-center-of-friction.hpp:41
Definition: residual-center-of-friction.hpp:118
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: residual-center-of-friction.hpp:122
MathBaseTpl< Scalar > MathBase
Definition: residual-center-of-friction.hpp:123
MathBase::VectorXs VectorXs
Definition: residual-center-of-friction.hpp:54