9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/frame-rotation.hpp" 12 #include <pinocchio/algorithm/frames.hpp> 16 template <
typename Scalar>
17 CostModelFrameRotationTpl<Scalar>::CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
18 boost::shared_ptr<ActivationModelAbstract> activation,
19 const FrameRotation& Rref,
const std::size_t& nu)
20 : Base(state, activation, nu), Rref_(Rref), oRf_inv_(Rref.oRf.transpose()) {
21 if (activation_->get_nr() != 3) {
22 throw_pretty(
"Invalid argument: " 23 <<
"nr is equals to 3");
27 template <
typename Scalar>
28 CostModelFrameRotationTpl<Scalar>::CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
29 boost::shared_ptr<ActivationModelAbstract> activation,
30 const FrameRotation& Rref)
31 : Base(state, activation), Rref_(Rref), oRf_inv_(Rref.oRf.transpose()) {
32 if (activation_->get_nr() != 3) {
33 throw_pretty(
"Invalid argument: " 34 <<
"nr is equals to 3");
38 template <
typename Scalar>
39 CostModelFrameRotationTpl<Scalar>::CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
40 const FrameRotation& Rref,
const std::size_t& nu)
41 : Base(state, 3, nu), Rref_(Rref), oRf_inv_(Rref.oRf.transpose()) {}
43 template <
typename Scalar>
44 CostModelFrameRotationTpl<Scalar>::CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
45 const FrameRotation& Rref)
46 : Base(state, 3), Rref_(Rref), oRf_inv_(Rref.oRf.transpose()) {}
48 template <
typename Scalar>
49 CostModelFrameRotationTpl<Scalar>::~CostModelFrameRotationTpl() {}
51 template <
typename Scalar>
52 void CostModelFrameRotationTpl<Scalar>::calc(
const boost::shared_ptr<CostDataAbstract>& data,
53 const Eigen::Ref<const VectorXs>&,
const Eigen::Ref<const VectorXs>&) {
54 CostDataFrameRotationTpl<Scalar>* d =
static_cast<CostDataFrameRotationTpl<Scalar>*
>(data.get());
57 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio, Rref_.frame);
58 d->rRf.noalias() = oRf_inv_ * d->pinocchio->oMf[Rref_.frame].rotation();
59 d->r = pinocchio::log3(d->rRf);
63 activation_->calc(d->activation, d->r);
64 d->cost = d->activation->a_value;
67 template <
typename Scalar>
68 void CostModelFrameRotationTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataAbstract>& data,
69 const Eigen::Ref<const VectorXs>&,
70 const Eigen::Ref<const VectorXs>&) {
72 CostDataFrameRotationTpl<Scalar>* d =
static_cast<CostDataFrameRotationTpl<Scalar>*
>(data.get());
75 pinocchio::Jlog3(d->rRf, d->rJf);
76 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, Rref_.frame, pinocchio::LOCAL, d->fJf);
77 d->J.noalias() = d->rJf * d->fJf.template bottomRows<3>();
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> > CostModelFrameRotationTpl<Scalar>::createData(
90 DataCollectorAbstract*
const data) {
91 return boost::make_shared<CostDataFrameRotationTpl<Scalar> >(
this, data);
94 template <
typename Scalar>
95 void CostModelFrameRotationTpl<Scalar>::set_referenceImpl(
const std::type_info& ti,
const void* pv) {
96 if (ti ==
typeid(FrameRotation)) {
97 Rref_ = *
static_cast<const FrameRotation*
>(pv);
99 throw_pretty(
"Invalid argument: incorrect type (it should be FrameRotation)");
103 template <
typename Scalar>
104 void CostModelFrameRotationTpl<Scalar>::get_referenceImpl(
const std::type_info& ti,
void* pv) {
105 if (ti ==
typeid(FrameRotation)) {
106 FrameRotation& ref_map = *
static_cast<FrameRotation*
>(pv);
109 throw_pretty(
"Invalid argument: incorrect type (it should be FrameRotation)");
113 template <
typename Scalar>
114 const FrameRotationTpl<Scalar>& CostModelFrameRotationTpl<Scalar>::get_Rref()
const {
118 template <
typename Scalar>
119 void CostModelFrameRotationTpl<Scalar>::set_Rref(
const FrameRotation& Rref_in) {
121 oRf_inv_ = Rref_.oRf.transpose();
Definition: action-base.hxx:11