9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/impulses/impulse-3d.hpp" 12 #include <pinocchio/algorithm/frames.hpp> 13 #include <pinocchio/algorithm/kinematics-derivatives.hpp> 17 template <
typename Scalar>
18 ImpulseModel3DTpl<Scalar>::ImpulseModel3DTpl(boost::shared_ptr<StateMultibody> state,
const std::size_t& frame)
19 : Base(state, 3), frame_(frame) {}
21 template <
typename Scalar>
22 ImpulseModel3DTpl<Scalar>::~ImpulseModel3DTpl() {}
24 template <
typename Scalar>
25 void ImpulseModel3DTpl<Scalar>::calc(
const boost::shared_ptr<ImpulseDataAbstract>& data,
26 const Eigen::Ref<const VectorXs>&) {
27 boost::shared_ptr<ImpulseData3D> d = boost::static_pointer_cast<ImpulseData3D>(data);
29 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, frame_, pinocchio::LOCAL, d->fJf);
30 d->Jc = d->fJf.template topRows<3>();
33 template <
typename Scalar>
34 void ImpulseModel3DTpl<Scalar>::calcDiff(
const boost::shared_ptr<ImpulseDataAbstract>& data,
35 const Eigen::Ref<const VectorXs>&) {
36 boost::shared_ptr<ImpulseData3D> d = boost::static_pointer_cast<ImpulseData3D>(data);
37 pinocchio::getJointVelocityDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, d->joint, pinocchio::LOCAL,
38 d->v_partial_dq, d->v_partial_dv);
39 d->dv0_dq.noalias() = d->fXj.template topRows<3>() * d->v_partial_dq;
42 template <
typename Scalar>
43 void ImpulseModel3DTpl<Scalar>::updateForce(
const boost::shared_ptr<ImpulseDataAbstract>& data,
44 const VectorXs& force) {
45 if (force.size() != 3) {
46 throw_pretty(
"Invalid argument: " 47 <<
"lambda has wrong dimension (it should be 3)");
49 boost::shared_ptr<ImpulseData3D> d = boost::static_pointer_cast<ImpulseData3D>(data);
50 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force, Vector3s::Zero()));
53 template <
typename Scalar>
54 boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > ImpulseModel3DTpl<Scalar>::createData(
55 pinocchio::DataTpl<Scalar>*
const data) {
56 return boost::make_shared<ImpulseData3D>(
this, data);
59 template <
typename Scalar>
60 const std::size_t& ImpulseModel3DTpl<Scalar>::get_frame()
const {
Definition: action-base.hxx:11