Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1725 from erwincoumans/master

Browse Source 
Use dofCount and not numJoints in PyBullet.calculateInverseKinematics…
This commit is contained in:
erwincoumans
2018-06-01 07:31:09 -07:00
committed by GitHub
parent 1d53c48322 e74cd05e37
commit 00c8f8f68e
4 changed files with 20 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/pybullet/gym/pybullet_envs/examples/ANYmal.py
View File

@@ -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):

2
examples/pybullet/gym/pybullet_utils/bullet_client.py
View File

@@ -3,7 +3,7 @@ from __future__ import absolute_import
from __future__ import division
import functools
import inspect
import pybullet
import pybullet
class BulletClient(object):

43
examples/pybullet/pybullet.c
View File

@@ -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);

2
setup.py
View File

@@ -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',