From 113e103bc234d25fdee11b0a8fe88cc1a8f755bf Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 27 Sep 2017 15:18:08 -0700 Subject: [PATCH] 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)