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
void getJacobian(const Model & model,
const Data & data,
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.
......
......@@ -77,7 +77,7 @@ namespace se3
void getJacobian(const Model & model,
const Data & data,
Model::Index jointId,
Eigen::MatrixXd & J)
Data::Matrix6x & J)
{
assert( J.rows() == data.J.rows() );
assert( J.cols() == data.J.cols() );
......
......@@ -109,7 +109,7 @@ inline void framesForwardKinematic(const Model & model,
template<bool localFrame>
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.cols() == data.J.cols() );
......
......@@ -92,14 +92,14 @@ namespace se3
const VectorXd_fx & q )
{ return jacobianCenterOfMass(*model,*data,q); }
static Eigen::MatrixXd jacobian_proxy( const ModelHandler& model,
static Data::Matrix6x jacobian_proxy( const ModelHandler& model,
DataHandler & data,
const VectorXd_fx & q,
Model::Index jointId,
bool local,
bool update_geometry )
{
Eigen::MatrixXd J( 6,model->nv ); J.setZero();
Data::Matrix6x J( 6,model->nv ); J.setZero();
if (update_geometry)
computeJacobians( *model,*data,q );
if(local) getJacobian<true> (*model, *data, jointId, J);
......@@ -115,7 +115,7 @@ namespace se3
bool update_geometry
)
{
Eigen::MatrixXd J( 6,model->nv ); J.setZero();
Data::Matrix6x J( 6,model->nv ); J.setZero();
if (update_geometry)
computeJacobians( *model,*data,q );
......
......@@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
computeJacobians(model,data,q);
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);
/* Test J*q == v */
......@@ -56,9 +56,9 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
/* 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);
MatrixXd XJrh(6,model.nv);
Data::Matrix6x XJrh(6,model.nv);
motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
......@@ -111,7 +111,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
{
computeJacobians(model,data,q);
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();
SMOOTH(NBT)
......@@ -126,7 +126,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
{
computeJacobians(model,data,q);
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();
SMOOTH(NBT)
......@@ -141,7 +141,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
{
computeJacobians(model,data,q);
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();
SMOOTH(NBT)
......
......@@ -79,8 +79,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
/// In global frame
MatrixXd Joj(6,model.nv); Joj.fill(0);
MatrixXd Jof(6,model.nv); Jof.fill(0);
Data::Matrix6x Joj(6,model.nv); Joj.fill(0);
Data::Matrix6x Jof(6,model.nv); Jof.fill(0);
Model::Index idx = model.getFrameId(frame_name);
getFrameJacobian<false>(model,data,idx,Jof);
......@@ -99,8 +99,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
/// In local frame
MatrixXd Jjj(6,model.nv); Jjj.fill(0);
MatrixXd Jff(6,model.nv); Jff.fill(0);
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian<true>(model,data,idx,Jff);
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