crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frame-velocity.hpp
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 #ifndef CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_HPP_
11 
12 #include "crocoddyl/multibody/fwd.hpp"
13 #include "crocoddyl/multibody/cost-base.hpp"
14 #include "crocoddyl/multibody/data/multibody.hpp"
15 #include "crocoddyl/multibody/frames.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
17 #include "crocoddyl/core/utils/deprecate.hpp"
18 
19 namespace crocoddyl {
20 
21 template <typename _Scalar>
23  public:
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
36  typedef typename MathBase::VectorXs VectorXs;
37  typedef typename MathBase::Matrix3s Matrix3s;
38 
39  CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
40  boost::shared_ptr<ActivationModelAbstract> activation, const FrameMotion& Fref,
41  const std::size_t& nu);
42  CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
43  boost::shared_ptr<ActivationModelAbstract> activation, const FrameMotion& Fref);
44  CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state, const FrameMotion& vref, const std::size_t& nu);
45  CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state, const FrameMotion& vref);
46  virtual ~CostModelFrameVelocityTpl();
47 
48  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
49  const Eigen::Ref<const VectorXs>& u);
50  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
51  const Eigen::Ref<const VectorXs>& u);
52  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
53 
54  DEPRECATED("Use set_reference<FrameMotionTpl<Scalar> >()", void set_vref(const FrameMotion& vref_in));
55  DEPRECATED("Use get_reference<FrameMotionTpl<Scalar> >()", const FrameMotion& get_vref() const);
56 
57  protected:
58  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
59  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
60 
61  using Base::activation_;
62  using Base::nu_;
63  using Base::state_;
64  using Base::unone_;
65 
66  private:
67  FrameMotion vref_;
68 };
69 
70 template <typename _Scalar>
72  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 
74  typedef _Scalar Scalar;
78  typedef typename MathBase::VectorXs VectorXs;
79  typedef typename MathBase::MatrixXs MatrixXs;
80  typedef typename MathBase::Matrix6xs Matrix6xs;
81 
82  template <template <typename Scalar> class Model>
83  CostDataFrameVelocityTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
84  : Base(model, data),
85  joint(model->get_state()
86  ->get_pinocchio()
87  ->frames[model->template get_reference<FrameMotionTpl<Scalar> >().frame]
88  .parent),
89  vr(pinocchio::MotionTpl<Scalar>::Zero()),
90  fXj(model->get_state()
91  ->get_pinocchio()
92  ->frames[model->template get_reference<FrameMotionTpl<Scalar> >().frame]
93  .placement.inverse()
94  .toActionMatrix()),
95  dv_dq(6, model->get_state()->get_nv()),
96  dv_dv(6, model->get_state()->get_nv()),
97  Arr_Rx(6, model->get_state()->get_nv()) {
98  dv_dq.setZero();
99  dv_dv.setZero();
100  Arr_Rx.setZero();
101  // Check that proper shared data has been passed
103  if (d == NULL) {
104  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
105  }
106 
107  // Avoids data casting at runtime
108  pinocchio = d->pinocchio;
109  }
110 
111  pinocchio::DataTpl<Scalar>* pinocchio;
112  pinocchio::JointIndex joint;
113  pinocchio::MotionTpl<Scalar> vr;
114  typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
115  Matrix6xs dv_dq;
116  Matrix6xs dv_dv;
117  Matrix6xs Arr_Rx;
118 
119  using Base::activation;
120  using Base::cost;
121  using Base::Lu;
122  using Base::Luu;
123  using Base::Lx;
124  using Base::Lxu;
125  using Base::Lxx;
126  using Base::r;
127  using Base::Ru;
128  using Base::Rx;
129  using Base::shared;
130 };
131 
132 } // namespace crocoddyl
133 
134 /* --- Details -------------------------------------------------------------- */
135 /* --- Details -------------------------------------------------------------- */
136 /* --- Details -------------------------------------------------------------- */
137 #include "crocoddyl/multibody/costs/frame-velocity.hxx"
138 
139 #endif // CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_HPP_