Commit 31562102 authored by jcarpent's avatar jcarpent
Browse files

[Joints] Add variation operation for Constraints struct

parent bf00b109
......@@ -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;}
......
......@@ -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>
......
......@@ -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>
......
......@@ -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
......
......@@ -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
......
......@@ -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
......
......@@ -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>
......
......@@ -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
......
......@@ -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
......
......@@ -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
......
......@@ -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{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment