From 38d1d6bc2875295e391de01a53c0ed387dbe9687 Mon Sep 17 00:00:00 2001 From: Church <1155036282@link.cuhk.edu.hk> Date: Thu, 30 Aug 2018 10:58:05 +0800 Subject: [PATCH] fix an inverse kinematics bug transform translation error to spatial frame --- cisstRobot/code/robManipulator.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/cisstRobot/code/robManipulator.cpp b/cisstRobot/code/robManipulator.cpp index 09dcb257d..b045db96a 100644 --- a/cisstRobot/code/robManipulator.cpp +++ b/cisstRobot/code/robManipulator.cpp @@ -451,11 +451,6 @@ robManipulator::InverseKinematics( vctDynamicVector& q, // Evaluate the spatial Jacobian (also evaluate the forward kin) JacobianSpatial( q ); - // compute the translation error - vctFixedSizeVector dt( Rts[0][3]-Rt[0][3], - Rts[1][3]-Rt[1][3], - Rts[2][3]-Rt[2][3] ); - // compute the orientation error // first build the [ n o a ] vectors vctFixedSizeVector n1( Rt[0][0], Rt[1][0], Rt[2][0] ); @@ -468,6 +463,11 @@ robManipulator::InverseKinematics( vctDynamicVector& q, // This is the orientation error vctFixedSizeVector dr = 0.5*( (n1%n2) + (o1%o2) + (a1%a2) ); + // compute the translation error + vctFixedSizeVector dt( Rts[0][3]-Rt[0][3]-Rt[2][3]*dr[1]+Rt[1][3]*dr[2], + Rts[1][3]-Rt[1][3]+Rt[2][3]*dr[0]-Rt[0][3]*dr[2], + Rts[2][3]-Rt[2][3]-Rt[1][3]*dr[0]+Rt[0][3]*dr[1]); + // combine both errors in one R^6 vector doublereal e[6] = { dt[0], dt[1], dt[2], dr[0], dr[1], dr[2] };