9 #include "crocoddyl/core/utils/exception.hpp" 11 #include <pinocchio/algorithm/frames.hpp> 12 #include <pinocchio/algorithm/kinematics-derivatives.hpp> 16 template <
typename Scalar>
17 ImpulseModel6DTpl<Scalar>::ImpulseModel6DTpl(boost::shared_ptr<StateMultibody> state,
const std::size_t& frame)
18 : Base(state, 6), frame_(frame) {}
20 template <
typename Scalar>
21 ImpulseModel6DTpl<Scalar>::~ImpulseModel6DTpl() {}
23 template <
typename Scalar>
24 void ImpulseModel6DTpl<Scalar>::calc(
const boost::shared_ptr<ImpulseDataAbstract>& data,
25 const Eigen::Ref<const VectorXs>&) {
26 boost::shared_ptr<ImpulseData6D> d = boost::static_pointer_cast<ImpulseData6D>(data);
28 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, frame_, pinocchio::LOCAL, d->Jc);
31 template <
typename Scalar>
32 void ImpulseModel6DTpl<Scalar>::calcDiff(
const boost::shared_ptr<ImpulseDataAbstract>& data,
33 const Eigen::Ref<const VectorXs>&) {
34 boost::shared_ptr<ImpulseData6D> d = boost::static_pointer_cast<ImpulseData6D>(data);
35 pinocchio::getJointVelocityDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, d->joint, pinocchio::LOCAL,
36 d->v_partial_dq, d->v_partial_dv);
37 d->dv0_dq.noalias() = d->fXj * d->v_partial_dq;
40 template <
typename Scalar>
41 void ImpulseModel6DTpl<Scalar>::updateForce(
const boost::shared_ptr<ImpulseDataAbstract>& data,
42 const VectorXs& force) {
43 if (force.size() != 6) {
44 throw_pretty(
"Invalid argument: " 45 <<
"lambda has wrong dimension (it should be 6)");
47 boost::shared_ptr<ImpulseData6D> d = boost::static_pointer_cast<ImpulseData6D>(data);
48 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force));
51 template <
typename Scalar>
52 boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > ImpulseModel6DTpl<Scalar>::createData(
53 pinocchio::DataTpl<Scalar>*
const data) {
54 return boost::make_shared<ImpulseData6D>(
this, data);
56 template <
typename Scalar>
57 const std::size_t& ImpulseModel6DTpl<Scalar>::get_frame()
const {
Definition: action-base.hxx:11