5 #ifndef __pinocchio_force_set_hpp__ 6 #define __pinocchio_force_set_hpp__ 8 #include "pinocchio/spatial/fwd.hpp" 9 #include <Eigen/Geometry> 13 template<
typename _Scalar,
int _Options>
19 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
20 typedef Eigen::Matrix<Scalar,3,3,Options>
Matrix3;
21 typedef Eigen::Matrix<Scalar,6,1,Options>
Vector6;
22 typedef Eigen::Matrix<Scalar,6,6,Options>
Matrix6;
25 typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options>
Matrix3x;
26 typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
Matrix6x;
31 {
m_f.fill(NAN);
m_n.fill(NAN); }
34 { assert( linear.cols() == angular.cols() ); }
67 <<
"F =\n" << phi.
linear() << std::endl
68 <<
"Tau =\n" << phi.
angular() << std::endl;
78 : ref(ref), idx(idx), len(len) {}
80 Eigen::Block<ForceSetTpl::Matrix3x>
linear() {
return ref.
m_f.block(0,idx,3,len); }
81 Eigen::Block<ForceSetTpl::Matrix3x>
angular() {
return ref.
m_n.block(0,idx,3,len); }
82 Eigen::Block<const ForceSetTpl::Matrix3x>
linear()
const 84 Eigen::Block<const ForceSetTpl::Matrix3x>
angular()
const 95 assert(copy.
size == len);
103 assert(copy.
len == len);
109 template <
typename D>
112 eigen_assert(D::RowsAtCompileTime == 6);
113 assert(m.cols() ==
len);
114 linear() = m.template topRows<3>();
115 angular() = m.template bottomRows<3>();
164 #endif // ifndef __pinocchio_force_set_hpp__
Eigen::Block< const ForceSetTpl::Matrix3x > angular() const
Eigen::Block< const ForceSetTpl::Matrix3x > linear() const
ForceSetTpl se3Action(const SE3 &m) const
af = aXb.act(bf)
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
ForceSetTpl::Matrix6x matrix() const
ForceSetTpl se3Action(const SE3 &m) const
af = aXb.act(bf)
Block block(const int &idx, const int &len)
ForceSetTpl< double, 0 > ForceSet
Block & operator=(const ForceSetTpl ©)
Eigen::Block< ForceSetTpl::Matrix3x > linear()
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
const Matrix3x & angular() const
ForceSetTpl(const int &ncols)
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
Eigen::Block< ForceSetTpl::Matrix3x > angular()
ConstAngularRef rotation() const
Main pinocchio namespace.
const Matrix3x & linear() const
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
void copy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &origin, DataTpl< Scalar, Options, JointCollectionTpl > &dest, KinematicLevel kinematic_level)
Copy part of the data from origin to dest. Template parameter can be used to select at which differen...
ForceSetTpl se3ActionInverse(const SE3 &m) const
bf = aXb.actInv(af)
ForceSetTpl se3ActionInverse(const SE3 &m) const
bf = aXb.actInv(af)
SE3Tpl< Scalar, Options > SE3
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
Block(ForceSetTpl &ref, const int &idx, const int &len)
Eigen::Matrix< Scalar, 3, 3, Options > Matrix3
friend std::ostream & operator<<(std::ostream &os, const ForceSetTpl &phi)
ConstLinearRef translation() const
ForceSetTpl(const Matrix3x &linear, const Matrix3x &angular)