9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_FRAME_VELOCITY_HPP_ 10 #define CROCODDYL_MULTIBODY_RESIDUALS_FRAME_VELOCITY_HPP_ 12 #include <pinocchio/multibody/fwd.hpp> 13 #include <pinocchio/spatial/motion.hpp> 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" 37 template <
typename _Scalar>
38 class ResidualModelFrameVelocityTpl :
public ResidualModelAbstractTpl<_Scalar> {
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
62 const Motion& velocity,
const pinocchio::ReferenceFrame type,
const std::size_t nu);
75 const Motion& velocity,
const pinocchio::ReferenceFrame type);
85 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
86 const Eigen::Ref<const VectorXs>& u);
95 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
96 const Eigen::Ref<const VectorXs>& u);
101 virtual boost::shared_ptr<ResidualDataAbstract>
createData(DataCollectorAbstract*
const data);
106 pinocchio::FrameIndex
get_id()
const;
116 pinocchio::ReferenceFrame
get_type()
const;
121 void set_id(
const pinocchio::FrameIndex
id);
131 void set_type(
const pinocchio::ReferenceFrame type);
138 virtual void print(std::ostream& os)
const;
148 pinocchio::FrameIndex id_;
150 pinocchio::ReferenceFrame type_;
151 boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
154 template <
typename _Scalar>
155 struct ResidualDataFrameVelocityTpl :
public ResidualDataAbstractTpl<_Scalar> {
156 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
164 template <
template <
typename Scalar>
class Model>
165 ResidualDataFrameVelocityTpl(Model<Scalar>*
const model, DataCollectorAbstract*
const data) : Base(model, data) {
167 DataCollectorMultibodyTpl<Scalar>* d =
dynamic_cast<DataCollectorMultibodyTpl<Scalar>*
>(
shared);
169 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorMultibody");
188 #include "crocoddyl/multibody/residuals/frame-velocity.hxx" 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.