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

Fix for 1643, allow to instantiate multiple PyBullet Gym environments (Ant, Humanoid, Hopper, Pendula etc) in the same process (same or other thread). It uses the pybullet_utils.bullet_client to achieve this.

Browse Source
This commit is contained in:
Erwin Coumans
2018-05-18 16:23:54 -07:00
parent d17d496f97
commit 0abe4151e5
24 changed files with 163 additions and 403 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
examples/pybullet/gym/pybullet_envs/robot_bases.py
View File

@@ -1,4 +1,4 @@
import pybullet as p
import pybullet
import gym, gym.spaces, gym.utils
import numpy as np
import os, inspect
@@ -14,8 +14,7 @@ class XmlBasedRobot:
"""
self_collision = True
def __init__(self, robot_name, action_dim, obs_dim, self_collision):
#def __init__(self, model_xml, robot_name, action_dim, obs_dim):
def __init__(self, robot_name, action_dim, obs_dim, self_collision):
self.parts = None
self.objects = []
self.jdict = None
@@ -31,7 +30,9 @@ class XmlBasedRobot:
self.robot_name = robot_name
self.self_collision = self_collision
def addToScene(self, bodies):
def addToScene(self, bullet_client, bodies):
self._p = bullet_client
if self.parts is not None:
parts = self.parts
else:
@@ -52,14 +53,14 @@ class XmlBasedRobot:
dump = 0
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
if self._p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = self._p.getBodyInfo(bodies[i])
self.robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, i, -1)
for j in range(p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
jointInfo = p.getJointInfo(bodies[i], j)
parts[part_name] = BodyPart(self._p, part_name, bodies, i, -1)
for j in range(self._p.getNumJoints(bodies[i])):
self._p.setJointMotorControl2(bodies[i],j,pybullet.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
jointInfo = self._p.getJointInfo(bodies[i], j)
joint_name=jointInfo[1]
part_name=jointInfo[12]
@@ -69,21 +70,21 @@ class XmlBasedRobot:
if dump: print("ROBOT PART '%s'" % part_name)
if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )
parts[part_name] = BodyPart(part_name, bodies, i, j)
parts[part_name] = BodyPart(self._p, part_name, bodies, i, j)
if part_name == self.robot_name:
self.robot_body = parts[part_name]
if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body
parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1)
parts[self.robot_name] = BodyPart(self._p, self.robot_name, bodies, 0, -1)
self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j).disable_motor()
Joint(self._p, joint_name, bodies, i, j).disable_motor()
continue
if joint_name[:8] != "jointfix":
joints[joint_name] = Joint(joint_name, bodies, i, j)
joints[joint_name] = Joint(self._p, joint_name, bodies, i, j)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
@@ -103,23 +104,24 @@ class MJCFBasedRobot(XmlBasedRobot):
Base class for mujoco .xml based agents.
"""
def __init__(self, model_xml, robot_name, action_dim, obs_dim, self_collision=True):
def __init__(self, model_xml, robot_name, action_dim, obs_dim, self_collision=True):
XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision)
self.model_xml = model_xml
self.doneLoading=0
def reset(self):
def reset(self, bullet_client):
self._p = bullet_client
print("Created bullet_client with id=", self._p._client)
if (self.doneLoading==0):
self.ordered_joints = []
self.doneLoading=1
if self.self_collision:
self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION|p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects )
self.objects = self._p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=pybullet.URDF_USE_SELF_COLLISION|pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, self.objects )
else:
self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml))
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects)
self.robot_specific_reset()
self.objects = self._p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml))
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, self.objects)
self.robot_specific_reset(self._p)
s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use
@@ -142,26 +144,27 @@ class URDFBasedRobot(XmlBasedRobot):
self.baseOrientation = baseOrientation
self.fixed_base = fixed_base
def reset(self):
def reset(self, bullet_client):
self._p = bullet_client
self.ordered_joints = []
print(os.path.join(os.path.dirname(__file__), "data", self.model_urdf))
if self.self_collision:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf),
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p,
self._p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf),
basePosition=self.basePosition,
baseOrientation=self.baseOrientation,
useFixedBase=self.fixed_base,
flags=p.URDF_USE_SELF_COLLISION))
flags=pybullet.URDF_USE_SELF_COLLISION))
else:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(
p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf),
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p,
self._p.loadURDF(os.path.join(pybullet_data.getDataPath(), self.model_urdf),
basePosition=self.basePosition,
baseOrientation=self.baseOrientation,
useFixedBase=self.fixed_base))
self.robot_specific_reset()
self.robot_specific_reset(self._p)
s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use
self.potential = self.calc_potential()
@@ -177,19 +180,21 @@ class SDFBasedRobot(XmlBasedRobot):
Base class for SDF robots in a Scene.
"""
def __init__(self, model_sdf, robot_name, action_dim, obs_dim, basePosition=[0, 0, 0], baseOrientation=[0, 0, 0, 1], fixed_base=False, self_collision=False):
def __init__(self, model_sdf, robot_name, action_dim, obs_dim, basePosition=[0, 0, 0], baseOrientation=[0, 0, 0, 1], fixed_base=False, self_collision=False):
XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision)
self.model_sdf = model_sdf
self.fixed_base = fixed_base
def reset(self):
def reset(self, bullet_client):
self._p = bullet_client
self.ordered_joints = []
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( # TODO: Not sure if this works, try it with kuka
p.loadSDF(os.path.join("models_robot", self.model_sdf)))
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self._p, # TODO: Not sure if this works, try it with kuka
self._p.loadSDF(os.path.join("models_robot", self.model_sdf)))
self.robot_specific_reset()
self.robot_specific_reset(self._p)
s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use
self.potential = self.calc_potential()
@@ -208,14 +213,15 @@ class Pose_Helper: # dummy class to comply to original interface
return self.body_part.current_position()
def rpy(self):
return p.getEulerFromQuaternion(self.body_part.current_orientation())
return pybullet.getEulerFromQuaternion(self.body_part.current_orientation())
def orientation(self):
return self.body_part.current_orientation()
class BodyPart:
def __init__(self, body_name, bodies, bodyIndex, bodyPartIndex):
def __init__(self, bullet_client, body_name, bodies, bodyIndex, bodyPartIndex):
self.bodies = bodies
self._p = bullet_client
self.bodyIndex = bodyIndex
self.bodyPartIndex = bodyPartIndex
self.initialPosition = self.current_position()
@@ -224,9 +230,9 @@ class BodyPart:
def state_fields_of_pose_of(self, body_id, link_id=-1): # a method you will most probably need a lot to get pose and orientation
if link_id == -1:
(x, y, z), (a, b, c, d) = p.getBasePositionAndOrientation(body_id)
(x, y, z), (a, b, c, d) = self._p.getBasePositionAndOrientation(body_id)
else:
(x, y, z), (a, b, c, d), _, _, _, _ = p.getLinkState(body_id, link_id)
(x, y, z), (a, b, c, d), _, _, _, _ = self._p.getLinkState(body_id, link_id)
return np.array([x, y, z, a, b, c, d])
def get_pose(self):
@@ -234,9 +240,9 @@ class BodyPart:
def speed(self):
if self.bodyPartIndex == -1:
(vx, vy, vz), _ = p.getBaseVelocity(self.bodies[self.bodyIndex])
(vx, vy, vz), _ = self._p.getBaseVelocity(self.bodies[self.bodyIndex])
else:
(x,y,z), (a,b,c,d), _,_,_,_, (vx, vy, vz), (vr,vp,vy) = p.getLinkState(self.bodies[self.bodyIndex], self.bodyPartIndex, computeLinkVelocity=1)
(x,y,z), (a,b,c,d), _,_,_,_, (vx, vy, vz), (vr,vp,vy) = self._p.getLinkState(self.bodies[self.bodyIndex], self.bodyPartIndex, computeLinkVelocity=1)
return np.array([vx, vy, vz])
def current_position(self):
@@ -249,39 +255,40 @@ class BodyPart:
return self.current_orientation()
def reset_position(self, position):
p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, self.get_orientation())
self._p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, self.get_orientation())
def reset_orientation(self, orientation):
p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation)
self._p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation)
def reset_velocity(self, linearVelocity=[0,0,0], angularVelocity =[0,0,0]):
p.resetBaseVelocity(self.bodies[self.bodyIndex], linearVelocity, angularVelocity)
self._p.resetBaseVelocity(self.bodies[self.bodyIndex], linearVelocity, angularVelocity)
def reset_pose(self, position, orientation):
p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, orientation)
self._p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, orientation)
def pose(self):
return self.bp_pose
def contact_list(self):
return p.getContactPoints(self.bodies[self.bodyIndex], -1, self.bodyPartIndex, -1)
return self._p.getContactPoints(self.bodies[self.bodyIndex], -1, self.bodyPartIndex, -1)
class Joint:
def __init__(self, joint_name, bodies, bodyIndex, jointIndex):
def __init__(self, bullet_client, joint_name, bodies, bodyIndex, jointIndex):
self.bodies = bodies
self._p = bullet_client
self.bodyIndex = bodyIndex
self.jointIndex = jointIndex
self.joint_name = joint_name
jointInfo = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex)
jointInfo = self._p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex)
self.lowerLimit = jointInfo[8]
self.upperLimit = jointInfo[9]
self.power_coeff = 0
def set_state(self, x, vx):
p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx)
self._p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx)
def current_position(self): # just some synonyme method
return self.get_state()
@@ -295,7 +302,7 @@ class Joint:
)
def get_state(self):
x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex)
x, vx,_,_ = self._p.getJointState(self.bodies[self.bodyIndex],self.jointIndex)
return x, vx
def get_position(self):
@@ -311,23 +318,23 @@ class Joint:
return vx
def set_position(self, position):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
self._p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,pybullet.POSITION_CONTROL, targetPosition=position)
def set_velocity(self, velocity):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity)
self._p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,pybullet.VELOCITY_CONTROL, targetVelocity=velocity)
def set_motor_torque(self, torque): # just some synonyme method
self.set_torque(torque)
def set_torque(self, torque):
p.setJointMotorControl2(bodyIndex=self.bodies[self.bodyIndex], jointIndex=self.jointIndex, controlMode=p.TORQUE_CONTROL, force=torque) #, positionGain=0.1, velocityGain=0.1)
self._p.setJointMotorControl2(bodyIndex=self.bodies[self.bodyIndex], jointIndex=self.jointIndex, controlMode=pybullet.TORQUE_CONTROL, force=torque) #, positionGain=0.1, velocityGain=0.1)
def reset_current_position(self, position, velocity): # just some synonyme method
self.reset_position(position, velocity)
def reset_position(self, position, velocity):
p.resetJointState(self.bodies[self.bodyIndex],self.jointIndex,targetValue=position, targetVelocity=velocity)
self._p.resetJointState(self.bodies[self.bodyIndex],self.jointIndex,targetValue=position, targetVelocity=velocity)
self.disable_motor()
def disable_motor(self):
p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=p.POSITION_CONTROL, targetPosition=0, targetVelocity=0, positionGain=0.1, velocityGain=0.1, force=0)
self._p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=pybullet.POSITION_CONTROL, targetPosition=0, targetVelocity=0, positionGain=0.1, velocityGain=0.1, force=0)