9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/frame-placement.hpp" 12 #include <pinocchio/algorithm/frames.hpp> 16 template <
typename Scalar>
17 CostModelFramePlacementTpl<Scalar>::CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state,
18 boost::shared_ptr<ActivationModelAbstract> activation,
19 const FramePlacement& Mref,
const std::size_t& nu)
20 : Base(state, activation, nu), Mref_(Mref), oMf_inv_(Mref.oMf.inverse()) {
21 if (activation_->get_nr() != 6) {
22 throw_pretty(
"Invalid argument: " 23 <<
"nr is equals to 6");
27 template <
typename Scalar>
28 CostModelFramePlacementTpl<Scalar>::CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state,
29 boost::shared_ptr<ActivationModelAbstract> activation,
30 const FramePlacement& Mref)
31 : Base(state, activation), Mref_(Mref), oMf_inv_(Mref.oMf.inverse()) {
32 if (activation_->get_nr() != 6) {
33 throw_pretty(
"Invalid argument: " 34 <<
"nr is equals to 6");
38 template <
typename Scalar>
39 CostModelFramePlacementTpl<Scalar>::CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state,
40 const FramePlacement& Mref,
const std::size_t& nu)
41 : Base(state, 6, nu), Mref_(Mref), oMf_inv_(Mref.oMf.inverse()) {}
43 template <
typename Scalar>
44 CostModelFramePlacementTpl<Scalar>::CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state,
45 const FramePlacement& Mref)
46 : Base(state, 6), Mref_(Mref), oMf_inv_(Mref.oMf.inverse()) {}
48 template <
typename Scalar>
49 CostModelFramePlacementTpl<Scalar>::~CostModelFramePlacementTpl() {}
51 template <
typename Scalar>
52 void CostModelFramePlacementTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
53 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
54 CostDataFramePlacementTpl<Scalar>* d =
static_cast<CostDataFramePlacementTpl<Scalar>*
>(data.get());
57 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio, Mref_.frame);
58 d->rMf = oMf_inv_ * d->pinocchio->oMf[Mref_.frame];
59 d->r = pinocchio::log6(d->rMf);
63 activation_->calc(d->activation, d->r);
64 d->cost = d->activation->a_value;
67 template <
typename Scalar>
68 void CostModelFramePlacementTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
69 const Eigen::Ref<const VectorXs>&,
70 const Eigen::Ref<const VectorXs>&) {
72 CostDataFramePlacementTpl<Scalar>* d =
static_cast<CostDataFramePlacementTpl<Scalar>*
>(data.get());
75 pinocchio::Jlog6(d->rMf, d->rJf);
76 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, Mref_.frame, pinocchio::LOCAL, d->fJf);
77 d->J.noalias() = d->rJf * d->fJf;
80 const std::size_t& nv = state_->get_nv();
81 activation_->calcDiff(data->activation, data->r);
82 data->Rx.leftCols(nv) = d->J;
83 data->Lx.head(nv).noalias() = d->J.transpose() * data->activation->Ar;
84 d->Arr_J.noalias() = data->activation->Arr * d->J;
85 data->Lxx.topLeftCorner(nv, nv).noalias() = d->J.transpose() * d->Arr_J;
88 template <
typename Scalar>
89 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelFramePlacementTpl<Scalar>::createData(
90 DataCollectorAbstract*
const data) {
91 return boost::make_shared<CostDataFramePlacementTpl<Scalar> >(
this, data);
94 template <
typename Scalar>
95 void CostModelFramePlacementTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
96 if (ti ==
typeid(FramePlacement)) {
97 Mref_ = *
static_cast<const FramePlacement*
>(pv);
99 throw_pretty(
"Invalid argument: incorrect type (it should be FramePlacement)");
103 template <
typename Scalar>
104 void CostModelFramePlacementTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
105 if (ti ==
typeid(FramePlacement)) {
106 FramePlacement& ref_map = *
static_cast<FramePlacement*
>(pv);
109 throw_pretty(
"Invalid argument: incorrect type (it should be FramePlacement)");
113 template <
typename Scalar>
114 const FramePlacementTpl<Scalar>& CostModelFramePlacementTpl<Scalar>::get_Mref()
const {
118 template <
typename Scalar>
119 void CostModelFramePlacementTpl<Scalar>::set_Mref(
const FramePlacement& Mref_in) {
121 oMf_inv_ = Mref_.oMf.inverse();
Definition: action-base.hxx:11