From 4c75e022c8ec53dcc91ae416f59e2691db34247b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 31 May 2018 21:07:04 -0700 Subject: [PATCH 1/3] Use dofCount and not numJoints in PyBullet.calculateInverseKinematics, fixes null space demo See baxter_ik_demo at https://github.com/erwincoumans/pybullet_robots --- examples/pybullet/pybullet.c | 43 ++++++++++++++---------------------- 1 file changed, 16 insertions(+), 27 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0e27316fb..d6954e0b8 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -8123,36 +8123,32 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, double* jointDamping = 0; double* currentPositions = 0; - if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && - (szJointRanges == numJoints) && (szRestPoses == numJoints)) + if (dofCount && (szLowerLimits == dofCount) && (szUpperLimits == dofCount) && + (szJointRanges == dofCount) && (szRestPoses == dofCount)) { - int szInBytes = sizeof(double) * numJoints; + int szInBytes = sizeof(double) * dofCount; int i; lowerLimits = (double*)malloc(szInBytes); upperLimits = (double*)malloc(szInBytes); jointRanges = (double*)malloc(szInBytes); restPoses = (double*)malloc(szInBytes); - for (i = 0; i < numJoints; i++) + for (i = 0; i < dofCount; i++) { - lowerLimits[i] = - pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); - upperLimits[i] = - pybullet_internalGetFloatFromSequence(upperLimitsObj, i); - jointRanges[i] = - pybullet_internalGetFloatFromSequence(jointRangesObj, i); - restPoses[i] = - pybullet_internalGetFloatFromSequence(restPosesObj, i); + lowerLimits[i] = pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); + upperLimits[i] = pybullet_internalGetFloatFromSequence(upperLimitsObj, i); + jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i); + restPoses[i] = pybullet_internalGetFloatFromSequence(restPosesObj, i); } hasNullSpace = 1; } if (szCurrentPositions > 0) { - if (szCurrentPositions < numJoints) + if (szCurrentPositions != dofCount) { PyErr_SetString(SpamError, - "calculateInverseKinematics the size of input current positions is smaller than the number of joints."); + "calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom."); return NULL; } else @@ -8170,17 +8166,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, if (szJointDamping > 0) { - // 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) + if (szJointDamping != dofCount) { PyErr_SetString(SpamError, - "calculateInverseKinematics the size of input joint damping values is smaller than the number of joints."); + "calculateInverseKinematics the size of input joint damping values is unequal to the number of degrees of freedom."); return NULL; } else @@ -8208,7 +8197,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, if (hasCurrentPositions) { - b3CalculateInverseKinematicsSetCurrentPositions(command, numJoints, currentPositions); + b3CalculateInverseKinematicsSetCurrentPositions(command, dofCount, currentPositions); } if (maxNumIterations>0) { @@ -8223,11 +8212,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, { if (hasOrn) { - b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); } else { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); } } else @@ -8244,7 +8233,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, if (hasJointDamping) { - b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); + b3CalculateInverseKinematicsSetJointDamping(command, dofCount, jointDamping); } free(currentPositions); free(jointDamping); From 7653ddbeba1958f0e4bc939ba75e887918a16c1d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 31 May 2018 21:17:39 -0700 Subject: [PATCH 2/3] bump up pybullet version (for fixed ik) --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 7a3b2f443..90c0875fe 100644 --- a/setup.py +++ b/setup.py @@ -450,7 +450,7 @@ print("-----") setup( name = 'pybullet', - version='2.0.1', + version='2.0.2', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From e74cd05e37494fd42a94b3e3d6a47ad01cabd10b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 31 May 2018 21:21:50 -0700 Subject: [PATCH 3/3] add space --- examples/pybullet/gym/pybullet_envs/examples/ANYmal.py | 4 ++-- examples/pybullet/gym/pybullet_utils/bullet_client.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py b/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py index e413d3971..90f80a754 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py +++ b/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py @@ -24,8 +24,8 @@ ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SH p.setPhysicsEngineParameter(solverResidualThreshold=1e-2) index = 0 -numX = 3 -numY = 3 +numX = 10 +numY = 10 for i in range (numX): for j in range (numY): diff --git a/examples/pybullet/gym/pybullet_utils/bullet_client.py b/examples/pybullet/gym/pybullet_utils/bullet_client.py index 8fc1fcd5c..9b7f907f3 100644 --- a/examples/pybullet/gym/pybullet_utils/bullet_client.py +++ b/examples/pybullet/gym/pybullet_utils/bullet_client.py @@ -3,7 +3,7 @@ from __future__ import absolute_import from __future__ import division import functools import inspect -import pybullet +import pybullet class BulletClient(object):