diff --git a/src/algorithm/operational-frames.hpp b/src/algorithm/operational-frames.hpp
index 1f452642eca2a5580a760126bb037d107316352c..0f49533d36d33741aa8b8a62b592d70252740915 100644
--- a/src/algorithm/operational-frames.hpp
+++ b/src/algorithm/operational-frames.hpp
@@ -28,8 +28,6 @@
namespace se3
{
-
-
/**
* @brief Update the position of each extra frame
*
@@ -37,9 +35,9 @@ namespace se3
* @param data Data associated to model
* @warning One of the algorithms forwardKinematics should have been called first
*/
- inline void framesForwardKinematic(const Model & model,
- Data & data
- );
+ inline void framesForwardKinematics(const Model & model,
+ Data & data
+ );
/**
* @brief Compute Kinematics of full model, then the position of each operational frame
@@ -48,10 +46,10 @@ namespace se3
* @param data Data associated to model
* @param[in] q Configuration vector
*/
- inline void framesForwardKinematic(const Model & model,
- Data & data,
- const Eigen::VectorXd & q
- );
+ inline void framesForwardKinematics(const Model & model,
+ Data & data,
+ const Eigen::VectorXd & q
+ );
/**
* @brief Return the jacobian of the extra frame in the world frame or
@@ -68,72 +66,67 @@ namespace se3
*/
template<bool localFrame>
inline void getFrameJacobian(const Model & model,
- const Data& data,
- Model::FrameIndex frame_id,
- Data::Matrix6x & J
- );
+ const Data& data,
+ const Model::FrameIndex frame_id,
+ Data::Matrix6x & J
+ );
-} // namespace se3
+} // namespace se3
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
-inline void framesForwardKinematic(const Model & model,
- Data & data
- )
-{
- using namespace se3;
-
- for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nOperationalFrames; ++i)
+ inline void framesForwardKinematics(const Model & model,
+ Data & data
+ )
{
- const Model::JointIndex & parent = model.operational_frames[i].parent_id;
- data.oMof[i] = (data.oMi[parent] * model.operational_frames[i].framePlacement);
+ for (Model::FrameIndex i=0; i < (Model::FrameIndex) model.nOperationalFrames; ++i)
+ {
+ const Model::JointIndex & parent = model.operational_frames[i].parent_id;
+ data.oMof[i] = (data.oMi[parent] * model.operational_frames[i].framePlacement);
+ }
}
-}
-
-inline void framesForwardKinematic(const Model & model,
- Data & data,
- const Eigen::VectorXd & q
- )
-{
- using namespace se3;
-
- forwardKinematics(model, data, q);
-
- framesForwardKinematic(model, data);
-}
-
-
-
-template<bool localFrame>
-inline void getFrameJacobian(const Model & model, const Data& data,
- Model::FrameIndex frame_id, Data::Matrix6x & J)
-{
- assert( J.rows() == data.J.rows() );
- assert( J.cols() == data.J.cols() );
-
- const Model::JointIndex & parent = model.operational_frames[frame_id].parent_id;
- const SE3 & oMframe = data.oMof[frame_id];
- int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
-
- SE3::Vector3 lever(data.oMi[parent].rotation() * (model.operational_frames[frame_id].framePlacement.translation()));
-
- if (!localFrame) getJacobian<localFrame>(model, data, parent, J);
- for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
+ inline void framesForwardKinematics(const Model & model,
+ Data & data,
+ const Eigen::VectorXd & q
+ )
+ {
+ forwardKinematics(model, data, q);
+ framesForwardKinematics(model, data);
+ }
+
+
+
+ template<bool localFrame>
+ inline void getFrameJacobian(const Model & model,
+ const Data & data,
+ const Model::FrameIndex frame_id,
+ Data::Matrix6x & J)
+ {
+ assert( J.rows() == data.J.rows() );
+ assert( J.cols() == data.J.cols() );
+
+ const Model::JointIndex & parent = model.operational_frames[frame_id].parent_id;
+ const SE3 & oMframe = data.oMof[frame_id];
+ const Frame & frame = model.operational_frames[frame_id];
+
+ const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
+
+ // Lever between the joint center and the frame center expressed in the global frame
+ const SE3::Vector3 lever(data.oMi[parent].rotation() * (frame.framePlacement.translation()));
+
+ getJacobian<localFrame>(model, data, parent, J);
+ for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
if(! localFrame )
- {
- J.col(j).topRows<3>() -= lever.cross( J.col(j).bottomRows<3>());
- }
+ J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
else
- {
J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
- }
}
-}
+ }
} // namespace se3
diff --git a/src/python/algorithms.hpp b/src/python/algorithms.hpp
index 38b10ff7fbea9ab983957bd684c5cccea98fc51d..f02c02c19b708056c81fb092720742fcf600387c 100644
--- a/src/python/algorithms.hpp
+++ b/src/python/algorithms.hpp
@@ -150,11 +150,11 @@ namespace se3
static void frames_fk_0_proxy(const ModelHandler& model,
- DataHandler & data,
- const VectorXd_fx & q
- )
+ DataHandler & data,
+ const VectorXd_fx & q
+ )
{
- framesForwardKinematic( *model,*data,q );
+ framesForwardKinematics( *model,*data,q );
}
static void fk_2_proxy(const ModelHandler& model,
diff --git a/unittest/operational-frames.cpp b/unittest/operational-frames.cpp
index 2f4041ef0e8f89982dad4ed84e3aaab093c8e7a0..e22e149db626b0652fa260928472a5f67f600351 100644
--- a/unittest/operational-frames.cpp
+++ b/unittest/operational-frames.cpp
@@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
- framesForwardKinematic(model, data, q);
+ framesForwardKinematics(model, data, q);
BOOST_CHECK(data.oMof[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));
@@ -71,7 +71,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd q_dot = VectorXd::Ones(model.nv);
- framesForwardKinematic(model, data, q);
+ framesForwardKinematics(model, data, q);
computeJacobians(model,data,q);