9 #ifndef SOBEC_RESIDUAL_FEET_COLLISION_HPP_
10 #define SOBEC_RESIDUAL_FEET_COLLISION_HPP_
12 #include <crocoddyl/core/residual-base.hpp>
13 #include <crocoddyl/multibody/data/multibody.hpp>
14 #include <crocoddyl/multibody/fwd.hpp>
15 #include <crocoddyl/multibody/states/multibody.hpp>
20 using namespace crocoddyl;
28 template <
typename _Scalar>
31 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 typedef ResidualModelAbstractTpl<Scalar>
Base;
52 const pinocchio::FrameIndex frame_id1,
53 const pinocchio::FrameIndex frame_id2,
54 const std::size_t nu);
66 const pinocchio::FrameIndex frame_id1,
67 const pinocchio::FrameIndex frame_id2);
77 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
78 const Eigen::Ref<const VectorXs>&
x,
79 const Eigen::Ref<const VectorXs>& u);
88 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
89 const Eigen::Ref<const VectorXs>&
x,
90 const Eigen::Ref<const VectorXs>& u);
91 virtual boost::shared_ptr<ResidualDataAbstract> createData(
95 const pinocchio::FrameIndex& get_frame_id1()
const;
97 const pinocchio::FrameIndex& get_frame_id2()
const;
100 void set_frame_id1(
const pinocchio::FrameIndex& fid1);
102 void set_frame_id2(
const pinocchio::FrameIndex& fid2);
107 using Base::u_dependent_;
109 using Base::v_dependent_;
112 pinocchio::FrameIndex frame_id1;
113 pinocchio::FrameIndex frame_id2;
114 typename StateMultibody::PinocchioModel
118 template <
typename _Scalar>
120 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124 typedef ResidualDataAbstractTpl<Scalar>
Base;
127 typedef Eigen::Matrix<Scalar, 2, Eigen::Dynamic>
Matrix2xs;
131 template <
template <
typename Scalar>
class Model>
135 J1(6, model->get_state()->get_nv()),
136 J2(6, model->get_state()->get_nv()),
137 dJ(2, model->get_state()->get_nv()),
140 DataCollectorMultibodyTpl<Scalar>* d =
141 dynamic_cast<DataCollectorMultibodyTpl<Scalar>*
>(shared);
144 "Invalid argument: the shared data should be derived from "
145 "DataCollectorMultibody");
149 pinocchio = d->pinocchio;
Cost penalizing distance between two frames r=||f1.translation-f2.translation||.
Definition: residual-feet-collision.hpp:29
ResidualModelAbstractTpl< Scalar > Base
Definition: residual-feet-collision.hpp:35
StateMultibodyTpl< Scalar > StateMultibody
Definition: residual-feet-collision.hpp:37
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-feet-collision.hpp:39
ResidualDataFeetCollisionTpl< Scalar > Data
Definition: residual-feet-collision.hpp:36
MathBase::VectorXs VectorXs
Definition: residual-feet-collision.hpp:40
ResidualDataAbstractTpl< Scalar > ResidualDataAbstract
Definition: residual-feet-collision.hpp:38
MathBaseTpl< Scalar > MathBase
Definition: residual-feet-collision.hpp:34
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: residual-feet-collision.hpp:33
Definition: activation-quad-ref.hpp:19
Definition: residual-feet-collision.hpp:119
Matrix6xs J1
Definition: residual-feet-collision.hpp:158
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: residual-feet-collision.hpp:122
ResidualDataFeetCollisionTpl(Model< Scalar > *const model, DataCollectorAbstract *const data)
Definition: residual-feet-collision.hpp:132
MathBase::Vector3s Vector3s
Definition: residual-feet-collision.hpp:126
Vector3s p1p2
Definition: residual-feet-collision.hpp:160
MathBase::Matrix6xs Matrix6xs
Definition: residual-feet-collision.hpp:128
MathBase::VectorXs VectorXs
Definition: residual-feet-collision.hpp:129
Matrix2xs dJ
Definition: residual-feet-collision.hpp:159
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-feet-collision.hpp:125
Eigen::Matrix< Scalar, 2, Eigen::Dynamic > Matrix2xs
Definition: residual-feet-collision.hpp:127
MathBaseTpl< Scalar > MathBase
Definition: residual-feet-collision.hpp:123
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: residual-feet-collision.hpp:157
ResidualDataAbstractTpl< Scalar > Base
Definition: residual-feet-collision.hpp:124