Commit 63775463 authored by jcarpent's avatar jcarpent Committed by Justin Carpentier
Browse files

[All] Use the notion of LOCAL and WORLD frame

It replaces the old way to access this frame through a boolean
templated parameter
parent 3ead5dca
...@@ -36,8 +36,8 @@ namespace se3 ...@@ -36,8 +36,8 @@ namespace se3
if (update_kinematics) if (update_kinematics)
computeJacobians(model,data,q); computeJacobians(model,data,q);
if(local) getJacobian<true> (model,data,jointId,J); if(local) getJacobian<LOCAL> (model,data,jointId,J);
else getJacobian<false> (model,data,jointId,J); else getJacobian<WORLD> (model,data,jointId,J);
return J; return J;
} }
...@@ -51,8 +51,8 @@ namespace se3 ...@@ -51,8 +51,8 @@ namespace se3
Data::Matrix6x dJ(6,model.nv); dJ.setZero(); Data::Matrix6x dJ(6,model.nv); dJ.setZero();
if(local) getJacobianTimeVariation<true> (model,data,jointId,dJ); if(local) getJacobianTimeVariation<LOCAL> (model,data,jointId,dJ);
else getJacobianTimeVariation<false> (model,data,jointId,dJ); else getJacobianTimeVariation<WORLD> (model,data,jointId,dJ);
return dJ; return dJ;
} }
......
...@@ -18,6 +18,7 @@ ...@@ -18,6 +18,7 @@
#ifndef __se3_jacobian_hpp__ #ifndef __se3_jacobian_hpp__
#define __se3_jacobian_hpp__ #define __se3_jacobian_hpp__
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/model.hpp"
namespace se3 namespace se3
...@@ -40,16 +41,18 @@ namespace se3 ...@@ -40,16 +41,18 @@ namespace se3
const Eigen::VectorXd & q); const Eigen::VectorXd & q);
/// ///
/// \brief Computes the Jacobian of a specific joint frame expressed either in the world frame or in the local frame of the joint. /// \brief Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or in the local frame (rf = LOCAL) of the joint.
/// \note This jacobian is extracted from data.J. You have to run se3::computeJacobians before calling it. /// \note This jacobian is extracted from data.J. You have to run se3::computeJacobians before calling it.
/// ///
/// \tparam rf Reference frame in which the Jacobian is expressed.
///
/// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system. /// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system.
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// \param[in] jointId The id of the joint. /// \param[in] jointId The id of the joint.
/// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.fill(0.). /// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).
/// ///
template<bool localFrame> template<ReferenceFrame rf>
void getJacobian(const Model & model, void getJacobian(const Model & model,
const Data & data, const Data & data,
const Model::JointIndex jointId, const Model::JointIndex jointId,
...@@ -118,20 +121,22 @@ namespace se3 ...@@ -118,20 +121,22 @@ namespace se3
const Eigen::VectorXd & v); const Eigen::VectorXd & v);
/// ///
/// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame or in the local frame of the joint. /// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (rf = WORLD) or in the local frame (rf = LOCAL) of the joint.
/// \note This jacobian is extracted from data.dJ. You have to run se3::computeJacobiansTimeVariation before calling it. /// \note This jacobian is extracted from data.dJ. You have to run se3::computeJacobiansTimeVariation before calling it.
/// ///
/// \tparam rf Reference frame in which the Jacobian is expressed.
///
/// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system. /// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system.
/// \param[in] model The model structure of the rigid body system. /// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system. /// \param[in] data The data structure of the rigid body system.
/// \param[in] jointId The id of the joint. /// \param[in] jointId The id of the joint.
/// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.). /// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.).
/// ///
template<bool localFrame> template<ReferenceFrame rf>
void getJacobianTimeVariation(const Model & model, void getJacobian(const Model & model,
const Data & data, const Data & data,
const Model::JointIndex jointId, const Model::JointIndex jointId,
Data::Matrix6x & dJ); Data::Matrix6x & dJ);
} // namespace se3 } // namespace se3
......
...@@ -74,7 +74,7 @@ namespace se3 ...@@ -74,7 +74,7 @@ namespace se3
/* Return the jacobian of the output frame attached to joint <jointId> in the /* Return the jacobian of the output frame attached to joint <jointId> in the
world frame or in the local frame depending on the template argument. The world frame or in the local frame depending on the template argument. The
function computeJacobians should have been called first. */ function computeJacobians should have been called first. */
template<bool localFrame> template<ReferenceFrame rf>
void getJacobian(const Model & model, void getJacobian(const Model & model,
const Data & data, const Data & data,
const Model::JointIndex jointId, const Model::JointIndex jointId,
...@@ -88,8 +88,8 @@ namespace se3 ...@@ -88,8 +88,8 @@ namespace se3
int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1; int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j]) for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
{ {
if(! localFrame ) J.col(j) = data.J.col(j); if(rf == WORLD) J.col(j) = data.J.col(j);
else J.col(j) = oMjoint.actInv(Motion(data.J.col(j))).toVector(); else J.col(j) = oMjoint.actInv(Motion(data.J.col(j))).toVector();
} }
} }
...@@ -211,7 +211,7 @@ namespace se3 ...@@ -211,7 +211,7 @@ namespace se3
return data.dJ; return data.dJ;
} }
template<bool localFrame> template<ReferenceFrame rf>
void getJacobianTimeVariation(const Model & model, void getJacobianTimeVariation(const Model & model,
const Data & data, const Data & data,
const Model::JointIndex jointId, const Model::JointIndex jointId,
...@@ -225,8 +225,8 @@ namespace se3 ...@@ -225,8 +225,8 @@ namespace se3
int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1; int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j]) for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
{ {
if(! localFrame ) dJ.col(j) = data.dJ.col(j); if(rf == WORLD) dJ.col(j) = data.dJ.col(j);
else dJ.col(j) = oMjoint.actInv(Motion(data.dJ.col(j))).toVector(); else dJ.col(j) = oMjoint.actInv(Motion(data.dJ.col(j))).toVector();
} }
} }
......
...@@ -87,7 +87,7 @@ BOOST_AUTO_TEST_CASE ( test_aba_with_fext ) ...@@ -87,7 +87,7 @@ BOOST_AUTO_TEST_CASE ( test_aba_with_fext )
VectorXd tau = data.M * a + data.nle; VectorXd tau = data.M * a + data.nle;
Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv); Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv);
for(Model::Index i=1;i<(Model::Index)model.njoints;++i) { for(Model::Index i=1;i<(Model::Index)model.njoints;++i) {
getJacobian<true>(model, data, i, J); getJacobian<LOCAL>(model, data, i, J);
tau -= J.transpose()*fext[i].toVector(); tau -= J.transpose()*fext[i].toVector();
J.setZero(); J.setZero();
} }
......
...@@ -51,10 +51,10 @@ BOOST_AUTO_TEST_CASE ( test_FD ) ...@@ -51,10 +51,10 @@ BOOST_AUTO_TEST_CASE ( test_FD )
Data::Matrix6x J_RF (6, model.nv); Data::Matrix6x J_RF (6, model.nv);
J_RF.setZero(); J_RF.setZero();
getJacobian <true> (model, data, model.getJointId(RF), J_RF); getJacobian<LOCAL> (model, data, model.getJointId(RF), J_RF);
Data::Matrix6x J_LF (6, model.nv); Data::Matrix6x J_LF (6, model.nv);
J_LF.setZero(); J_LF.setZero();
getJacobian <true> (model, data, model.getJointId(LF), J_LF); getJacobian<LOCAL> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv); Eigen::MatrixXd J (12, model.nv);
J.setZero(); J.setZero();
...@@ -113,10 +113,10 @@ BOOST_AUTO_TEST_CASE ( test_ID ) ...@@ -113,10 +113,10 @@ BOOST_AUTO_TEST_CASE ( test_ID )
Data::Matrix6x J_RF (6, model.nv); Data::Matrix6x J_RF (6, model.nv);
J_RF.setZero(); J_RF.setZero();
getJacobian <true> (model, data, model.getJointId(RF), J_RF); getJacobian<LOCAL> (model, data, model.getJointId(RF), J_RF);
Data::Matrix6x J_LF (6, model.nv); Data::Matrix6x J_LF (6, model.nv);
J_LF.setZero(); J_LF.setZero();
getJacobian <true> (model, data, model.getJointId(LF), J_LF); getJacobian<LOCAL> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv); Eigen::MatrixXd J (12, model.nv);
J.setZero(); J.setZero();
...@@ -184,9 +184,9 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt) ...@@ -184,9 +184,9 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt)
const std::string LF = "lleg6_joint"; const std::string LF = "lleg6_joint";
Data::Matrix6x J_RF (6, model.nv); Data::Matrix6x J_RF (6, model.nv);
getJacobian <true> (model, data, model.getJointId(RF), J_RF); getJacobian<LOCAL> (model, data, model.getJointId(RF), J_RF);
Data::Matrix6x J_LF (6, model.nv); Data::Matrix6x J_LF (6, model.nv);
getJacobian <true> (model, data, model.getJointId(LF), J_LF); getJacobian<LOCAL> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv); Eigen::MatrixXd J (12, model.nv);
J.topRows<6> () = J_RF; J.topRows<6> () = J_RF;
......
...@@ -212,11 +212,11 @@ BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff) ...@@ -212,11 +212,11 @@ BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff)
Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1); Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0); Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh); getJacobian<WORLD>(model,data,idx,Jrh);
Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model,data,q,idx); Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model,data,q,idx);
BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,fd_increment.maxCoeff()*1e1)); BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,fd_increment.maxCoeff()*1e1));
getJacobian<true>(model,data,idx,Jrh); getJacobian<LOCAL>(model,data,idx,Jrh);
Jrh_finite_diff = finiteDiffJacobian<true>(model,data,q,idx); Jrh_finite_diff = finiteDiffJacobian<true>(model,data,q,idx);
BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,fd_increment.maxCoeff()*1e1)); BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,fd_increment.maxCoeff()*1e1));
} }
......
...@@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0); Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0); Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian(model,data,idx,Jff); getFrameJacobian(model,data,idx,Jff);
getJacobian<true>(model, data_ref, parent_idx, Jjj); getJacobian<LOCAL>(model, data_ref, parent_idx, Jjj);
Motion nu_frame = Motion(Jff*q_dot); Motion nu_frame = Motion(Jff*q_dot);
Motion nu_joint = Motion(Jjj*q_dot); Motion nu_joint = Motion(Jjj*q_dot);
......
...@@ -51,7 +51,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -51,7 +51,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1); Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0); Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh); getJacobian<WORLD>(model,data,idx,Jrh);
/* Test J*q == v */ /* Test J*q == v */
VectorXd qdot = VectorXd::Random(model.nv); VectorXd qdot = VectorXd::Random(model.nv);
...@@ -63,7 +63,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian ) ...@@ -63,7 +63,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
/* Test local jacobian: rhJrh == rhXo oJrh */ /* Test local jacobian: rhJrh == rhXo oJrh */
Data::Matrix6x rhJrh(6,model.nv); rhJrh.fill(0); Data::Matrix6x rhJrh(6,model.nv); rhJrh.fill(0);
getJacobian<true>(model,data,idx,rhJrh); getJacobian<LOCAL>(model,data,idx,rhJrh);
Data::Matrix6x 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));
...@@ -98,8 +98,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation ) ...@@ -98,8 +98,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
Data::Matrix6x dJ(6,model.nv); dJ.fill(0.); Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
// Regarding to the world origin // Regarding to the world origin
getJacobian<false>(model,data,idx,J); getJacobian<WORLD>(model,data,idx,J);
getJacobianTimeVariation<false>(model,data,idx,dJ); getJacobianTimeVariation<WORLD>(model,data,idx,dJ);
Motion v_idx(J*v); Motion v_idx(J*v);
BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx]))); BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx])));
...@@ -110,8 +110,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation ) ...@@ -110,8 +110,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
// Regarding to the local frame // Regarding to the local frame
getJacobian<true>(model,data,idx,J); getJacobian<LOCAL>(model,data,idx,J);
getJacobianTimeVariation<true>(model,data,idx,dJ); getJacobianTimeVariation<LOCAL>(model,data,idx,dJ);
v_idx = (Motion::Vector6)(J*v); v_idx = (Motion::Vector6)(J*v);
BOOST_CHECK(v_idx.isApprox(data_ref.v[idx])); BOOST_CHECK(v_idx.isApprox(data_ref.v[idx]));
...@@ -168,7 +168,7 @@ BOOST_AUTO_TEST_CASE ( test_timings ) ...@@ -168,7 +168,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer.tic(); timer.tic();
SMOOTH(NBT) SMOOTH(NBT)
{ {
getJacobian<false>(model,data,idx,Jrh); getJacobian<WORLD>(model,data,idx,Jrh);
} }
if(verbose) std::cout << "Copy =\t"; if(verbose) std::cout << "Copy =\t";
timer.toc(std::cout,NBT); timer.toc(std::cout,NBT);
...@@ -183,7 +183,7 @@ BOOST_AUTO_TEST_CASE ( test_timings ) ...@@ -183,7 +183,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer.tic(); timer.tic();
SMOOTH(NBT) SMOOTH(NBT)
{ {
getJacobian<true>(model,data,idx,Jrh); getJacobian<LOCAL>(model,data,idx,Jrh);
} }
if(verbose) std::cout << "Change frame =\t"; if(verbose) std::cout << "Change frame =\t";
timer.toc(std::cout,NBT); timer.toc(std::cout,NBT);
......
...@@ -145,8 +145,8 @@ BOOST_AUTO_TEST_CASE (vsRX) ...@@ -145,8 +145,8 @@ BOOST_AUTO_TEST_CASE (vsRX)
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRevoluteUnaligned;jacobianRevoluteUnaligned.resize(6,1);jacobianRevoluteUnaligned.setZero(); Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRevoluteUnaligned;jacobianRevoluteUnaligned.resize(6,1);jacobianRevoluteUnaligned.setZero();
computeJacobians(modelRX, dataRX, q); computeJacobians(modelRX, dataRX, q);
computeJacobians(modelRevoluteUnaligned, dataRevoluteUnaligned, q); computeJacobians(modelRevoluteUnaligned, dataRevoluteUnaligned, q);
getJacobian<true>(modelRX, dataRX, 1, jacobianRX); getJacobian<LOCAL>(modelRX, dataRX, 1, jacobianRX);
getJacobian<true>(modelRevoluteUnaligned, dataRevoluteUnaligned, 1, jacobianRevoluteUnaligned); getJacobian<LOCAL>(modelRevoluteUnaligned, dataRevoluteUnaligned, 1, jacobianRevoluteUnaligned);
BOOST_CHECK(jacobianRX.isApprox(jacobianRevoluteUnaligned)); BOOST_CHECK(jacobianRX.isApprox(jacobianRevoluteUnaligned));
...@@ -221,8 +221,8 @@ BOOST_AUTO_TEST_CASE (vsPX) ...@@ -221,8 +221,8 @@ BOOST_AUTO_TEST_CASE (vsPX)
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero(); Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
computeJacobians(modelPX, dataPX, q); computeJacobians(modelPX, dataPX, q);
computeJacobians(modelPrismaticUnaligned, dataPrismaticUnaligned, q); computeJacobians(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
getJacobian<true>(modelPX, dataPX, 1, jacobianPX); getJacobian<LOCAL>(modelPX, dataPX, 1, jacobianPX);
getJacobian<true>(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, jacobianPrismaticUnaligned); getJacobian<LOCAL>(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, jacobianPrismaticUnaligned);
BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned)); BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
} }
...@@ -305,8 +305,8 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer) ...@@ -305,8 +305,8 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero(); Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
computeJacobians(modelSpherical, dataSpherical, q); computeJacobians(modelSpherical, dataSpherical, q);
computeJacobians(modelFreeflyer, dataFreeFlyer, qff); computeJacobians(modelFreeflyer, dataFreeFlyer, qff);
getJacobian<true>(modelSpherical, dataSpherical, 1, jacobian_planar); getJacobian<LOCAL>(modelSpherical, dataSpherical, 1, jacobian_planar);
getJacobian<true>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff); getJacobian<LOCAL>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(3), Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(3),
...@@ -674,8 +674,8 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer) ...@@ -674,8 +674,8 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero(); Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
computeJacobians(modelPlanar, dataPlanar, q); computeJacobians(modelPlanar, dataPlanar, q);
computeJacobians(modelFreeflyer, dataFreeFlyer, qff); computeJacobians(modelFreeflyer, dataFreeFlyer, qff);
getJacobian<true>(modelPlanar, dataPlanar, 1, jacobian_planar); getJacobian<LOCAL>(modelPlanar, dataPlanar, 1, jacobian_planar);
getJacobian<true>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff); getJacobian<LOCAL>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0), Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
jacobian_ff.col(1), jacobian_ff.col(1),
...@@ -759,8 +759,8 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer) ...@@ -759,8 +759,8 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero(); Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
computeJacobians(modelTranslation, dataTranslation, q); computeJacobians(modelTranslation, dataTranslation, q);
computeJacobians(modelFreeflyer, dataFreeFlyer, qff); computeJacobians(modelFreeflyer, dataFreeFlyer, qff);
getJacobian<true>(modelTranslation, dataTranslation, 1, jacobian_planar); getJacobian<LOCAL>(modelTranslation, dataTranslation, 1, jacobian_planar);
getJacobian<true>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff); getJacobian<LOCAL>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0), Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
...@@ -845,8 +845,8 @@ BOOST_AUTO_TEST_CASE (vsRX) ...@@ -845,8 +845,8 @@ BOOST_AUTO_TEST_CASE (vsRX)
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero(); Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
computeJacobians(modelRX, dataRX, q_rx); computeJacobians(modelRX, dataRX, q_rx);
computeJacobians(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx); computeJacobians(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);
getJacobian<true>(modelRX, dataRX, 1, jacobianPX); getJacobian<LOCAL>(modelRX, dataRX, 1, jacobianPX);
getJacobian<true>(modelRevoluteUnbounded, dataRevoluteUnbounded, 1, jacobianPrismaticUnaligned); getJacobian<LOCAL>(modelRevoluteUnbounded, dataRevoluteUnbounded, 1, jacobianPrismaticUnaligned);
BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned)); BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
......
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