Commit 87db739f authored by jcarpent's avatar jcarpent
Browse files

[Constraint] Avoid explicit cast of sparse constraint to dense one. Add matrix...

[Constraint] Avoid explicit cast of sparse constraint to dense one. Add matrix methods with correct output
parent dc475917
......@@ -42,6 +42,8 @@ namespace se3
typedef typename traits<Derived>::JointMotion JointMotion;
typedef typename traits<Derived>::JointForce JointForce;
typedef typename traits<Derived>::DenseBase DenseBase;
typedef typename traits<Derived>::MatrixReturnType MatrixReturnType;
typedef typename traits<Derived>::ConstMatrixReturnType ConstMatrixReturnType;
public:
Derived & derived() { return *static_cast<Derived*>(this); }
......@@ -49,8 +51,9 @@ namespace se3
Motion operator* (const JointMotion& vj) const { return derived().__mult__(vj); }
DenseBase & matrix() { return derived().matrix_impl(); }
const DenseBase & matrix() const { return derived().matrix_impl(); }
MatrixReturnType matrix() { return derived().matrix_impl(); }
ConstMatrixReturnType matrix() const { return derived().matrix_impl(); }
int nv() const { return derived().nv_impl(); }
template<class OtherDerived>
......@@ -97,6 +100,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,D,1,U> JointMotion;
typedef Eigen::Matrix<Scalar,D,1,U> JointForce;
typedef Eigen::Matrix<Scalar,6,D> DenseBase;
typedef typename EIGEN_REF_CONSTTYPE(DenseBase) ConstMatrixReturnType;
typedef typename EIGEN_REF_TYPE(DenseBase) MatrixReturnType;
}; // traits ConstraintTpl
......@@ -125,6 +130,8 @@ namespace se3
typedef typename Base::JointMotion JointMotion;
typedef typename Base::JointForce JointForce;
typedef typename Base::DenseBase DenseBase;
typedef typename Base::ConstMatrixReturnType ConstMatrixReturnType;
typedef typename Base::MatrixReturnType MatrixReturnType;
enum { NV = _Dim, Options = _Options };
......@@ -181,8 +188,8 @@ namespace se3
Transpose transpose() const { return Transpose(*this); }
DenseBase & matrix_impl() { return S; }
const DenseBase & matrix_impl() const { return S; }
MatrixReturnType matrix_impl() { return S; }
ConstMatrixReturnType matrix_impl() const { return S; }
int nv_impl() const
{
......@@ -194,9 +201,9 @@ namespace se3
template<typename S2,int O2>
friend typename ConstraintTpl<_Dim,_Scalar,_Options>::DenseBase
operator*(const InertiaTpl<S2,O2> & Y, const ConstraintTpl<_Dim,_Scalar,_Options> & S)
operator*(const InertiaTpl<S2,O2> & Y, const ConstraintTpl & S)
{
typedef typename ConstraintTpl<_Dim,_Scalar,_Options>::DenseBase ReturnType;
typedef typename ConstraintTpl::DenseBase ReturnType;
ReturnType res(6,S.nv());
motionSet::inertiaAction(Y,S.S,res);
return res;
......@@ -204,14 +211,13 @@ namespace se3
template<typename S2,int O2>
friend Eigen::Matrix<_Scalar,6,_Dim>
operator*(const Eigen::Matrix<S2,6,6,O2> & Ymatrix, const ConstraintTpl<_Dim,_Scalar,_Options> & S)
operator*(const Eigen::Matrix<S2,6,6,O2> & Ymatrix, const ConstraintTpl & S)
{
typedef Eigen::Matrix<_Scalar,6,_Dim> ReturnType;
return ReturnType(Ymatrix*S.matrix());
}
DenseBase se3Action(const SE3 & m) const
{
DenseBase res(6,nv());
......
......@@ -488,7 +488,8 @@ namespace se3
public:
template <typename D>
ConstraintXd operator()(const JointDataBase<D> & jdata) const
{ return ConstraintXd(jdata.S()); }
{
return ConstraintXd(jdata.S().matrix()); }
static ConstraintXd run( const JointDataVariant & jdata)
{ return boost::apply_visitor( JointConstraintVisitor (), jdata ); }
......
......@@ -61,6 +61,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,6,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,6,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,6> DenseBase;
typedef const DenseBase ConstMatrixReturnType;
typedef DenseBase MatrixReturnType;
}; // traits ConstraintRevolute
......@@ -85,7 +87,7 @@ namespace se3
};
TransposeConst transpose() const { return TransposeConst(); }
operator ConstraintXd () const { return ConstraintXd(SE3::Matrix6::Identity()); }
DenseBase matrix_impl() const { return DenseBase::Identity(); }
DenseBase motionAction(const Motion & v) const { return v.toActionMatrix(); }
}; // struct ConstraintIdentity
......
......@@ -122,6 +122,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,3,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,3,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,3> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // struct traits ConstraintPlanar
......@@ -169,15 +171,15 @@ namespace se3
ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
operator ConstraintXd () const
DenseBase matrix_impl() const
{
Eigen::Matrix<Scalar,6,3> S;
DenseBase S;
S.block <3,3> (Inertia::LINEAR, 0).setZero ();
S.block <3,3> (Inertia::ANGULAR, 0).setZero ();
S(Inertia::LINEAR + 0,0) = 1.;
S(Inertia::LINEAR + 1,1) = 1.;
S(Inertia::ANGULAR + 2,2) = 1.;
return ConstraintXd(S);
return S;
}
Eigen::Matrix <Scalar,6,3> se3Action (const SE3 & m) const
......
......@@ -107,6 +107,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,1,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,1,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,1> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // traits ConstraintPrismaticUnaligned
struct ConstraintPrismaticUnaligned : ConstraintBase <ConstraintPrismaticUnaligned>
......@@ -193,11 +195,11 @@ namespace se3
* - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
* - SE3::act(ForceSet::Block)
*/
operator ConstraintXd () const
DenseBase matrix_impl() const
{
Vector6 S;
DenseBase S;
S << axis, Vector3::Zero();
return ConstraintXd(S);
return S;
}
DenseBase motionAction(const Motion & m) const
......
......@@ -136,6 +136,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,1,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,1,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,1> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // traits ConstraintRevolute
template <int axis>
......@@ -192,11 +194,11 @@ namespace se3
* - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
* - SE3::act(ForceSet::Block)
*/
operator ConstraintXd () const
DenseBase matrix_impl() const
{
Eigen::Matrix<double,6,1> S;
DenseBase S;
S << prismatic::CartesianVector3<axis>(1).vector(), Eigen::Vector3d::Zero();
return ConstraintXd(S);
return S;
}
DenseBase motionAction(const Motion & m) const
......
......@@ -110,6 +110,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,1,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,1,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,1> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // traits ConstraintRevoluteUnaligned
......@@ -195,11 +197,11 @@ namespace se3
* - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
* - SE3::act(ForceSet::Block)
*/
operator ConstraintXd () const
DenseBase matrix_impl() const
{
DenseBase S;
S << Eigen::Vector3d::Zero(), axis;
return ConstraintXd(S);
return S;
}
DenseBase motionAction(const Motion & m) const
......
......@@ -151,6 +151,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,1,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,1,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,1> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // traits ConstraintRevolute
template<int axis>
......@@ -205,11 +207,11 @@ namespace se3
* - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
* - SE3::act(ForceSet::Block)
*/
operator ConstraintXd () const
DenseBase matrix_impl() const
{
Eigen::Matrix<double,6,1> S;
DenseBase S;
S << Eigen::Vector3d::Zero(), revolute::CartesianVector3<axis>(1).vector();
return ConstraintXd(S);
return S;
}
DenseBase motionAction(const Motion & m) const
......
......@@ -128,7 +128,41 @@ namespace se3
}
template <typename _Scalar, int _Options>
struct ConstraintRotationalSubspaceTpl
struct ConstraintRotationalSubspaceTpl;
template <typename _Scalar, int _Options>
struct traits < struct ConstraintRotationalSubspaceTpl<_Scalar,_Options> >
{
typedef _Scalar Scalar;
enum { Options = _Options };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef Matrix3 Angular_t;
typedef Vector3 Linear_t;
typedef const Matrix3 ConstAngular_t;
typedef const Vector3 ConstLinear_t;
typedef Matrix6 ActionMatrix_t;
typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
typedef SE3Tpl<Scalar,Options> SE3;
typedef ForceTpl<Scalar,Options> Force;
typedef MotionTpl<Scalar,Options> Motion;
typedef Symmetric3Tpl<Scalar,Options> Symmetric3;
enum {
LINEAR = 0,
ANGULAR = 3
};
typedef Eigen::Matrix<Scalar,3,1,Options> JointMotion;
typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // struct traits struct ConstraintRotationalSubspace
template <typename _Scalar, int _Options>
struct ConstraintRotationalSubspaceTpl : public ConstraintBase< ConstraintRotationalSubspaceTpl<_Scalar,_Options> >
{
enum { NV = 3, Options = _Options };
typedef _Scalar Scalar;
......@@ -149,9 +183,6 @@ namespace se3
Matrix3 & operator() () { return S_minimal; }
const Matrix3 & operator() () const { return S_minimal; }
Matrix3 & matrix () { return S_minimal; }
const Matrix3 & matrix () const { return S_minimal; }
int nv_impl() const { return NV; }
struct ConstraintTranspose
......@@ -199,12 +230,12 @@ namespace se3
ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
operator ConstraintXd () const
DenseBase matrix_impl() const
{
ConstraintDense S;
(S.template block <3,3> (Inertia::LINEAR, 0)).setZero ();
S.template block <3,3> (Inertia::ANGULAR, 0) = S_minimal;
return ConstraintXd(S);
return S;
}
// const typename Eigen::ProductReturnType<
......@@ -294,7 +325,7 @@ namespace se3
M.template bottomRows<3> () = (Y.inertia () -
typename Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ())).matrix();
return (M * S.matrix ()).eval();
return (M * S.S_minimal).eval();
}
/* [ABA] Y*S operator (Inertia Y,Constraint S) */
......@@ -428,7 +459,7 @@ namespace se3
c1 * s2,
c1 * c2;
data.S.matrix () << -s1, 0., 1., c1 * s2, c2, 0, c1 * c2, -s2, 0;
data.S.S_minimal << -s1, 0., 1., c1 * s2, c2, 0, c1 * c2, -s2, 0;
}
void calc (JointDataDerived & data,
......@@ -453,9 +484,9 @@ namespace se3
c1 * c2;
data.S.matrix () << -s1, 0., 1., c1 * s2, c2, 0, c1 * c2, -s2, 0;
data.S.S_minimal << -s1, 0., 1., c1 * s2, c2, 0, c1 * c2, -s2, 0;
data.v () = data.S.matrix () * q_dot;
data.v () = data.S.S_minimal * q_dot;
data.c ()(0) = -c1 * q_dot (0) * q_dot (1);
data.c ()(1) = -s1 * s2 * q_dot (0) * q_dot (1) + c1 * c2 * q_dot (0) * q_dot (2) - s2 * q_dot (1) * q_dot (2);
......@@ -464,8 +495,8 @@ namespace se3
void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
{
data.U = I.middleCols<3> (Inertia::ANGULAR) * data.S.matrix();
Inertia::Matrix3 tmp (data.S.matrix().transpose() * data.U.middleRows<3> (Inertia::ANGULAR));
data.U = I.middleCols<3> (Inertia::ANGULAR) * data.S.S_minimal;
Inertia::Matrix3 tmp (data.S.S_minimal.transpose() * data.U.middleRows<3> (Inertia::ANGULAR));
data.Dinv = tmp.inverse();
data.UDinv.noalias() = data.U * data.Dinv;
......
......@@ -114,9 +114,11 @@ namespace se3
typedef Eigen::Matrix<Scalar,3,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,3,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,3> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // struct traits struct ConstraintRotationalSubspace
struct ConstraintRotationalSubspace
struct ConstraintRotationalSubspace : public ConstraintBase< ConstraintRotationalSubspace >
{
SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintRotationalSubspace);
enum { NV = 3, Options = 0 };
......@@ -150,12 +152,12 @@ namespace se3
TransposeConst transpose() const { return TransposeConst(); }
operator ConstraintXd () const
DenseBase matrix_impl() const
{
Eigen::Matrix<double,6,3> S;
DenseBase S;
S.block <3,3> (Inertia::LINEAR, 0).setZero ();
S.block <3,3> (Inertia::ANGULAR, 0).setIdentity ();
return ConstraintXd(S);
return S;
}
Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
......
......@@ -119,6 +119,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,3,1,0> JointMotion;
typedef Eigen::Matrix<Scalar,3,1,0> JointForce;
typedef Eigen::Matrix<Scalar,6,3> DenseBase;
typedef DenseBase MatrixReturnType;
typedef const DenseBase ConstMatrixReturnType;
}; // traits ConstraintTranslationSubspace
struct ConstraintTranslationSubspace : ConstraintBase < ConstraintTranslationSubspace >
......@@ -160,12 +162,12 @@ namespace se3
ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
operator ConstraintXd () const
DenseBase matrix_impl() const
{
Eigen::Matrix<double,6,3> S;
DenseBase S;
S.block <3,3> (Inertia::LINEAR, 0).setIdentity ();
S.block <3,3> (Inertia::ANGULAR, 0).setZero ();
return ConstraintXd(S);
return S;
}
Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
......
......@@ -76,7 +76,7 @@ BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3).matrix();
BOOST_CHECK(StF2.isApprox(ConstraintXd(S).matrix().transpose()*F2.matrix().block(0,5,6,3)
BOOST_CHECK(StF2.isApprox(S.matrix().transpose()*F2.matrix().block(0,5,6,3)
, 1e-12));
}
......
......@@ -92,7 +92,7 @@ struct FiniteDiffJoint
TV v(jmodel.nv()); v.setZero();
double eps = 1e-4;
Eigen::Matrix<double,6,JointModel::NV> S(6,jmodel.nv()), S_ref(ConstraintXd(jdata.S).matrix());
Eigen::Matrix<double,6,JointModel::NV> S(6,jmodel.nv()), S_ref(jdata.S.matrix());
eps = jmodel.finiteDifferenceIncrement();
for(int k=0;k<jmodel.nv();++k)
......@@ -109,6 +109,9 @@ struct FiniteDiffJoint
}
BOOST_CHECK(S.isApprox(S_ref,eps*1e1));
std::cout << "name: " << jmodel.classname() << std::endl;
std::cout << "S_ref:\n" << S_ref << std::endl;
std::cout << "S:\n" << S << std::endl;
}
};
......
......@@ -67,7 +67,7 @@ void test_joint_methods (JointModel & jmodel, typename JointModel::JointDataDeri
BOOST_CHECK_MESSAGE(fabs(jmodel.distance(q1,q2) - jma.distance(q1,q2)) < 1e-12 ,std::string(error_prefix + " - distance "));
BOOST_CHECK_MESSAGE((jda.S().matrix()).isApprox((((se3::ConstraintXd)jdata.S).matrix())),std::string(error_prefix + " - ConstraintXd "));
BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.S.matrix()),std::string(error_prefix + " - ConstraintXd "));
BOOST_CHECK_MESSAGE( (jda.M()).isApprox((jdata.M)),std::string(error_prefix + " - Joint transforms ")); // == or isApprox ?
BOOST_CHECK_MESSAGE( (jda.v()).isApprox( (se3::Motion(jdata.v))),std::string(error_prefix + " - Joint motions "));
BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c),std::string(error_prefix + " - Joint bias "));
......@@ -82,7 +82,7 @@ void test_joint_methods (JointModel & jmodel, typename JointModel::JointDataDeri
Motion v(Motion::Random());
ConstraintDense vxS(v.cross(jdata.S));
ConstraintDense vxS_ref = v.toActionMatrix() * ConstraintXd(jdata.S).matrix();
ConstraintDense vxS_ref = v.toActionMatrix() * jdata.S.matrix();
BOOST_CHECK_MESSAGE(vxS.isApprox(vxS_ref),std::string(error_prefix + "- Joint vxS operation "));
......
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