crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frame-placement.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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/core/cost-base.hpp"
14 #include "crocoddyl/multibody/states/multibody.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 
38 template <typename _Scalar>
40  public:
41  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42 
43  typedef _Scalar Scalar;
52  typedef typename MathBase::VectorXs VectorXs;
53 
62  CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state,
63  boost::shared_ptr<ActivationModelAbstract> activation, const FramePlacement& Fref,
64  const std::size_t& nu);
65 
75  CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state,
76  boost::shared_ptr<ActivationModelAbstract> activation, const FramePlacement& Fref);
77 
87  CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state, const FramePlacement& Fref,
88  const std::size_t& nu);
89 
100  CostModelFramePlacementTpl(boost::shared_ptr<StateMultibody> state, const FramePlacement& Fref);
101  virtual ~CostModelFramePlacementTpl();
102 
110  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
111  const Eigen::Ref<const VectorXs>& u);
112 
120  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
121  const Eigen::Ref<const VectorXs>& u);
122 
126  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
127 
128  DEPRECATED("Use set_reference<FramePlacementTpl<Scalar> >()", void set_Mref(const FramePlacement& Mref_in));
129  DEPRECATED("Use get_reference<FramePlacementTpl<Scalar> >()", const FramePlacement& get_Mref() const);
130 
131  protected:
135  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
136 
140  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
141 
142  using Base::activation_;
143  using Base::nu_;
144  using Base::state_;
145  using Base::unone_;
146 
147  private:
148  FramePlacement Mref_;
149  pinocchio::SE3Tpl<Scalar> oMf_inv_;
150  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
151 };
152 
153 template <typename _Scalar>
155  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
156 
157  typedef _Scalar Scalar;
161  typedef typename MathBase::Matrix6xs Matrix6xs;
162  typedef typename MathBase::Matrix6s Matrix6s;
163  typedef typename MathBase::Vector6s Vector6s;
164 
165  template <template <typename Scalar> class Model>
166  CostDataFramePlacementTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
167  : Base(model, data),
168  J(6, model->get_state()->get_nv()),
169  rJf(6, 6),
170  fJf(6, model->get_state()->get_nv()),
171  Arr_J(6, model->get_state()->get_nv()) {
172  r.setZero();
173  J.setZero();
174  rJf.setZero();
175  fJf.setZero();
176  Arr_J.setZero();
177  // Check that proper shared data has been passed
179  if (d == NULL) {
180  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
181  }
182 
183  // Avoids data casting at runtime
184  pinocchio = d->pinocchio;
185  }
186 
187  pinocchio::DataTpl<Scalar>* pinocchio;
188  Vector6s r;
189  pinocchio::SE3Tpl<Scalar> rMf;
190  Matrix6xs J;
191  Matrix6s rJf;
192  Matrix6xs fJf;
193  Matrix6xs Arr_J;
194 
195  using Base::activation;
196  using Base::cost;
197  using Base::Lu;
198  using Base::Luu;
199  using Base::Lx;
200  using Base::Lxu;
201  using Base::Lxx;
202  using Base::shared;
203  // using Base::r;
204  using Base::Ru;
205  using Base::Rx;
206 };
207 
208 } // namespace crocoddyl
209 
210 /* --- Details -------------------------------------------------------------- */
211 /* --- Details -------------------------------------------------------------- */
212 /* --- Details -------------------------------------------------------------- */
213 #include "crocoddyl/multibody/costs/frame-placement.hxx"
214 
215 #endif // CROCODDYL_MULTIBODY_COSTS_FRAME_PLACEMENT_HPP_
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the frame placement cost data.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Return the frame placement reference.
Abstract class for cost models.
Definition: cost-base.hpp:47
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Modify the frame placement reference.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the frame placement cost.
virtual void calcDiff(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the frame placement cost.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
CostModelFramePlacementTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FramePlacement &Fref, const std::size_t &nu)
Initialize the frame placement cost model.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193