From adcece792768751df3cb443eba8fe4e12bea5b13 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 27 Sep 2017 14:14:57 -0700 Subject: [PATCH 1/3] 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() From 113e103bc234d25fdee11b0a8fe88cc1a8f755bf Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 27 Sep 2017 15:18:08 -0700 Subject: [PATCH 2/3] Modify the joint damping input check, and output error message when it doesn't pass the size check. Modify the kuka grasping example accordingly. Setting joint damping in this example was a no-op before. --- .../pybullet/gym/pybullet_envs/bullet/kuka.py | 2 +- examples/pybullet/pybullet.c | 30 ++++++++++++++----- 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kuka.py b/examples/pybullet/gym/pybullet_envs/bullet/kuka.py index 5a35bf9a7..3e7ee6c49 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kuka.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kuka.py @@ -34,7 +34,7 @@ class Kuka: #restposes for null space self.rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] #joint damping coefficents - self.jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1] + self.jd=[0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001] self.reset() def reset(self): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c1f7e2eb2..40e234421 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6735,16 +6735,32 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, hasNullSpace = 1; } - if (numJoints && (szJointDamping == numJoints)) + if (szJointDamping > 0) { - int szInBytes = sizeof(double) * numJoints; - int i; - jointDamping = (double*)malloc(szInBytes); - for (i = 0; i < numJoints; i++) + // We allow the number of joint damping values to be larger than + // the number of degrees of freedom (DOFs). On the server side, it does + // the check and only sets joint damping for all DOFs. + // We can use the number of DOFs here when that is exposed. Since the + // number of joints is larger than the number of DOFs (we assume the + // joint is either fixed joint or one DOF joint), it is safe to use + // number of joints here. + if (szJointDamping < numJoints) { - jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); + PyErr_SetString(SpamError, + "calculateInverseKinematics the size of input joint damping values is smaller than the number of joints."); + return NULL; + } + else + { + int szInBytes = sizeof(double) * szJointDamping; + int i; + jointDamping = (double*)malloc(szInBytes); + for (i = 0; i < szJointDamping; i++) + { + jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); + } + hasJointDamping = 1; } - hasJointDamping = 1; } if (hasPos) From 54eada7579ba1b485ada22375780269e977ee46d Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 27 Sep 2017 16:42:29 -0700 Subject: [PATCH 3/3] Fix memory leak. --- examples/pybullet/pybullet.c | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c52ff4630..a6a51d23b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6945,6 +6945,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, { b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); } + free(jointDamping); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);