From adcece792768751df3cb443eba8fe4e12bea5b13 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 27 Sep 2017 14:14:57 -0700 Subject: [PATCH] Adjust the IK setup to address the inverse kinematics issues mentioned in #1249. --- examples/SharedMemory/IKTrajectoryHelper.cpp | 4 +++- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 6 ++++++ examples/pybullet/examples/inverse_kinematics.py | 7 ++++--- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 0a6cbbf80..0145251bb 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -185,7 +185,9 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, { m_data->m_nullSpaceVelocity.SetLength(numQ); m_data->m_nullSpaceVelocity.SetZero(); - double stayCloseToZeroGain = 0.1; + // TODO: Expose the coefficents of the null space term so that the user can choose to balance the null space task and the IK target task. + // Can also adaptively adjust the coefficients based on the residual of the null space velocity in the IK target task space. + double stayCloseToZeroGain = 0.001; double stayAwayFromLimitsGain = 10.0; // Stay close to zero diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 301910a9c..c082bef31 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -354,6 +354,12 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV) // Compute null space velocity VectorRn nullV(J.GetNumColumns()); P.Multiply(desiredV, nullV); + + // Compute residual + VectorRn residual(J.GetNumRows()); + J.Multiply(nullV, residual); + // TODO: Use residual to set the null space term coefficient adaptively. + //printf("residual: %f\n", residual.Norm()); // Add null space velocity dTheta += nullV; diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py index 0dba11bf9..9aa32afed 100644 --- a/examples/pybullet/examples/inverse_kinematics.py +++ b/examples/pybullet/examples/inverse_kinematics.py @@ -37,20 +37,21 @@ hasPrevPose = 0 useNullSpace = 0 useOrientation = 1 -useSimulation = 1 +#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control. +#This can be used to test the IK result accuracy. +useSimulation = 0 useRealTimeSimulation = 1 p.setRealTimeSimulation(useRealTimeSimulation) #trailDuration is duration (in seconds) after debug lines will be removed automatically #use 0 for no-removal trailDuration = 15 - while 1: if (useRealTimeSimulation): dt = datetime.now() t = (dt.second/60.)*2.*math.pi else: - t=t+0.1 + t=t+0.001 if (useSimulation and useRealTimeSimulation==0): p.stepSimulation()