crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
frame-velocity.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_FRAME_VELOCITY_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_FRAME_VELOCITY_HPP_
11 
12 #include <pinocchio/multibody/fwd.hpp>
13 #include <pinocchio/spatial/motion.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 
37 template <typename _Scalar>
38 class ResidualModelFrameVelocityTpl : public ResidualModelAbstractTpl<_Scalar> {
39  public:
40  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 
42  typedef _Scalar Scalar;
43  typedef MathBaseTpl<Scalar> MathBase;
44  typedef ResidualModelAbstractTpl<Scalar> Base;
45  typedef ResidualDataFrameVelocityTpl<Scalar> Data;
46  typedef StateMultibodyTpl<Scalar> StateMultibody;
47  typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
48  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
49  typedef pinocchio::MotionTpl<Scalar> Motion;
50  typedef typename MathBase::VectorXs VectorXs;
51 
61  ResidualModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
62  const Motion& velocity, const pinocchio::ReferenceFrame type, const std::size_t nu);
63 
74  ResidualModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
75  const Motion& velocity, const pinocchio::ReferenceFrame type);
77 
85  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
86  const Eigen::Ref<const VectorXs>& u);
87 
95  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
96  const Eigen::Ref<const VectorXs>& u);
97 
101  virtual boost::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract* const data);
102 
106  pinocchio::FrameIndex get_id() const;
107 
111  const Motion& get_reference() const;
112 
116  pinocchio::ReferenceFrame get_type() const;
117 
121  void set_id(const pinocchio::FrameIndex id);
122 
126  void set_reference(const Motion& velocity);
127 
131  void set_type(const pinocchio::ReferenceFrame type);
132 
138  virtual void print(std::ostream& os) const;
139 
140  protected:
141  using Base::nr_;
142  using Base::nu_;
143  using Base::state_;
144  using Base::u_dependent_;
145  using Base::unone_;
146 
147  private:
148  pinocchio::FrameIndex id_;
149  Motion vref_;
150  pinocchio::ReferenceFrame type_;
151  boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
152 };
153 
154 template <typename _Scalar>
155 struct ResidualDataFrameVelocityTpl : public ResidualDataAbstractTpl<_Scalar> {
156  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
157 
158  typedef _Scalar Scalar;
159  typedef MathBaseTpl<Scalar> MathBase;
160  typedef ResidualDataAbstractTpl<Scalar> Base;
161  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
162  typedef typename MathBase::Matrix6xs Matrix6xs;
163 
164  template <template <typename Scalar> class Model>
165  ResidualDataFrameVelocityTpl(Model<Scalar>* const model, DataCollectorAbstract* const data) : Base(model, data) {
166  // Check that proper shared data has been passed
167  DataCollectorMultibodyTpl<Scalar>* d = dynamic_cast<DataCollectorMultibodyTpl<Scalar>*>(shared);
168  if (d == NULL) {
169  throw_pretty("Invalid argument: the shared data should be derived from DataCollectorMultibody");
170  }
171 
172  // Avoids data casting at runtime
173  pinocchio = d->pinocchio;
174  }
175 
176  pinocchio::DataTpl<Scalar>* pinocchio;
177  using Base::r;
178  using Base::Ru;
179  using Base::Rx;
180  using Base::shared;
181 };
182 
183 } // namespace crocoddyl
184 
185 /* --- Details -------------------------------------------------------------- */
186 /* --- Details -------------------------------------------------------------- */
187 /* --- Details -------------------------------------------------------------- */
188 #include "crocoddyl/multibody/residuals/frame-velocity.hxx"
189 
190 #endif // CROCODDYL_MULTIBODY_RESIDUALS_FRAME_VELOCITY_HPP_
pinocchio::FrameIndex get_id() const
Modify the reference frame id.
void set_id(const pinocchio::FrameIndex id)
Return reference frame id.
ResidualModelFrameVelocityTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Motion &velocity, const pinocchio::ReferenceFrame type, const std::size_t nu)
Initialize the frame velocity residual model.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
std::size_t nr_
Residual vector dimension.
bool u_dependent_
Label that indicates if the residual function depends on u.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the frame velocity residual data.
std::size_t nu_
Control dimension.
void set_type(const pinocchio::ReferenceFrame type)
Return reference type of velocity.
void set_reference(const Motion &velocity)
Return reference velocity.
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the frame velocity residual vector.
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobians of the frame velocity residual.
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-velocity residual.
boost::shared_ptr< StateAbstract > state_
State description.
VectorXs unone_
No control vector.
const Motion & get_reference() const
Modify the reference velocity.
VectorXs r
Residual vector.
DataCollectorAbstract * shared
Shared data allocated by the action model.
pinocchio::ReferenceFrame get_type() const
Modify the reference type of velocity.