Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
bindings/python/algorithm/expose-jacobian.cpp
View file @ 63775463
......@@ -36,8 +36,8 @@ namespace se3
if (update_kinematics)
computeJacobians(model,data,q);
if(local) getJacobian<true> (model,data,jointId,J);
else getJacobian<false> (model,data,jointId,J);
if(local) getJacobian<LOCAL> (model,data,jointId,J);
else getJacobian<WORLD> (model,data,jointId,J);
return J;
}
......@@ -51,8 +51,8 @@ namespace se3
Data::Matrix6x dJ(6,model.nv); dJ.setZero();
if(local) getJacobianTimeVariation<true> (model,data,jointId,dJ);
else getJacobianTimeVariation<false> (model,data,jointId,dJ);
if(local) getJacobianTimeVariation<LOCAL> (model,data,jointId,dJ);
else getJacobianTimeVariation<WORLD> (model,data,jointId,dJ);
return dJ;
}
......
src/algorithm/jacobian.hpp
View file @ 63775463
......@@ -18,6 +18,7 @@
#ifndef __se3_jacobian_hpp__
#define __se3_jacobian_hpp__
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/model.hpp"
namespace se3
......@@ -40,16 +41,18 @@ namespace se3
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.
///
/// \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] model The model 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[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,
const Data & data,
const Model::JointIndex jointId,
......@@ -118,20 +121,22 @@ namespace se3
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.
///
/// \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] model The model 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[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>
void getJacobianTimeVariation(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & dJ);
template<ReferenceFrame rf>
void getJacobian(const Model & model,
const Data & data,
const Model::JointIndex jointId,
Data::Matrix6x & dJ);
} // namespace se3
......
src/algorithm/jacobian.hxx
View file @ 63775463
......@@ -74,7 +74,7 @@ namespace se3
/* 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
function computeJacobians should have been called first. */
template<bool localFrame>
template<ReferenceFrame rf>
void getJacobian(const Model & model,
const Data & data,
const Model::JointIndex jointId,
......@@ -88,8 +88,8 @@ namespace se3
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])
{
if(! localFrame ) J.col(j) = data.J.col(j);
else J.col(j) = oMjoint.actInv(Motion(data.J.col(j))).toVector();
if(rf == WORLD) J.col(j) = data.J.col(j);
else J.col(j) = oMjoint.actInv(Motion(data.J.col(j))).toVector();
}
}
......@@ -211,7 +211,7 @@ namespace se3
return data.dJ;
}
template<bool localFrame>
template<ReferenceFrame rf>
void getJacobianTimeVariation(const Model & model,
const Data & data,
const Model::JointIndex jointId,
......@@ -225,8 +225,8 @@ namespace se3
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])
{
if(! localFrame ) dJ.col(j) = data.dJ.col(j);
else dJ.col(j) = oMjoint.actInv(Motion(data.dJ.col(j))).toVector();
if(rf == WORLD) dJ.col(j) = data.dJ.col(j);
else dJ.col(j) = oMjoint.actInv(Motion(data.dJ.col(j))).toVector();
}
}
......
unittest/aba.cpp
View file @ 63775463
......@@ -87,7 +87,7 @@ BOOST_AUTO_TEST_CASE ( test_aba_with_fext )
VectorXd tau = data.M * a + data.nle;
Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv);
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();
J.setZero();
}
......
unittest/dynamics.cpp
View file @ 63775463
......@@ -51,10 +51,10 @@ BOOST_AUTO_TEST_CASE ( test_FD )
Data::Matrix6x J_RF (6, model.nv);
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);
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);
J.setZero();
......@@ -113,10 +113,10 @@ BOOST_AUTO_TEST_CASE ( test_ID )
Data::Matrix6x J_RF (6, model.nv);
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);
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);
J.setZero();
......@@ -184,9 +184,9 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt)
const std::string LF = "lleg6_joint";
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);
getJacobian <true> (model, data, model.getJointId(LF), J_LF);
getJacobian<LOCAL> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv);
J.topRows<6> () = J_RF;
......
unittest/finite-differences.cpp
View file @ 63775463
......@@ -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);
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);
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);
BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,fd_increment.maxCoeff()*1e1));
}
......
unittest/frames.cpp
View file @ 63775463
......@@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
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_joint = Motion(Jjj*q_dot);
......
unittest/jacobian.cpp
View file @ 63775463
......@@ -51,7 +51,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
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 */
VectorXd qdot = VectorXd::Random(model.nv);
......@@ -63,7 +63,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
/* Test local jacobian: rhJrh == rhXo oJrh */
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);
motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
......@@ -98,8 +98,8 @@ BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
// Regarding to the world origin
getJacobian<false>(model,data,idx,J);
getJacobianTimeVariation<false>(model,data,idx,dJ);
getJacobian<WORLD>(model,data,idx,J);
getJacobianTimeVariation<WORLD>(model,data,idx,dJ);
Motion v_idx(J*v);
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 )
// Regarding to the local frame
getJacobian<true>(model,data,idx,J);
getJacobianTimeVariation<true>(model,data,idx,dJ);
getJacobian<LOCAL>(model,data,idx,J);
getJacobianTimeVariation<LOCAL>(model,data,idx,dJ);
v_idx = (Motion::Vector6)(J*v);
BOOST_CHECK(v_idx.isApprox(data_ref.v[idx]));
......@@ -168,7 +168,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer.tic();
SMOOTH(NBT)
{
getJacobian<false>(model,data,idx,Jrh);
getJacobian<WORLD>(model,data,idx,Jrh);
}
if(verbose) std::cout << "Copy =\t";
timer.toc(std::cout,NBT);
......@@ -183,7 +183,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer.tic();
SMOOTH(NBT)
{
getJacobian<true>(model,data,idx,Jrh);
getJacobian<LOCAL>(model,data,idx,Jrh);
}
if(verbose) std::cout << "Change frame =\t";
timer.toc(std::cout,NBT);
......
unittest/joints.cpp
View file @ 63775463
......@@ -145,8 +145,8 @@ BOOST_AUTO_TEST_CASE (vsRX)
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRevoluteUnaligned;jacobianRevoluteUnaligned.resize(6,1);jacobianRevoluteUnaligned.setZero();
computeJacobians(modelRX, dataRX, q);
computeJacobians(modelRevoluteUnaligned, dataRevoluteUnaligned, q);
getJacobian<true>(modelRX, dataRX, 1, jacobianRX);
getJacobian<true>(modelRevoluteUnaligned, dataRevoluteUnaligned, 1, jacobianRevoluteUnaligned);
getJacobian<LOCAL>(modelRX, dataRX, 1, jacobianRX);
getJacobian<LOCAL>(modelRevoluteUnaligned, dataRevoluteUnaligned, 1, jacobianRevoluteUnaligned);
BOOST_CHECK(jacobianRX.isApprox(jacobianRevoluteUnaligned));
......@@ -221,8 +221,8 @@ BOOST_AUTO_TEST_CASE (vsPX)
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
computeJacobians(modelPX, dataPX, q);
computeJacobians(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
getJacobian<true>(modelPX, dataPX, 1, jacobianPX);
getJacobian<true>(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, jacobianPrismaticUnaligned);
getJacobian<LOCAL>(modelPX, dataPX, 1, jacobianPX);
getJacobian<LOCAL>(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, jacobianPrismaticUnaligned);
BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
}
......@@ -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();
computeJacobians(modelSpherical, dataSpherical, q);
computeJacobians(modelFreeflyer, dataFreeFlyer, qff);
getJacobian<true>(modelSpherical, dataSpherical, 1, jacobian_planar);
getJacobian<true>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
getJacobian<LOCAL>(modelSpherical, dataSpherical, 1, jacobian_planar);
getJacobian<LOCAL>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(3),
......@@ -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();
computeJacobians(modelPlanar, dataPlanar, q);
computeJacobians(modelFreeflyer, dataFreeFlyer, qff);
getJacobian<true>(modelPlanar, dataPlanar, 1, jacobian_planar);
getJacobian<true>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
getJacobian<LOCAL>(modelPlanar, dataPlanar, 1, jacobian_planar);
getJacobian<LOCAL>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
jacobian_ff.col(1),
......@@ -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();
computeJacobians(modelTranslation, dataTranslation, q);
computeJacobians(modelFreeflyer, dataFreeFlyer, qff);
getJacobian<true>(modelTranslation, dataTranslation, 1, jacobian_planar);
getJacobian<true>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
getJacobian<LOCAL>(modelTranslation, dataTranslation, 1, jacobian_planar);
getJacobian<LOCAL>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
......@@ -845,8 +845,8 @@ BOOST_AUTO_TEST_CASE (vsRX)
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
computeJacobians(modelRX, dataRX, q_rx);
computeJacobians(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);
getJacobian<true>(modelRX, dataRX, 1, jacobianPX);
getJacobian<true>(modelRevoluteUnbounded, dataRevoluteUnbounded, 1, jacobianPrismaticUnaligned);
getJacobian<LOCAL>(modelRX, dataRX, 1, jacobianPX);
getJacobian<LOCAL>(modelRevoluteUnbounded, dataRevoluteUnbounded, 1, 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