impulse-6d.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 
11 #include <pinocchio/algorithm/frames.hpp>
12 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
13 
14 namespace crocoddyl {
15 
16 template <typename Scalar>
17 ImpulseModel6DTpl<Scalar>::ImpulseModel6DTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& frame)
18  : Base(state, 6), frame_(frame) {}
19 
20 template <typename Scalar>
21 ImpulseModel6DTpl<Scalar>::~ImpulseModel6DTpl() {}
22 
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);
27 
28  pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, frame_, pinocchio::LOCAL, d->Jc);
29 }
30 
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;
38 }
39 
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)");
46  }
47  boost::shared_ptr<ImpulseData6D> d = boost::static_pointer_cast<ImpulseData6D>(data);
48  data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force));
49 }
50 
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);
55 }
56 template <typename Scalar>
57 const std::size_t& ImpulseModel6DTpl<Scalar>::get_frame() const {
58  return frame_;
59 }
60 
61 } // namespace crocoddyl
Definition: action-base.hxx:11