Commit bf3e70a7 authored by Valenza Florian's avatar Valenza Florian
Browse files

Switch from MatrixXd to Data::Matrix6x for holding jacobians

parent 99ad179d
...@@ -53,7 +53,7 @@ namespace se3 ...@@ -53,7 +53,7 @@ namespace se3
void getJacobian(const Model & model, void getJacobian(const Model & model,
const Data & data, const Data & data,
Model::Index jointId, Model::Index jointId,
Eigen::MatrixXd & J); Data::Matrix6x & J);
/// ///
/// \brief Computes the jacobian of a specific joint frame expressed in the local frame of the joint. The result is stored in data.J. /// \brief Computes the jacobian of a specific joint frame expressed in the local frame of the joint. The result is stored in data.J.
......
...@@ -77,7 +77,7 @@ namespace se3 ...@@ -77,7 +77,7 @@ namespace se3
void getJacobian(const Model & model, void getJacobian(const Model & model,
const Data & data, const Data & data,
Model::Index jointId, Model::Index jointId,
Eigen::MatrixXd & J) Data::Matrix6x & J)
{ {
assert( J.rows() == data.J.rows() ); assert( J.rows() == data.J.rows() );
assert( J.cols() == data.J.cols() ); assert( J.cols() == data.J.cols() );
......
...@@ -109,7 +109,7 @@ inline void framesForwardKinematic(const Model & model, ...@@ -109,7 +109,7 @@ inline void framesForwardKinematic(const Model & model,
template<bool localFrame> template<bool localFrame>
inline void getFrameJacobian(const Model & model, const Data& data, inline void getFrameJacobian(const Model & model, const Data& data,
Model::Index frame_id, Eigen::MatrixXd & J) Model::Index frame_id, Data::Matrix6x & J)
{ {
assert( J.rows() == data.J.rows() ); assert( J.rows() == data.J.rows() );
assert( J.cols() == data.J.cols() ); assert( J.cols() == data.J.cols() );
......
...@@ -92,14 +92,14 @@ namespace se3 ...@@ -92,14 +92,14 @@ namespace se3
const VectorXd_fx & q ) const VectorXd_fx & q )
{ return jacobianCenterOfMass(*model,*data,q); } { return jacobianCenterOfMass(*model,*data,q); }
static Eigen::MatrixXd jacobian_proxy( const ModelHandler& model, static Data::Matrix6x jacobian_proxy( const ModelHandler& model,
DataHandler & data, DataHandler & data,
const VectorXd_fx & q, const VectorXd_fx & q,
Model::Index jointId, Model::Index jointId,
bool local, bool local,
bool update_geometry ) bool update_geometry )
{ {
Eigen::MatrixXd J( 6,model->nv ); J.setZero(); Data::Matrix6x J( 6,model->nv ); J.setZero();
if (update_geometry) if (update_geometry)
computeJacobians( *model,*data,q ); computeJacobians( *model,*data,q );
if(local) getJacobian<true> (*model, *data, jointId, J); if(local) getJacobian<true> (*model, *data, jointId, J);
...@@ -115,7 +115,7 @@ namespace se3 ...@@ -115,7 +115,7 @@ namespace se3
bool update_geometry bool update_geometry
) )
{ {
Eigen::MatrixXd J( 6,model->nv ); J.setZero(); Data::Matrix6x J( 6,model->nv ); J.setZero();
if (update_geometry) if (update_geometry)
computeJacobians( *model,*data,q ); computeJacobians( *model,*data,q );
......
...@@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
computeJacobians(model,data,q); computeJacobians(model,data,q);
Model::Index idx = model.existBodyName("rarm2")?model.getBodyId("rarm2"):(Model::Index)(model.nbody-1); Model::Index idx = model.existBodyName("rarm2")?model.getBodyId("rarm2"):(Model::Index)(model.nbody-1);
MatrixXd Jrh(6,model.nv); Jrh.fill(0); Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh); getJacobian<false>(model,data,idx,Jrh);
/* Test J*q == v */ /* Test J*q == v */
...@@ -56,9 +56,9 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -56,9 +56,9 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
/* Test local jacobian: rhJrh == rhXo oJrh */ /* Test local jacobian: rhJrh == rhXo oJrh */
MatrixXd rhJrh(6,model.nv); rhJrh.fill(0); Data::Matrix6x rhJrh(6,model.nv); rhJrh.fill(0);
getJacobian<true>(model,data,idx,rhJrh); getJacobian<true>(model,data,idx,rhJrh);
MatrixXd XJrh(6,model.nv); Data::Matrix6x XJrh(6,model.nv);
motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh ); motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12)); BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
...@@ -111,7 +111,7 @@ BOOST_AUTO_TEST_CASE ( test_timings ) ...@@ -111,7 +111,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
{ {
computeJacobians(model,data,q); computeJacobians(model,data,q);
Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1); Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0); Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic(); timer.tic();
SMOOTH(NBT) SMOOTH(NBT)
...@@ -126,7 +126,7 @@ BOOST_AUTO_TEST_CASE ( test_timings ) ...@@ -126,7 +126,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
{ {
computeJacobians(model,data,q); computeJacobians(model,data,q);
Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1); Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0); Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic(); timer.tic();
SMOOTH(NBT) SMOOTH(NBT)
...@@ -141,7 +141,7 @@ BOOST_AUTO_TEST_CASE ( test_timings ) ...@@ -141,7 +141,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
{ {
computeJacobians(model,data,q); computeJacobians(model,data,q);
Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1); Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0); Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic(); timer.tic();
SMOOTH(NBT) SMOOTH(NBT)
......
...@@ -79,8 +79,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -79,8 +79,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
/// In global frame /// In global frame
MatrixXd Joj(6,model.nv); Joj.fill(0); Data::Matrix6x Joj(6,model.nv); Joj.fill(0);
MatrixXd Jof(6,model.nv); Jof.fill(0); Data::Matrix6x Jof(6,model.nv); Jof.fill(0);
Model::Index idx = model.getFrameId(frame_name); Model::Index idx = model.getFrameId(frame_name);
getFrameJacobian<false>(model,data,idx,Jof); getFrameJacobian<false>(model,data,idx,Jof);
...@@ -99,8 +99,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -99,8 +99,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
/// In local frame /// In local frame
MatrixXd Jjj(6,model.nv); Jjj.fill(0); Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
MatrixXd Jff(6,model.nv); Jff.fill(0); Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian<true>(model,data,idx,Jff); getFrameJacobian<true>(model,data,idx,Jff);
getJacobian<true>(model, data, parent_idx, Jjj); getJacobian<true>(model, data, parent_idx, Jjj);
......
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