crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frame-velocity.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_VELOCITY_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_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 
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  CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
64  boost::shared_ptr<ActivationModelAbstract> activation, const FrameMotion& Fref,
65  const std::size_t& nu);
66 
76  CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
77  boost::shared_ptr<ActivationModelAbstract> activation, const FrameMotion& Fref);
78 
88  CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state, const FrameMotion& Fref, const std::size_t& nu);
89 
99  CostModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state, const FrameMotion& Fref);
100  virtual ~CostModelFrameVelocityTpl();
101 
109  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
110  const Eigen::Ref<const VectorXs>& u);
111 
119  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
120  const Eigen::Ref<const VectorXs>& u);
121 
125  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
126 
127  DEPRECATED("Use set_reference<FrameMotionTpl<Scalar> >()", void set_vref(const FrameMotion& vref_in));
128  DEPRECATED("Use get_reference<FrameMotionTpl<Scalar> >()", const FrameMotion& get_vref() const);
129 
130  protected:
134  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
135 
139  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
140 
141  using Base::activation_;
142  using Base::nu_;
143  using Base::state_;
144  using Base::unone_;
145 
146  private:
147  FrameMotion vref_;
148  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
149 };
150 
151 template <typename _Scalar>
153  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
154 
155  typedef _Scalar Scalar;
159  typedef typename MathBase::Matrix6xs Matrix6xs;
160 
161  template <template <typename Scalar> class Model>
162  CostDataFrameVelocityTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
163  : Base(model, data), Arr_Rx(6, model->get_state()->get_nv()) {
164  Arr_Rx.setZero();
165  // Check that proper shared data has been passed
167  if (d == NULL) {
168  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
169  }
170 
171  // Avoids data casting at runtime
172  pinocchio = d->pinocchio;
173  }
174 
175  pinocchio::DataTpl<Scalar>* pinocchio;
176  Matrix6xs Arr_Rx;
177 
178  using Base::activation;
179  using Base::cost;
180  using Base::Lu;
181  using Base::Luu;
182  using Base::Lx;
183  using Base::Lxu;
184  using Base::Lxx;
185  using Base::r;
186  using Base::Ru;
187  using Base::Rx;
188  using Base::shared;
189 };
190 
191 } // namespace crocoddyl
192 
193 /* --- Details -------------------------------------------------------------- */
194 /* --- Details -------------------------------------------------------------- */
195 /* --- Details -------------------------------------------------------------- */
196 #include "crocoddyl/multibody/costs/frame-velocity.hxx"
197 
198 #endif // CROCODDYL_MULTIBODY_COSTS_FRAME_VELOCITY_HPP_
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the frame velocity cost data.
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 velocity cost.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Return the frame velocity reference.
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 velocity cost.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
CostModelFrameVelocityTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameMotion &Fref, const std::size_t &nu)
Initialize the frame velocity cost model.
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 velocity reference.
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193