crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frame-translation.hpp
1 
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_MULTIBODY_COSTS_FRAME_TRANSLATION_HPP_
11 #define CROCODDYL_MULTIBODY_COSTS_FRAME_TRANSLATION_HPP_
12 
13 #include "crocoddyl/multibody/fwd.hpp"
14 #include "crocoddyl/multibody/cost-base.hpp"
15 #include "crocoddyl/multibody/data/multibody.hpp"
16 #include "crocoddyl/multibody/frames.hpp"
17 #include "crocoddyl/core/utils/exception.hpp"
18 #include "crocoddyl/core/utils/deprecate.hpp"
19 
20 namespace crocoddyl {
21 
22 template <typename _Scalar>
24  public:
25  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 
27  typedef _Scalar Scalar;
37  typedef typename MathBase::VectorXs VectorXs;
38  typedef typename MathBase::MatrixXs MatrixXs;
39 
40  CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state,
41  boost::shared_ptr<ActivationModelAbstract> activation, const FrameTranslation& xref,
42  const std::size_t& nu);
43  CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state,
44  boost::shared_ptr<ActivationModelAbstract> activation, const FrameTranslation& xref);
45  CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref,
46  const std::size_t& nu);
47  CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref);
49 
50  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
51  const Eigen::Ref<const VectorXs>& u);
52  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
53  const Eigen::Ref<const VectorXs>& u);
54  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
55 
56  DEPRECATED("Use set_reference<FrameTranslation<Scalar> >()", void set_xref(const FrameTranslation& xref_in));
57  DEPRECATED("Use get_reference<FrameTranslation<Scalar> >()", const FrameTranslation& get_xref() const);
58 
59  protected:
60  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
61  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
62 
63  using Base::activation_;
64  using Base::nu_;
65  using Base::state_;
66  using Base::unone_;
67 
68  private:
69  FrameTranslation xref_;
70 };
71 
72 template <typename _Scalar>
74  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75 
76  typedef _Scalar Scalar;
80  typedef typename MathBase::VectorXs VectorXs;
81  typedef typename MathBase::MatrixXs MatrixXs;
82  typedef typename MathBase::Matrix3xs Matrix3xs;
83  typedef typename MathBase::Matrix6xs Matrix6xs;
84 
85  template <template <typename Scalar> class Model>
86  CostDataFrameTranslationTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
87  : Base(model, data), J(3, model->get_state()->get_nv()), fJf(6, model->get_state()->get_nv()) {
88  J.setZero();
89  fJf.setZero();
90  // Check that proper shared data has been passed
92  if (d == NULL) {
93  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
94  }
95 
96  // Avoids data casting at runtime
97  pinocchio = d->pinocchio;
98  }
99 
100  pinocchio::DataTpl<Scalar>* pinocchio;
101  Matrix3xs J;
102  Matrix6xs fJf;
103 
104  using Base::activation;
105  using Base::cost;
106  using Base::Lu;
107  using Base::Luu;
108  using Base::Lx;
109  using Base::Lxu;
110  using Base::Lxx;
111  using Base::r;
112  using Base::Ru;
113  using Base::Rx;
114  using Base::shared;
115 };
116 
117 } // namespace crocoddyl
118 
119 /* --- Details -------------------------------------------------------------- */
120 /* --- Details -------------------------------------------------------------- */
121 /* --- Details -------------------------------------------------------------- */
122 #include "crocoddyl/multibody/costs/frame-translation.hxx"
123 
124 #endif // CROCODDYL_MULTIBODY_COSTS_FRAME_TRANSLATION_HPP_