mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-05 23:31:06 +00:00
added getJointStates and make humanoid_running.py use it to reduce Python<->C++ calling overhead a lot.
This commit is contained in:
parent
8b90885cf6
commit
bb4c195118
@ -2694,6 +2694,128 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args, PyObject
|
|||||||
return Py_None;
|
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)
|
static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject* keywds)
|
||||||
{
|
{
|
||||||
PyObject* pyLinkState;
|
PyObject* pyLinkState;
|
||||||
@ -5913,7 +6035,10 @@ static PyMethodDef SpamMethods[] = {
|
|||||||
|
|
||||||
{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
|
{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
|
||||||
"Get the state (position, velocity etc) for a joint on a body."},
|
"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,
|
{"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS,
|
||||||
"Provides extra information such as the Cartesian world coordinates"
|
"Provides extra information such as the Cartesian world coordinates"
|
||||||
" center of mass (COM) of the link, relative to the world reference"
|
" center of mass (COM) of the link, relative to the world reference"
|
||||||
|
@ -4,7 +4,7 @@ import numpy as np
|
|||||||
|
|
||||||
import pybullet as p
|
import pybullet as p
|
||||||
import time
|
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.setGravity(0,0,-9.8)
|
||||||
p.setPhysicsEngineParameter(fixedTimeStep=1.0/60., numSolverIterations=5, numSubSteps=2)
|
p.setPhysicsEngineParameter(fixedTimeStep=1.0/60., numSolverIterations=5, numSubSteps=2)
|
||||||
#this mp4 recording requires ffmpeg installed
|
#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)
|
plane, human = p.loadMJCF("mjcf/humanoid_symmetric.xml",flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
|
||||||
|
|
||||||
ordered_joints = []
|
ordered_joints = []
|
||||||
|
ordered_joint_indices = []
|
||||||
|
|
||||||
|
|
||||||
jdict = {}
|
jdict = {}
|
||||||
for j in range( p.getNumJoints(human) ):
|
for j in range( p.getNumJoints(human) ):
|
||||||
@ -21,11 +23,15 @@ for j in range( p.getNumJoints(human) ):
|
|||||||
link_name = info[12].decode("ascii")
|
link_name = info[12].decode("ascii")
|
||||||
if link_name=="left_foot": left_foot = j
|
if link_name=="left_foot": left_foot = j
|
||||||
if link_name=="right_foot": right_foot = j
|
if link_name=="right_foot": right_foot = j
|
||||||
|
ordered_joint_indices.append(j)
|
||||||
|
|
||||||
if info[2] != p.JOINT_REVOLUTE: continue
|
if info[2] != p.JOINT_REVOLUTE: continue
|
||||||
jname = info[1].decode("ascii")
|
jname = info[1].decode("ascii")
|
||||||
jdict[jname] = j
|
jdict[jname] = j
|
||||||
lower, upper = (info[8], info[9])
|
lower, upper = (info[8], info[9])
|
||||||
ordered_joints.append( (j, lower, upper) )
|
ordered_joints.append( (j, lower, upper) )
|
||||||
|
|
||||||
|
|
||||||
p.setJointMotorControl2(human, j, controlMode=p.VELOCITY_CONTROL, force=0)
|
p.setJointMotorControl2(human, j, controlMode=p.VELOCITY_CONTROL, force=0)
|
||||||
|
|
||||||
|
|
||||||
@ -47,10 +53,12 @@ class Dummy:
|
|||||||
dummy = Dummy()
|
dummy = Dummy()
|
||||||
dummy.initial_z = None
|
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")
|
||||||
#print(j)
|
#print(j)
|
||||||
temp = p.getJointState(human, j)
|
#print (len(jointStates))
|
||||||
|
#print(j)
|
||||||
|
temp = jointStates[j]
|
||||||
pos = temp[0]
|
pos = temp[0]
|
||||||
vel = temp[1]
|
vel = temp[1]
|
||||||
#print("pos")
|
#print("pos")
|
||||||
@ -64,7 +72,11 @@ def current_relative_position(human, j, lower, upper):
|
|||||||
)
|
)
|
||||||
|
|
||||||
def collect_observations(human):
|
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")
|
||||||
#print(j)
|
#print(j)
|
||||||
body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human)
|
body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human)
|
||||||
|
Loading…
Reference in New Issue
Block a user