Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests 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
Download Patch File Download Diff File Expand all files Collapse all files

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)