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/core/residual-base.hpp"
16 #include "crocoddyl/core/utils/exception.hpp"
17 #include "crocoddyl/multibody/data/multibody.hpp"
18 #include "crocoddyl/multibody/fwd.hpp"
19 #include "crocoddyl/multibody/states/multibody.hpp"
20 
21 namespace crocoddyl {
22 
39 template <typename _Scalar>
41  public:
42  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 
44  typedef _Scalar Scalar;
51  typedef pinocchio::MotionTpl<Scalar> Motion;
52  typedef typename MathBase::VectorXs VectorXs;
53 
64  ResidualModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
65  const pinocchio::FrameIndex id,
66  const Motion& velocity,
67  const pinocchio::ReferenceFrame type,
68  const std::size_t nu);
69 
81  ResidualModelFrameVelocityTpl(boost::shared_ptr<StateMultibody> state,
82  const pinocchio::FrameIndex id,
83  const Motion& velocity,
84  const pinocchio::ReferenceFrame type);
86 
94  virtual void calc(const boost::shared_ptr<ResidualDataAbstract>& data,
95  const Eigen::Ref<const VectorXs>& x,
96  const Eigen::Ref<const VectorXs>& u);
97 
105  virtual void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data,
106  const Eigen::Ref<const VectorXs>& x,
107  const Eigen::Ref<const VectorXs>& u);
108 
112  virtual boost::shared_ptr<ResidualDataAbstract> createData(
113  DataCollectorAbstract* const data);
114 
118  pinocchio::FrameIndex get_id() const;
119 
123  const Motion& get_reference() const;
124 
128  pinocchio::ReferenceFrame get_type() const;
129 
133  void set_id(const pinocchio::FrameIndex id);
134 
138  void set_reference(const Motion& velocity);
139 
143  void set_type(const pinocchio::ReferenceFrame type);
144 
150  virtual void print(std::ostream& os) const;
151 
152  protected:
153  using Base::nr_;
154  using Base::nu_;
155  using Base::state_;
156  using Base::u_dependent_;
157 
158  private:
159  pinocchio::FrameIndex id_;
160  Motion vref_;
161  pinocchio::ReferenceFrame type_;
162  boost::shared_ptr<typename StateMultibody::PinocchioModel>
163  pin_model_;
164 };
165 
166 template <typename _Scalar>
168  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
169 
170  typedef _Scalar Scalar;
174  typedef typename MathBase::Matrix6xs Matrix6xs;
175 
176  template <template <typename Scalar> class Model>
177  ResidualDataFrameVelocityTpl(Model<Scalar>* const model,
178  DataCollectorAbstract* const data)
179  : Base(model, data) {
180  // Check that proper shared data has been passed
183  if (d == NULL) {
184  throw_pretty(
185  "Invalid argument: the shared data should be derived from "
186  "DataCollectorMultibody");
187  }
188 
189  // Avoids data casting at runtime
190  pinocchio = d->pinocchio;
191  }
192 
193  pinocchio::DataTpl<Scalar>* pinocchio;
194  using Base::r;
195  using Base::Ru;
196  using Base::Rx;
197  using Base::shared;
198 };
199 
200 } // namespace crocoddyl
201 
202 /* --- Details -------------------------------------------------------------- */
203 /* --- Details -------------------------------------------------------------- */
204 /* --- Details -------------------------------------------------------------- */
205 #include "crocoddyl/multibody/residuals/frame-velocity.hxx"
206 
207 #endif // CROCODDYL_MULTIBODY_RESIDUALS_FRAME_VELOCITY_HPP_
Abstract class for residual models.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
std::size_t nr_
Residual vector dimension.
const Motion & get_reference() const
Return the reference velocity.
virtual void print(std::ostream &os) const
Print relevant information of the frame-velocity residual.
void set_reference(const Motion &velocity)
Modify reference velocity.
ResidualModelFrameVelocityTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Motion &velocity, const pinocchio::ReferenceFrame type)
Initialize the frame velocity residual model.
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.
void set_id(const pinocchio::FrameIndex id)
Modify 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 velocity residual vector.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
void set_type(const pinocchio::ReferenceFrame type)
Modify reference type of velocity.
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.
pinocchio::ReferenceFrame get_type() const
Return the reference type of velocity.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the frame velocity residual data.
State multibody representation.
Definition: multibody.hpp:35
MatrixXs Ru
Jacobian of the residual vector with respect the control.
MatrixXs Rx
Jacobian of the residual vector with respect the state.
DataCollectorAbstract * shared
Shared data allocated by the action model.
DataCollectorAbstract * shared
Shared data allocated by the action model.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.