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