Commit b40e7d15 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Add SE3 method to generate the dual action matrix

parent 6a9490b5
......@@ -69,8 +69,9 @@ namespace se3
"The translation part of the transformation."
)
.add_property("homogeneous",&SE3::toHomogeneousMatrix,"Returns the homegeneous matrix of *this.")
.add_property("action",&SE3::toActionMatrix,"Returns the action matrix of *this.")
.add_property("homogeneous",&SE3::toHomogeneousMatrix,"Returns the homegeneous matrix of *this (acting on SE3).")
.add_property("action",&SE3::toActionMatrix,"Returns the action matrix of *this (acting on Motion).")
.add_property("dualAction",&SE3::toDualActionMatrix,"Returns the dual action matrix of *this (acting on Force).")
.def("setIdentity",&SE3PythonVisitor::setIdentity,"Set *this to the identity placement.")
.def("setRandom",&SE3PythonVisitor::setRandom,"Set *this to a random placement.")
......
......@@ -77,6 +77,8 @@ namespace se3
return derived().toActionMatrix_impl();
}
operator Matrix6() const { return toActionMatrix(); }
Matrix6 toDualActionMatrix() const { return derived().toDualActionMatrix_impl(); }
......@@ -254,6 +256,21 @@ namespace se3
B.col(2) = trans.cross(rot.col(2));
return M;
}
Matrix6 toDualActionMatrix_impl() const
{
typedef Eigen::Block<Matrix6,3,3> Block3;
Matrix6 M;
M.template block<3,3>(ANGULAR,ANGULAR)
= M.template block<3,3>(LINEAR,LINEAR) = rot;
M.template block<3,3>(LINEAR,ANGULAR).setZero();
Block3 B = M.template block<3,3>(ANGULAR,LINEAR);
B.col(0) = trans.cross(rot.col(0));
B.col(1) = trans.cross(rot.col(1));
B.col(2) = trans.cross(rot.col(2));
return M;
}
void disp_impl(std::ostream & os) const
{
......
......@@ -65,10 +65,13 @@ BOOST_AUTO_TEST_CASE ( test_SE3 )
Matrix6 bXc = bmc;
Matrix6 aXc = amc;
BOOST_CHECK(aXc.isApprox(aXb*bXc, 1e-12));
Matrix6 bXa = amb.inverse();
BOOST_CHECK(bXa.isApprox(aXb.inverse(), 1e-12));
// Test dual action matrix
BOOST_CHECK(aXb.inverse().transpose().isApprox(amb.toDualActionMatrix(),1e-12));
// Test isIdentity
SE3 identity = SE3::Identity();
BOOST_CHECK(identity.isIdentity());
......
Markdown is supported
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