[Joints] Add variation operation for Constraints struct

src/multibody/constraint.hpp

@@ -189,6 +189,13 @@ namespace se3
     {
       return (m.inverse().toActionMatrix()*S).eval();
     }
+    
+    DenseBase variation(const Motion & v) const
+    {
+      DenseBase res(v.toActionMatrix() * S);
+      
+      return res;
+    }
 
     void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
     

src/multibody/joint/joint-free-flyer.hpp

@@ -84,6 +84,8 @@ namespace se3
       
       TransposeConst transpose() const { return TransposeConst(); }
       operator ConstraintXd () const { return ConstraintXd(SE3::Matrix6::Identity()); }
+      
+      DenseBase variation(const Motion & m) const { return m.toActionMatrix(); }
     }; // struct ConstraintIdentity
 
     template<typename D>

src/multibody/joint/joint-planar.hpp

@@ -189,6 +189,20 @@ namespace se3
       return X_subspace;
     }
 
+    DenseBase variation(const Motion & m) const
+    {
+      const Motion::ConstLinear_t v = m.linear();
+      const Motion::ConstAngular_t w = m.angular();
+      DenseBase res(DenseBase::Zero());
+      
+      res(0,1) = -w[2]; res(0,2) = v[1];
+      res(1,0) = w[2]; res(1,2) = -v[0];
+      res(2,0) = -w[1]; res(2,1) = w[0];
+      res(3,2) = w[1];
+      res(4,2) = -w[0];
+      
+      return res;
+    }
   }; // struct ConstraintPlanar
 
   template<typename D>

src/multibody/joint/joint-prismatic-unaligned.hpp

@@ -191,6 +191,14 @@ namespace se3
       	return ConstraintXd(S);
       }
       
+      DenseBase variation(const Motion & m) const
+      {
+        DenseBase res;
+        res << m.angular().cross(axis), Vector3::Zero();
+        
+        return res;
+      }
+      
     }; // struct ConstraintPrismaticUnaligned
 
 

src/multibody/joint/joint-prismatic.hpp

@@ -196,6 +196,14 @@ namespace se3
       S << prismatic::CartesianVector3<axis>(1).vector(), Eigen::Vector3d::Zero();
       return ConstraintXd(S);
     }
+    
+    DenseBase variation(const Motion & m) const
+    {
+      DenseBase res;
+      res << m.angular().cross(prismatic::CartesianVector3<axis>(1).vector()), Vector3::Zero();
+      
+      return res;
+    }
 
   }; // struct ConstraintPrismatic
 

src/multibody/joint/joint-revolute-unaligned.hpp

@@ -192,6 +192,17 @@ namespace se3
       	return ConstraintXd(S);
       }
       
+      DenseBase variation(const Motion & m) const
+      {
+        const Motion::ConstLinear_t v = m.linear();
+        const Motion::ConstAngular_t w = m.angular();
+        
+        DenseBase res;
+        res << v.cross(axis), w.cross(axis);
+        
+        return res;
+      }
+      
     }; // struct ConstraintRevoluteUnaligned
 
 

src/multibody/joint/joint-revolute.hpp

@@ -198,6 +198,18 @@ namespace se3
       S << Eigen::Vector3d::Zero(), revolute::CartesianVector3<axis>(1).vector();
       return ConstraintXd(S);
     }
+    
+    DenseBase variation(const Motion & m) const
+    {
+      const typename Motion::ConstLinear_t v = m.linear();
+      const typename Motion::ConstAngular_t w = m.angular();
+      
+      const Vector3 a(revolute::CartesianVector3<axis>(1).vector());
+      DenseBase res;
+      res << v.cross(a), w.cross(a);
+      
+      return res;
+    }
   }; // struct ConstraintRevolute
 
   template<int axis> 

src/multibody/joint/joint-spherical-ZYX.hpp

@@ -97,6 +97,7 @@ namespace se3
       typedef Eigen::Matrix <_Scalar,3,3,_Options> Matrix3;
       typedef Eigen::Matrix <_Scalar,3,1,_Options> Vector3;
       typedef Eigen::Matrix <_Scalar,6,3,_Options> ConstraintDense;
+      typedef Eigen::Matrix <_Scalar,6,3,_Options> DenseBase;
 
       Matrix3 S_minimal;
 
@@ -187,6 +188,18 @@ namespace se3
                                  
         return result;
       }
+      
+      DenseBase variation(const Motion & m) const
+      {
+        const typename Motion::ConstLinear_t v = m.linear();
+        const typename Motion::ConstAngular_t w = m.angular();
+        
+        DenseBase res;
+        res.template middleRows<3>(Motion::LINEAR) = cross(v,S_minimal);
+        res.template middleRows<3>(Motion::ANGULAR) = cross(w,S_minimal);
+        
+        return res;
+      }
 
     }; // struct ConstraintRotationalSubspace
 

src/multibody/joint/joint-spherical.hpp

@@ -156,11 +156,23 @@ namespace se3
     Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
     {
       Eigen::Matrix <double,6,3> X_subspace;
-      X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
+      X_subspace.block <3,3> (Motion::LINEAR, 0) = cross(m.translation(),m.rotation ());
       X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
 
       return X_subspace;
     }
+    
+    DenseBase variation(const Motion & m) const
+    {
+      const Motion::ConstLinear_t v = m.linear();
+      const Motion::ConstAngular_t w = m.angular();
+      
+      DenseBase res;
+      res.middleRows<3>(LINEAR) = skew(v);
+      res.middleRows<3>(ANGULAR) = skew(w);
+      
+      return res;
+    }
 
   }; // struct ConstraintRotationalSubspace
 

src/multibody/joint/joint-translation.hpp

@@ -172,6 +172,17 @@ namespace se3
 
       return M;
     }
+    
+    DenseBase variation(const Motion & m) const
+    {
+      const Motion::ConstAngular_t w = m.angular();
+      
+      DenseBase res;
+      res.middleRows<3>(LINEAR) = skew(w);
+      res.middleRows<3>(ANGULAR).setZero();
+      
+      return res;
+    }
 
   }; // struct ConstraintTranslationSubspace
 

unittest/joint.cpp

@@ -23,8 +23,8 @@
 
 using namespace se3;
 
-template <typename T>
-void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
+template <typename JointModel>
+void test_joint_methods (JointModel & jmodel, typename JointModel::JointDataDerived & jdata)
 {
     std::cout << "Testing Joint over " << jmodel.shortname() << std::endl;
     Eigen::VectorXd q1(Eigen::VectorXd::Random (jmodel.nq()));
@@ -75,6 +75,18 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
     BOOST_CHECK_MESSAGE((jda.U()).isApprox(jdata.U),std::string(error_prefix + " - Joint U inertia matrix decomposition "));
     BOOST_CHECK_MESSAGE((jda.Dinv()).isApprox(jdata.Dinv),std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
     BOOST_CHECK_MESSAGE((jda.UDinv()).isApprox(jdata.UDinv),std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));
+  
+  // Test vxS
+  typedef typename JointModel::Constraint_t Constraint_t;
+  typedef typename Constraint_t::DenseBase ConstraintDense;
+  
+  Motion v(Motion::Random());
+  ConstraintDense vxS(jdata.S.variation(v));
+  ConstraintDense vxS_ref = v.toActionMatrix() * ConstraintXd(jdata.S).matrix();
+  
+  BOOST_CHECK_MESSAGE(vxS.isApprox(vxS_ref),std::string(error_prefix + "- Joint vxS operation "));
+  
+  
 }
 
 struct TestJoint{