crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frame-placement.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_PLACEMENT_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_FRAME_PLACEMENT_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::MatrixXs MatrixXs;
38 
39  CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state,
40  boost::shared_ptr<ActivationModelAbstract> activation, const FramePlacement& Fref,
41  const std::size_t& nu);
42  CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state,
43  boost::shared_ptr<ActivationModelAbstract> activation, const FramePlacement& Fref);
44  CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state, const FramePlacement& Fref,
45  const std::size_t& nu);
46  CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state, const FramePlacement& Fref);
47  virtual ~CostModelFramePlacementTpl();
48 
49  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
50  const Eigen::Ref<const VectorXs>& u);
51  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
52  const Eigen::Ref<const VectorXs>& u);
53  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
54 
55  DEPRECATED("Use set_reference<FramePlacementTpl<Scalar> >()", void set_Mref(const FramePlacement& Mref_in));
56  DEPRECATED("Use get_reference<FramePlacementTpl<Scalar> >()", const FramePlacement& get_Mref() const);
57 
58  protected:
59  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
60  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
61 
62  using Base::activation_;
63  using Base::nu_;
64  using Base::state_;
65  using Base::unone_;
66 
67  private:
68  FramePlacement Mref_;
69  pinocchio::SE3Tpl<Scalar> oMf_inv_;
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::Matrix6xs Matrix6xs;
83  typedef typename MathBase::Matrix6s Matrix6s;
84  typedef typename MathBase::Vector6s Vector6s;
85 
86  template <template <typename Scalar> class Model>
87  CostDataFramePlacementTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
88  : Base(model, data),
89  J(6, model->get_state()->get_nv()),
90  rJf(6, 6),
91  fJf(6, model->get_state()->get_nv()),
92  Arr_J(6, model->get_state()->get_nv()) {
93  r.setZero();
94  J.setZero();
95  rJf.setZero();
96  fJf.setZero();
97  Arr_J.setZero();
98  // Check that proper shared data has been passed
100  if (d == NULL) {
101  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
102  }
103 
104  // Avoids data casting at runtime
105  pinocchio = d->pinocchio;
106  }
107 
108  pinocchio::DataTpl<Scalar>* pinocchio;
109  Vector6s r;
110  pinocchio::SE3Tpl<Scalar> rMf;
111  Matrix6xs J;
112  Matrix6s rJf;
113  Matrix6xs fJf;
114  Matrix6xs Arr_J;
115 
116  using Base::activation;
117  using Base::cost;
118  using Base::Lu;
119  using Base::Luu;
120  using Base::Lx;
121  using Base::Lxu;
122  using Base::Lxx;
123  using Base::shared;
124  // using Base::r;
125  using Base::Ru;
126  using Base::Rx;
127 };
128 
129 } // namespace crocoddyl
130 
131 /* --- Details -------------------------------------------------------------- */
132 /* --- Details -------------------------------------------------------------- */
133 /* --- Details -------------------------------------------------------------- */
134 #include "crocoddyl/multibody/costs/frame-placement.hxx"
135 
136 #endif // CROCODDYL_MULTIBODY_COSTS_FRAME_PLACEMENT_HPP_