9 #ifndef CROCODDYL_MULTIBODY_RESIDUALS_FRAME_PLACEMENT_HPP_
10 #define CROCODDYL_MULTIBODY_RESIDUALS_FRAME_PLACEMENT_HPP_
12 #include <pinocchio/multibody/fwd.hpp>
13 #include <pinocchio/spatial/se3.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"
36 template <
typename _Scalar>
39 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 typedef _Scalar Scalar;
48 typedef typename MathBase::VectorXs VectorXs;
49 typedef pinocchio::SE3Tpl<Scalar> SE3;
60 const SE3& pref,
const std::size_t nu);
82 virtual void calc(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
83 const Eigen::Ref<const VectorXs>& u);
92 virtual void calcDiff(
const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
93 const Eigen::Ref<const VectorXs>& u);
113 void set_id(
const pinocchio::FrameIndex
id);
125 virtual void print(std::ostream& os)
const;
135 pinocchio::FrameIndex id_;
137 pinocchio::SE3Tpl<Scalar> oMf_inv_;
138 boost::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
141 template <
typename _Scalar>
143 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145 typedef _Scalar Scalar;
149 typedef typename MathBase::Matrix6xs Matrix6xs;
150 typedef typename MathBase::Matrix6s Matrix6s;
151 typedef typename MathBase::Vector6s Vector6s;
153 template <
template <
typename Scalar>
class Model>
155 :
Base(model, data),
rJf(6, 6),
fJf(6, model->get_state()->get_nv()) {
162 throw_pretty(
"Invalid argument: the shared data should be derived from DataCollectorMultibody");
170 pinocchio::SE3Tpl<Scalar>
rMf;
185 #include "crocoddyl/multibody/residuals/frame-placement.hxx"
Abstract class for residual models.
bool u_dependent_
Label that indicates if the residual function depends on u.
boost::shared_ptr< StateAbstract > state_
State description.
std::size_t nu_
Control dimension.
VectorXs unone_
No control vector.
bool v_dependent_
Label that indicates if the residual function depends on v.
Frame placement residual.
ResidualModelFramePlacementTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const SE3 &pref, const std::size_t nu)
Initialize the frame placement residual model.
ResidualModelFramePlacementTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const SE3 &pref)
Initialize the frame placement residual model.
const SE3 & get_reference() const
Return the reference frame placement.
virtual void print(std::ostream &os) const
Print relevant information of the frame-placement residual.
void set_id(const pinocchio::FrameIndex id)
Modify 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 placement residual.
pinocchio::FrameIndex get_id() const
Return the reference frame id.
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 placement residual.
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Create the frame placement residual data.
void set_reference(const SE3 &reference)
Modify the reference frame placement.
State multibody representation.
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.
VectorXs r
Residual vector.
pinocchio::SE3Tpl< Scalar > rMf
Error frame placement of the frame.
Matrix6s rJf
Error Jacobian of the frame.
Matrix6xs fJf
Local Jacobian of the frame.
DataCollectorAbstract * shared
Shared data allocated by the action model.
VectorXs r
Residual vector.
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.