joint-prismatic.hpp 9.87 KB
 Valenza Florian committed Mar 30, 2015 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 ``````#ifndef __se3_joint_prismatic_hpp__ #define __se3_joint_prismatic_hpp__ #include "pinocchio/multibody/joint/joint-base.hpp" #include "pinocchio/multibody/constraint.hpp" #include "pinocchio/spatial/inertia.hpp" namespace se3 { template struct JointDataPrismatic; template struct JointModelPrismatic; namespace prismatic { template struct CartesianVector3 { double v; CartesianVector3(const double & v) : v(v) {} `````` 21 22 `````` CartesianVector3() : v(NAN) {} operator Eigen::Vector3d () const; // { return Eigen::Vector3d(w,0,0); } `````` Valenza Florian committed Mar 30, 2015 23 `````` }; `````` 24 25 26 `````` template<>CartesianVector3<0>::operator Eigen::Vector3d () const { return Eigen::Vector3d(v,0,0); } template<>CartesianVector3<1>::operator Eigen::Vector3d () const { return Eigen::Vector3d(0,v,0); } template<>CartesianVector3<2>::operator Eigen::Vector3d () const { return Eigen::Vector3d(0,0,v); } `````` Valenza Florian committed Mar 30, 2015 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 `````` Eigen::Vector3d operator+ (const Eigen::Vector3d & v1,const CartesianVector3<0> & vx) { return Eigen::Vector3d(v1[0]+vx.v,v1[1],v1[2]); } Eigen::Vector3d operator+ (const Eigen::Vector3d & v1,const CartesianVector3<1> & vy) { return Eigen::Vector3d(v1[0],v1[1]+vy.v,v1[2]); } Eigen::Vector3d operator+ (const Eigen::Vector3d & v1,const CartesianVector3<2> & vz) { return Eigen::Vector3d(v1[0],v1[1],v1[2]+vz.v); } } // namespace prismatic template struct JointPrismatic { struct BiasZero { operator Motion () const { return Motion::Zero(); } }; friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; } friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; } struct MotionPrismatic { MotionPrismatic() : v(NAN) {} MotionPrismatic( const double & v ) : v(v) {} double v; operator Motion() const { return Motion((Motion::Vector3)typename prismatic::CartesianVector3(v), Motion::Vector3::Zero()); } }; // struct MotionPrismatic friend const MotionPrismatic& operator+ (const MotionPrismatic& m, const BiasZero&) { return m; } friend Motion operator+( const MotionPrismatic& m1, const Motion& m2) { return Motion( m2.linear()+typename prismatic::CartesianVector3(m1.v),m2.angular()); } struct ConstraintPrismatic { template MotionPrismatic operator*( const Eigen::MatrixBase & v ) const `````` 69 70 71 72 `````` { // EIGEN_STATIC_ASSERT_SIZE_1x1(D); // There is actually a bug in Eigen with such a macro return MotionPrismatic(v[0]); } `````` Valenza Florian committed Mar 30, 2015 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 `````` Eigen::Matrix se3Action(const SE3 & m) const { Eigen::Matrix res; res.head<3>() = m.rotation().col(axis); res.tail<3>() = Motion::Vector3::Zero(); // Eigen::Vector3d::Zero() ? return res; } struct TransposeConst { const ConstraintPrismatic & ref; TransposeConst(const ConstraintPrismatic & ref) : ref(ref) {} Force::Vector3::ConstFixedSegmentReturnType<1>::Type operator*( const Force& f ) const { return f.linear().segment<1>(axis); } /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ template friend typename Eigen::MatrixBase::ConstRowXpr operator*( const TransposeConst &, const Eigen::MatrixBase & F ) { assert(F.rows()==6); return F.row(axis); } }; TransposeConst transpose() const { return TransposeConst(*this); } /* CRBA joint operators * - ForceSet::Block = ForceSet * - ForceSet operator* (Inertia Y,Constraint S) * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) * - SE3::act(ForceSet::Block) */ operator ConstraintXd () const { Eigen::Matrix S; S << (Eigen::Vector3d)prismatic::CartesianVector3(), Eigen::Vector3d::Zero() ; return ConstraintXd(S); } }; // struct ConstraintPrismatic static Eigen::Vector3d cartesianTranslation(const double & shift); }; Motion operator^( const Motion& m1, const JointPrismatic<0>::MotionPrismatic& m2) { /* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 ) * nu1^(v2,0) = ( w1^v2 , 0 ) * (x,y,z)^(v,0,0) = ( 0,zv,-yv ) * nu1^(0,vx) = ( 0,wz1 vx,-wy1 vx, 0, 0, 0) */ const Motion::Vector3& w = m1.angular(); const double & vx = m2.v; return Motion( Motion::Vector3(0,w[2]*vx,-w[1]*vx), Motion::Vector3::Zero()); } Motion operator^( const Motion& m1, const JointPrismatic<1>::MotionPrismatic& m2) { /* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 ) * nu1^(v2,0) = ( w1^v2 , 0 ) * (x,y,z)^(0,v,0) = ( -zv,0,xv ) * nu1^(0,vx) = ( -vz1 vx,0,vx1 vx, 0, 0, 0) */ const Motion::Vector3& w = m1.angular(); const double & vx = m2.v; return Motion( Motion::Vector3(-w[2]*vx,0,w[0]*vx), Motion::Vector3::Zero()); } Motion operator^( const Motion& m1, const JointPrismatic<2>::MotionPrismatic& m2) { /* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 ) * nu1^(v2,0) = ( w1^v2 , 0 ) * (x,y,z)^(0,0,v) = ( yv,-xv,0 ) * nu1^(0,vx) = ( vy1 vx,-vx1 vx, 0, 0, 0, 0 ) */ const Motion::Vector3& w = m1.angular(); const double & vx = m2.v; return Motion( Motion::Vector3(w[1]*vx,-w[0]*vx,0), Motion::Vector3::Zero()); } template<> Eigen::Vector3d JointPrismatic<0>::cartesianTranslation(const double & shift) { return Motion::Vector3(shift,0,0); } template<> Eigen::Vector3d JointPrismatic<1>::cartesianTranslation(const double & shift) { return Motion::Vector3(0,shift,0); } template<> Eigen::Vector3d JointPrismatic<2>::cartesianTranslation(const double & shift) { return Motion::Vector3(0,0,shift); } /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ Eigen::Matrix operator*( const Inertia& Y,const JointPrismatic<0>::ConstraintPrismatic & ) { /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */ const double &m = Y.mass(), &y = Y.lever()[1], &z = Y.lever()[2]; Eigen::Matrix res; res << m,0.0,0.0, 0.0, m*z, -m*y ; return res; } /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ Eigen::Matrix operator*( const Inertia& Y,const JointPrismatic<1>::ConstraintPrismatic & ) { /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */ const double &m = Y.mass(), &x = Y.lever()[0], &z = Y.lever()[2]; Eigen::Matrix res; res << 0.0,m,0.0, -m*z, 0.0, m*x ; return res; } /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ Eigen::Matrix operator*( const Inertia& Y,const JointPrismatic<2>::ConstraintPrismatic & ) { /* Y(:,2) = ( 0,0, 1, y , -x , 0) */ const double &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1]; Eigen::Matrix res; res << 0.0,0.0,m, m*y, -m*x, 0.0; return res; } namespace internal { // TODO: I am not able to write the next three lines as a template. Why? template<> struct ActionReturn::ConstraintPrismatic > { typedef Eigen::Matrix Type; }; template<> struct ActionReturn::ConstraintPrismatic > { typedef Eigen::Matrix Type; }; template<> struct ActionReturn::ConstraintPrismatic > { typedef Eigen::Matrix Type; }; } template struct traits< JointPrismatic > { typedef JointDataPrismatic JointData; typedef JointModelPrismatic JointModel; typedef typename JointPrismatic::ConstraintPrismatic Constraint_t; typedef SE3 Transformation_t; typedef typename JointPrismatic::MotionPrismatic Motion_t; typedef typename JointPrismatic::BiasZero Bias_t; typedef Eigen::Matrix F_t; enum { NQ = 1, NV = 1 }; }; template struct traits< JointDataPrismatic > { typedef JointPrismatic Joint; }; template struct traits< JointModelPrismatic > { typedef JointPrismatic Joint; }; template struct JointDataPrismatic : public JointDataBase< JointDataPrismatic > { //TODO : check. typedef JointPrismatic Joint; SE3_JOINT_TYPEDEF; Constraint_t S; Transformation_t M; Motion_t v; Bias_t c; F_t F; JointDataPrismatic() : M(1) // Etat initial de la liaison ? { M.rotation(SE3::Matrix3::Identity()); } }; template struct JointModelPrismatic : public JointModelBase< JointModelPrismatic > { //TODO typedef JointPrismatic Joint; SE3_JOINT_TYPEDEF; using JointModelBase::idx_q; using JointModelBase::idx_v; using JointModelBase::setIndexes; JointData createData() const { return JointData(); } void calc( JointData& data, const Eigen::VectorXd & qs ) const { const double & q = qs[idx_q()]; data.M.translation(JointPrismatic::cartesianTranslation(q)); } void calc( JointData& data, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs ) const { const double & q = qs[idx_q()]; const double & v = vs[idx_v()]; data.M.translation(JointPrismatic::cartesianTranslation(q)); data.v.v = v; } }; typedef JointPrismatic<0> JointPX; typedef JointDataPrismatic<0> JointDataPX; typedef JointModelPrismatic<0> JointModelPX; typedef JointPrismatic<1> JointPY; typedef JointDataPrismatic<1> JointDataPY; typedef JointModelPrismatic<1> JointModelPY; typedef JointPrismatic<2> JointPZ; typedef JointDataPrismatic<2> JointDataPZ; typedef JointModelPrismatic<2> JointModelPZ; } //namespace se3 #endif // ifndef __se3_joint_prismatic_hpp__``````