crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frame-translation.hpp
1 
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2019-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/core/cost-base.hpp"
15 #include "crocoddyl/multibody/states/multibody.hpp"
16 #include "crocoddyl/multibody/data/multibody.hpp"
17 #include "crocoddyl/multibody/frames.hpp"
18 #include "crocoddyl/core/utils/exception.hpp"
19 #include "crocoddyl/core/utils/deprecate.hpp"
20 
21 namespace crocoddyl {
22 
39 template <typename _Scalar>
41  public:
42  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 
44  typedef _Scalar Scalar;
53  typedef typename MathBase::VectorXs VectorXs;
54 
63  CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state,
64  boost::shared_ptr<ActivationModelAbstract> activation, const FrameTranslation& xref,
65  const std::size_t& nu);
66 
76  CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state,
77  boost::shared_ptr<ActivationModelAbstract> activation, const FrameTranslation& xref);
78 
88  CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref,
89  const std::size_t& nu);
90 
100  CostModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state, const FrameTranslation& xref);
101  virtual ~CostModelFrameTranslationTpl();
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<FrameTranslation<Scalar> >()", void set_xref(const FrameTranslation& xref_in));
129  DEPRECATED("Use get_reference<FrameTranslation<Scalar> >()", const FrameTranslation& get_xref() 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  FrameTranslation xref_;
149  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
150 };
151 
152 template <typename _Scalar>
154  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
155 
156  typedef _Scalar Scalar;
160  typedef typename MathBase::Matrix3xs Matrix3xs;
161  typedef typename MathBase::Matrix6xs Matrix6xs;
162 
163  template <template <typename Scalar> class Model>
164  CostDataFrameTranslationTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
165  : Base(model, data), J(3, model->get_state()->get_nv()), fJf(6, model->get_state()->get_nv()) {
166  J.setZero();
167  fJf.setZero();
168  // Check that proper shared data has been passed
170  if (d == NULL) {
171  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
172  }
173 
174  // Avoids data casting at runtime
175  pinocchio = d->pinocchio;
176  }
177 
178  pinocchio::DataTpl<Scalar>* pinocchio;
179  Matrix3xs J;
180  Matrix6xs fJf;
181 
182  using Base::activation;
183  using Base::cost;
184  using Base::Lu;
185  using Base::Luu;
186  using Base::Lx;
187  using Base::Lxu;
188  using Base::Lxx;
189  using Base::r;
190  using Base::Ru;
191  using Base::Rx;
192  using Base::shared;
193 };
194 
195 } // namespace crocoddyl
196 
197 /* --- Details -------------------------------------------------------------- */
198 /* --- Details -------------------------------------------------------------- */
199 /* --- Details -------------------------------------------------------------- */
200 #include "crocoddyl/multibody/costs/frame-translation.hxx"
201 
202 #endif // CROCODDYL_MULTIBODY_COSTS_FRAME_TRANSLATION_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the frame translation cost.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Return the frame translation reference.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the frame translation cost data.
CostModelFrameTranslationTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameTranslation &xref, const std::size_t &nu)
Initialize the frame translation cost model.
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 translation cost.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Modify the frame translation reference.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193