From 076d0a3450b6b4b1a30c7dbe1dc0fa4832b7cee9 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Tue, 9 Feb 2016 18:26:57 +0100
Subject: [PATCH] [frame jacobian] Express jacobian in correct frame
---
src/algorithm/operational-frames.hpp | 3 ++-
unittest/operational-frames.cpp | 3 +--
2 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/algorithm/operational-frames.hpp b/src/algorithm/operational-frames.hpp
index f4d48f881..cb92fef3b 100644
--- a/src/algorithm/operational-frames.hpp
+++ b/src/algorithm/operational-frames.hpp
@@ -119,13 +119,14 @@ inline void getFrameJacobian(const Model & model, const Data& data,
int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
+ SE3::Vector3 lever(data.oMi[parent].rotation() * (model.operational_frames[frame_id].frame_placement.translation()));
if (!localFrame) getJacobian<localFrame>(model, data, parent, J);
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
{
if(! localFrame )
{
- J.col(j).topRows<3>() += model.operational_frames[frame_id].frame_placement.inverse().translation().cross( J.col(j).bottomRows<3>());
+ J.col(j).topRows<3>() -= lever.cross( J.col(j).bottomRows<3>());
}
else
{
diff --git a/unittest/operational-frames.cpp b/unittest/operational-frames.cpp
index 8fa846eae..2937b8906 100644
--- a/unittest/operational-frames.cpp
+++ b/unittest/operational-frames.cpp
@@ -85,13 +85,12 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
getFrameJacobian<false>(model,data,idx,Jof);
getJacobian<false>(model, data, parent_idx, Joj);
- // expected = frame_placement.inverse().toActionMatrix() * expected;
Motion nu_frame(Jof*q_dot);
Motion nu_joint(Joj*q_dot);
Motion nu_frame_from_nu_joint(nu_joint);
- nu_frame_from_nu_joint.linear() += frame_placement.inverse().translation().cross(nu_joint.angular());
+ nu_frame_from_nu_joint.linear() -= (data.oMi[parent_idx].rotation() *frame_placement.translation()).cross(nu_joint.angular());
BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));
--
GitLab