impulse-3d.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
11 
12 #include <pinocchio/algorithm/frames.hpp>
13 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
14 
15 namespace crocoddyl {
16 
17 template <typename Scalar>
18 ImpulseModel3DTpl<Scalar>::ImpulseModel3DTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& frame)
19  : Base(state, 3), frame_(frame) {}
20 
21 template <typename Scalar>
22 ImpulseModel3DTpl<Scalar>::~ImpulseModel3DTpl() {}
23 
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);
28 
29  pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, frame_, pinocchio::LOCAL, d->fJf);
30  d->Jc = d->fJf.template topRows<3>();
31 }
32 
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;
40 }
41 
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)");
48  }
49  boost::shared_ptr<ImpulseData3D> d = boost::static_pointer_cast<ImpulseData3D>(data);
50  data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force, Vector3s::Zero()));
51 }
52 
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);
57 }
58 
59 template <typename Scalar>
60 const std::size_t& ImpulseModel3DTpl<Scalar>::get_frame() const {
61  return frame_;
62 }
63 
64 } // namespace crocoddyl
Definition: action-base.hxx:11