crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frame-translation.hpp
1 
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2021, LAAS-CNRS, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
11 #define CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
12 
13 #include <pinocchio/multibody/fwd.hpp>
14 
15 #include "crocoddyl/multibody/fwd.hpp"
16 #include "crocoddyl/core/residual-base.hpp"
17 #include "crocoddyl/multibody/states/multibody.hpp"
18 #include "crocoddyl/multibody/data/multibody.hpp"
19 #include "crocoddyl/core/utils/exception.hpp"
20 
21 namespace crocoddyl {
22 
36 template <typename _Scalar>
37 class ResidualModelFrameTranslationTpl : public ResidualModelAbstractTpl<_Scalar> {
38  public:
39  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
40 
41  typedef _Scalar Scalar;
42  typedef MathBaseTpl<Scalar> MathBase;
43  typedef ResidualModelAbstractTpl<Scalar> Base;
44  typedef ResidualDataFrameTranslationTpl<Scalar> Data;
45  typedef StateMultibodyTpl<Scalar> StateMultibody;
46  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
47  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
48  typedef typename MathBase::VectorXs VectorXs;
49  typedef typename MathBase::Vector3s Vector3s;
50 
59  ResidualModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex,
60  const Vector3s& xref, const std::size_t nu);
61 
71  ResidualModelFrameTranslationTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
72  const Vector3s& xref);
74 
82  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
83  const Eigen::Ref<const VectorXs>& u);
84 
92  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
93  const Eigen::Ref<const VectorXs>& u);
94 
98  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
99 
103  pinocchio::FrameIndex get_id() const;
104 
108  const Vector3s& get_reference() const;
109 
113  void set_id(const pinocchio::FrameIndex id);
114 
118  void set_reference(const Vector3s& reference);
119 
125  virtual void print(std::ostream& os) const;
126 
127  protected:
128  using Base::nu_;
129  using Base::state_;
130  using Base::u_dependent_;
131  using Base::unone_;
132  using Base::v_dependent_;
133 
134  private:
135  pinocchio::FrameIndex id_;
136  Vector3s xref_;
137  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
138 };
139 
140 template <typename _Scalar>
141 struct ResidualDataFrameTranslationTpl : public ResidualDataAbstractTpl<_Scalar> {
142  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
143 
144  typedef _Scalar Scalar;
145  typedef MathBaseTpl<Scalar> MathBase;
146  typedef ResidualDataAbstractTpl<Scalar> Base;
147  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
148  typedef typename MathBase::Matrix6xs Matrix6xs;
149 
150  template <template <typename Scalar> class Model>
151  ResidualDataFrameTranslationTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
152  : Base(model, data), fJf(6, model->get_state()->get_nv()) {
153  fJf.setZero();
154  // Check that proper shared data has been passed
155  DataCollectorMultibodyTpl<Scalar>* d = dynamic_cast<DataCollectorMultibodyTpl<Scalar>*>(shared);
156  if (d == NULL) {
157  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
158  }
159 
160  // Avoids data casting at runtime
161  pinocchio = d->pinocchio;
162  }
163 
164  pinocchio::DataTpl<Scalar>* pinocchio;
165  Matrix6xs fJf;
166 
167  using Base::r;
168  using Base::Ru;
169  using Base::Rx;
170  using Base::shared;
171 };
172 
173 } // namespace crocoddyl
174 
175 /* --- Details -------------------------------------------------------------- */
176 /* --- Details -------------------------------------------------------------- */
177 /* --- Details -------------------------------------------------------------- */
178 #include "crocoddyl/multibody/residuals/frame-translation.hxx"
179 
180 #endif // CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the frame translation residual.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Matrix6xs fJf
Local Jacobian of the frame.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the frame translation residual data.
bool u_dependent_
Label that indicates if the residual function depends on u.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the frame translation residual.
std::size_t nu_
Control dimension.
void set_id(const pinocchio::FrameIndex id)
Modify the reference frame id.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
MatrixXs Ru
Jacobian of the residual vector with respect the control.
virtual void print(std::ostream &os) const
Print relevant information of the frame-translation residual.
const Vector3s & get_reference() const
Return the reference frame translation.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
ResidualModelFrameTranslationTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex, const Vector3s &xref, const std::size_t nu)
Initialize the frame translation residual model.
void set_reference(const Vector3s &reference)
Modify the reference frame translation reference.
bool v_dependent_
Label that indicates if the residual function depends on v.
VectorXs r
Residual vector.
DataCollectorAbstract * shared
Shared data allocated by the action model.