crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frame-rotation.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_ROTATION_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_FRAME_ROTATION_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  typedef typename MathBase::Matrix3s Matrix3s;
54 
63  CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
64  boost::shared_ptr<ActivationModelAbstract> activation, const FrameRotation& Fref,
65  const std::size_t& nu);
66 
77  CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state,
78  boost::shared_ptr<ActivationModelAbstract> activation, const FrameRotation& Fref);
79 
90  CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state, const FrameRotation& Fref, const std::size_t& nu);
91 
103  CostModelFrameRotationTpl(boost::shared_ptr<StateMultibody> state, const FrameRotation& Fref);
104  virtual ~CostModelFrameRotationTpl();
105 
113  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
114  const Eigen::Ref<const VectorXs>& u);
115 
123  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
124  const Eigen::Ref<const VectorXs>& u);
125 
129  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);
130 
131  DEPRECATED("Use set_reference<FrameRotationTpl<Scalar> >()", void set_Rref(const FrameRotation& Rref_in));
132  DEPRECATED("Use get_reference<FrameRotationTpl<Scalar> >()", const FrameRotation& get_Rref() const);
133 
134  protected:
138  virtual void set_referenceImpl(const std::type_info& ti, const void* pv);
139 
143  virtual void get_referenceImpl(const std::type_info& ti, void* pv) const;
144 
145  using Base::activation_;
146  using Base::nu_;
147  using Base::state_;
148  using Base::unone_;
149 
150  private:
151  FrameRotation Rref_;
152  Matrix3s oRf_inv_;
153  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
154 };
155 
156 template <typename _Scalar>
158  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
159 
160  typedef _Scalar Scalar;
164  typedef typename MathBase::Vector3s Vector3s;
165  typedef typename MathBase::Matrix3s Matrix3s;
166  typedef typename MathBase::Matrix3xs Matrix3xs;
167  typedef typename MathBase::Matrix6xs Matrix6xs;
168 
169  template <template <typename Scalar> class Model>
170  CostDataFrameRotationTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
171  : Base(model, data),
172  J(3, model->get_state()->get_nv()),
173  rJf(3, 3),
174  fJf(6, model->get_state()->get_nv()),
175  Arr_J(3, model->get_state()->get_nv()) {
176  r.setZero();
177  rRf.setIdentity();
178  J.setZero();
179  rJf.setZero();
180  fJf.setZero();
181  Arr_J.setZero();
182  // Check that proper shared data has been passed
184  if (d == NULL) {
185  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
186  }
187 
188  // Avoids data casting at runtime
189  pinocchio = d->pinocchio;
190  }
191 
192  pinocchio::DataTpl<Scalar>* pinocchio;
193  Vector3s r;
194  Matrix3s rRf;
195  Matrix3xs J;
196  Matrix3s rJf;
197  Matrix6xs fJf;
198  Matrix3xs Arr_J;
199 
200  using Base::activation;
201  using Base::cost;
202  using Base::Lu;
203  using Base::Luu;
204  using Base::Lx;
205  using Base::Lxu;
206  using Base::Lxx;
207  using Base::shared;
208  // using Base::r;
209  using Base::Ru;
210  using Base::Rx;
211 };
212 
213 } // namespace crocoddyl
214 
215 /* --- Details -------------------------------------------------------------- */
216 /* --- Details -------------------------------------------------------------- */
217 /* --- Details -------------------------------------------------------------- */
218 #include "crocoddyl/multibody/costs/frame-rotation.hxx"
219 
220 #endif // CROCODDYL_MULTIBODY_COSTS_FRAME_ROTATION_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
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 rotation cost.
boost::shared_ptr< ActivationModelAbstract > activation_
Activation model.
Definition: cost-base.hpp:194
virtual boost::shared_ptr< CostDataAbstract > createData(DataCollectorAbstract *const data)
Create the frame rotation cost data.
virtual void get_referenceImpl(const std::type_info &ti, void *pv) const
Return the frame rotation reference.
virtual void set_referenceImpl(const std::type_info &ti, const void *pv)
Modify the frame rotation reference.
VectorXs unone_
No control vector.
Definition: cost-base.hpp:196
std::size_t nu_
Control dimension.
Definition: cost-base.hpp:195
boost::shared_ptr< StateAbstract > state_
State description.
Definition: cost-base.hpp:193
virtual void calc(const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the frame rotation cost.
CostModelFrameRotationTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameRotation &Fref, const std::size_t &nu)
Initialize the frame rotation cost model.