AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

added getJointStates and make humanoid_running.py use it to reduce Python<->C++ calling overhead a lot.

Browse Source
This commit is contained in:
Erwin Coumans 2017-05-13 18:07:49 -07:00
parent 8b90885cf6
commit bb4c195118
2 changed files with 142 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
examples/pybullet/pybullet.c
View File

@ -2694,6 +2694,128 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args, PyObject
return Py_None;
}
static PyObject* pybullet_getJointStates(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* pyListJointForceTorque;
PyObject* pyListJointState;
PyObject* jointIndicesObj=0;
struct b3JointSensorState sensorState;
int bodyUniqueId = -1;
int sensorStateSize = 4; // size of struct b3JointSensorState
int forceTorqueSize = 6; // size of force torque list from b3JointSensorState
int j;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "jointIndices", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist, &bodyUniqueId, &jointIndicesObj, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
{
int i;
int status_type = 0;
int numRequestedJoints = 0;
PyObject* jointIndicesSeq = 0;
int numJoints = 0;
PyObject* resultListJointState=0;
b3SharedMemoryCommandHandle cmd_handle;
b3SharedMemoryStatusHandle status_handle;
if (bodyUniqueId < 0)
{
PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex");
return NULL;
}
numJoints = b3GetNumJoints(sm, bodyUniqueId);
jointIndicesSeq = PySequence_Fast(jointIndicesObj, "expected a sequence of joint indices");
if (jointIndicesSeq==0)
{
PyErr_SetString(SpamError, "expected a sequence of joint indices");
return NULL;
}
numRequestedJoints = PySequence_Size(jointIndicesObj);
if (numRequestedJoints==0)
{
Py_DECREF(jointIndicesSeq);
Py_INCREF(Py_None);
return Py_None;
}
cmd_handle =
b3RequestActualStateCommandInit(sm, bodyUniqueId);
status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
status_type = b3GetStatusType(status_handle);
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getJointState failed.");
return NULL;
}
resultListJointState = PyTuple_New(numRequestedJoints);
for (i = 0; i < numRequestedJoints; i++)
{
int jointIndex = pybullet_internalGetFloatFromSequence(jointIndicesSeq, i);
if ((jointIndex >= numJoints) || (jointIndex < 0))
{
Py_DECREF(jointIndicesSeq);
PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL;
}
pyListJointState = PyTuple_New(sensorStateSize);
pyListJointForceTorque = PyTuple_New(forceTorqueSize);
if (b3GetJointState(sm, status_handle, jointIndex, &sensorState))
{
PyTuple_SetItem(pyListJointState, 0,
PyFloat_FromDouble(sensorState.m_jointPosition));
PyTuple_SetItem(pyListJointState, 1,
PyFloat_FromDouble(sensorState.m_jointVelocity));
for (j = 0; j < forceTorqueSize; j++)
{
PyTuple_SetItem(pyListJointForceTorque, j,
PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
}
PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
PyTuple_SetItem(pyListJointState, 3,
PyFloat_FromDouble(sensorState.m_jointMotorTorque));
PyTuple_SetItem(resultListJointState, i, pyListJointState);
}
else
{
PyErr_SetString(SpamError, "getJointState failed (2).");
return NULL;
}
}
return resultListJointState;
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* pyLinkState;
@ -5913,7 +6035,10 @@ static PyMethodDef SpamMethods[] = {
{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for a joint on a body."},
{"getJointStates", (PyCFunction)pybullet_getJointStates, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for multiple joints on a body."},
{"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS,
"Provides extra information such as the Cartesian world coordinates"
" center of mass (COM) of the link, relative to the world reference"

20
examples/pybullet/tensorflow/humanoid_running.py
View File

@ -4,7 +4,7 @@ import numpy as np
import pybullet as p
import time
p.connect(p.GUI) #GUI is slower, but shows the running gait
p.connect(p.GUI) #DIRECT is much faster, but GUI shows the running gait
p.setGravity(0,0,-9.8)
p.setPhysicsEngineParameter(fixedTimeStep=1.0/60., numSolverIterations=5, numSubSteps=2)
#this mp4 recording requires ffmpeg installed
@ -14,6 +14,8 @@ p.setPhysicsEngineParameter(fixedTimeStep=1.0/60., numSolverIterations=5, numSub
plane, human = p.loadMJCF("mjcf/humanoid_symmetric.xml",flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
ordered_joints = []
ordered_joint_indices = []
jdict = {}
for j in range( p.getNumJoints(human) ):
@ -21,11 +23,15 @@ for j in range( p.getNumJoints(human) ):
link_name = info[12].decode("ascii")
if link_name=="left_foot": left_foot = j
if link_name=="right_foot": right_foot = j
ordered_joint_indices.append(j)
if info[2] != p.JOINT_REVOLUTE: continue
jname = info[1].decode("ascii")
jdict[jname] = j
lower, upper = (info[8], info[9])
ordered_joints.append( (j, lower, upper) )
p.setJointMotorControl2(human, j, controlMode=p.VELOCITY_CONTROL, force=0)
@ -47,10 +53,12 @@ class Dummy:
dummy = Dummy()
dummy.initial_z = None
def current_relative_position(human, j, lower, upper):
def current_relative_position(jointStates, human, j, lower, upper):
#print("j")
#print(j)
temp = p.getJointState(human, j)
#print (len(jointStates))
#print(j)
temp = jointStates[j]
pos = temp[0]
vel = temp[1]
#print("pos")
@ -64,7 +72,11 @@ def current_relative_position(human, j, lower, upper):
)
def collect_observations(human):
j = np.array([current_relative_position(human, *jtuple) for jtuple in ordered_joints]).flatten()
#print("ordered_joint_indices")
#print(ordered_joint_indices)
jointStates = p.getJointStates(human,ordered_joint_indices)
j = np.array([current_relative_position(jointStates, human, *jtuple) for jtuple in ordered_joints]).flatten()
#print("j")
#print(j)
body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human)