9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/frame-velocity.hpp" 12 #include <pinocchio/algorithm/frames.hpp> 13 #include <pinocchio/algorithm/kinematics-derivatives.hpp> 17 template <
typename Scalar>
18 CostModelFrameVelocityTpl<Scalar>::CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
19 boost::shared_ptr<ActivationModelAbstract> activation,
20 const FrameMotion& vref,
const std::size_t& nu)
21 : Base(state, activation, nu), vref_(vref) {
22 if (activation_->get_nr() != 6) {
23 throw_pretty(
"Invalid argument: " 24 <<
"nr is equals to 6");
28 template <
typename Scalar>
29 CostModelFrameVelocityTpl<Scalar>::CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
30 boost::shared_ptr<ActivationModelAbstract> activation,
31 const FrameMotion& vref)
32 : Base(state, activation), vref_(vref) {
33 if (activation_->get_nr() != 6) {
34 throw_pretty(
"Invalid argument: " 35 <<
"nr is equals to 6");
39 template <
typename Scalar>
40 CostModelFrameVelocityTpl<Scalar>::CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
41 const FrameMotion& vref,
const std::size_t& nu)
42 : Base(state, 6, nu), vref_(vref) {}
44 template <
typename Scalar>
45 CostModelFrameVelocityTpl<Scalar>::CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
46 const FrameMotion& vref)
47 : Base(state, 6), vref_(vref) {}
49 template <
typename Scalar>
50 CostModelFrameVelocityTpl<Scalar>::~CostModelFrameVelocityTpl() {}
52 template <
typename Scalar>
53 void CostModelFrameVelocityTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
54 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
55 CostDataFrameVelocityTpl<Scalar>* d =
static_cast<CostDataFrameVelocityTpl<Scalar>*
>(data.get());
58 d->vr = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(), *d->pinocchio, vref_.frame) - vref_.oMf;
59 data->r = d->vr.toVector();
62 activation_->calc(data->activation, data->r);
63 data->cost = data->activation->a_value;
66 template <
typename Scalar>
67 void CostModelFrameVelocityTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
68 const Eigen::Ref<const VectorXs>&,
69 const Eigen::Ref<const VectorXs>&) {
71 CostDataFrameVelocityTpl<Scalar>* d =
static_cast<CostDataFrameVelocityTpl<Scalar>*
>(data.get());
72 pinocchio::getJointVelocityDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, d->joint, pinocchio::LOCAL,
76 const std::size_t& nv = state_->get_nv();
77 activation_->calcDiff(data->activation, data->r);
78 data->Rx.leftCols(nv).noalias() = d->fXj * d->dv_dq;
79 data->Rx.rightCols(nv).noalias() = d->fXj * d->dv_dv;
80 data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
81 d->Arr_Rx.noalias() = data->activation->Arr * data->Rx;
82 data->Lxx.noalias() = data->Rx.transpose() * d->Arr_Rx;
85 template <
typename Scalar>
86 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelFrameVelocityTpl<Scalar>::createData(
87 DataCollectorAbstract*
const data) {
88 return boost::make_shared<CostDataFrameVelocityTpl<Scalar> >(
this, data);
91 template <
typename Scalar>
92 void CostModelFrameVelocityTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
93 if (ti ==
typeid(FrameMotion)) {
94 vref_ = *
static_cast<const FrameMotion*
>(pv);
96 throw_pretty(
"Invalid argument: incorrect type (it should be FrameMotion)");
100 template <
typename Scalar>
101 void CostModelFrameVelocityTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
102 if (ti ==
typeid(FrameMotion)) {
103 FrameMotion& ref_map = *
static_cast<FrameMotion*
>(pv);
106 throw_pretty(
"Invalid argument: incorrect type (it should be FrameMotion)");
110 template <
typename Scalar>
111 const FrameMotionTpl<Scalar>& CostModelFrameVelocityTpl<Scalar>::get_vref()
const {
115 template <
typename Scalar>
116 void CostModelFrameVelocityTpl<Scalar>::set_vref(
const FrameMotion& vref_in) {
Definition: action-base.hxx:11