9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/frame-translation.hpp" 12 #include <pinocchio/algorithm/frames.hpp> 16 template <
typename Scalar>
17 CostModelFrameTranslationTpl<Scalar>::CostModelFrameTranslationTpl(
18 boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
19 const FrameTranslation& xref,
const std::size_t& nu)
20 : Base(state, activation, nu), xref_(xref) {
21 if (activation_->get_nr() != 3) {
22 throw_pretty(
"Invalid argument: " 23 <<
"nr is equals to 3");
27 template <
typename Scalar>
28 CostModelFrameTranslationTpl<Scalar>::CostModelFrameTranslationTpl(
29 boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
30 const FrameTranslation& xref)
31 : Base(state, activation), xref_(xref) {
32 if (activation_->get_nr() != 3) {
33 throw_pretty(
"Invalid argument: " 34 <<
"nr is equals to 3");
38 template <
typename Scalar>
39 CostModelFrameTranslationTpl<Scalar>::CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state,
40 const FrameTranslation& xref,
const std::size_t& nu)
41 : Base(state, 3, nu), xref_(xref) {}
43 template <
typename Scalar>
44 CostModelFrameTranslationTpl<Scalar>::CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state,
45 const FrameTranslation& xref)
46 : Base(state, 3), xref_(xref) {}
48 template <
typename Scalar>
49 CostModelFrameTranslationTpl<Scalar>::~CostModelFrameTranslationTpl() {}
51 template <
typename Scalar>
52 void CostModelFrameTranslationTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
53 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
55 CostDataFrameTranslationTpl<Scalar>* d =
static_cast<CostDataFrameTranslationTpl<Scalar>*
>(data.get());
56 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame);
57 data->r = d->pinocchio->oMf[xref_.frame].translation() - xref_.oxf;
60 activation_->calc(data->activation, data->r);
61 data->cost = data->activation->a_value;
64 template <
typename Scalar>
65 void CostModelFrameTranslationTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
66 const Eigen::Ref<const VectorXs>&,
67 const Eigen::Ref<const VectorXs>&) {
69 CostDataFrameTranslationTpl<Scalar>* d =
static_cast<CostDataFrameTranslationTpl<Scalar>*
>(data.get());
72 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, xref_.frame, pinocchio::LOCAL, d->fJf);
73 d->J = d->pinocchio->oMf[xref_.frame].rotation() * d->fJf.template topRows<3>();
76 const std::size_t& nv = state_->get_nv();
77 activation_->calcDiff(d->activation, d->r);
78 d->Rx.leftCols(nv) = d->J;
79 d->Lx.head(nv) = d->J.transpose() * d->activation->Ar;
80 d->Lxx.topLeftCorner(nv, nv) = d->J.transpose() * d->activation->Arr * d->J;
83 template <
typename Scalar>
84 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelFrameTranslationTpl<Scalar>::createData(
85 DataCollectorAbstract*
const data) {
86 return boost::make_shared<CostDataFrameTranslationTpl<Scalar> >(
this, data);
89 template <
typename Scalar>
90 void CostModelFrameTranslationTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
91 if (ti ==
typeid(FrameTranslation)) {
92 xref_ = *
static_cast<const FrameTranslation*
>(pv);
94 throw_pretty(
"Invalid argument: incorrect type (it should be FrameTranslation)");
98 template <
typename Scalar>
99 void CostModelFrameTranslationTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
100 if (ti ==
typeid(FrameTranslation)) {
101 FrameTranslation& ref_map = *
static_cast<FrameTranslation*
>(pv);
104 throw_pretty(
"Invalid argument: incorrect type (it should be FrameTranslation)");
108 template <
typename Scalar>
109 const FrameTranslationTpl<Scalar>& CostModelFrameTranslationTpl<Scalar>::get_xref()
const {
113 template <
typename Scalar>
114 void CostModelFrameTranslationTpl<Scalar>::set_xref(
const FrameTranslation& xref_in) {
Definition: action-base.hxx:11