def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.robotName = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[1].decode("utf-8")
self.linkNameToIndex[baseLink.link_name] = len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid, physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid, j, physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink, physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name] = len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_lower_limit = jointInfo[8]
urdfJoint.joint_upper_limit = jointInfo[9]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex < 0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid, orgParentIndex, physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid, j, physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos = jointInfo[14]
parentCom2JointOrn = jointInfo[15]
tmpPos, tmpOrn = p.multiplyTransforms(childInertiaPos, childInertiaOrn, parentCom2JointPos,
parentCom2JointOrn)
tmpPosInv, tmpOrnInv = p.invertTransform(tmpPos, tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid, orgParentIndex, physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos, orn = p.multiplyTransforms(parentInertiaPos, parentInertiaOrn, tmpPosInv, tmpOrnInv)
pos, orn_unused = p.multiplyTransforms(parentInertiaPos, parentInertiaOrn,
parentCom2JointPos, [0, 0, 0, 1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def getTableCenter(tableID):
TABLE_HEIGHT = .63
print('%s = %s\n' % ('p.getBasePositionAndOrientation(tableID)',
p.getBasePositionAndOrientation(tableID)))
print('%s = %s\n' % ('p.getNumJoints(tableID)', p.getNumJoints(tableID)))
#print('%s = %s\n'%('p.getJointInfo(tableID)',p.getJointInfo(tableID,0)));
print('%s = %s\n' % ('p.getBodyInfo(tableID)', p.getBodyInfo(tableID)))
print('%s = %s\n' %
('p.getBodyInfo(tableID)[0]', p.getBodyInfo(tableID)[0]))
return [0, 0, TABLE_HEIGHT]
def main():
print("create env")
# env = gym.make("HumanoidFlagrunHarderPyBulletEnv-v0")
env = gym.make("ParkourBiped-v0")
env.render(mode="human")
print(env.observation_space)
print(type(env.action_space))
pi = ReactivePolicy(env.observation_space, env.action_space)
while 1:
frame = 0
score = 0
restart_delay = 0
obs = env.reset()
torsoId = -1
for i in range(p.getNumBodies()):
print(p.getBodyInfo(i))
if p.getBodyInfo(i)[0].decode() == "base":
torsoId = i
print("found humanoid torso")
while 1:
time.sleep(0.02)
a = pi.act(obs)
obs, r, done, _ = env.step(a)
score += r
frame += 1
humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId)
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance = camInfo[10]
yaw = camInfo[8]
pitch = camInfo[9]
targetPos = [
0.95 * curTargetPos[0] + 0.05 * humanPos[0],
0.95 * curTargetPos[1] + 0.05 * humanPos[1], curTargetPos[2]
]
p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos)
still_open = env.render("human")
if still_open is None:
return
if not done:
continue
if restart_delay == 0:
print("score=%0.2f in %i frames" % (score, frame))
restart_delay = 60 * 2 # 2 sec at 60 fps
else:
restart_delay -= 1
if restart_delay == 0:
break
def test_remove_nao(self):
"""
Test the @removeNao method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
nao = manager.spawnNao(client, spawn_ground_plane=True)
manager.removeNao(nao)
with self.assertRaises(pybullet.error):
pybullet.getBodyInfo(nao.getRobotModel())
manager.stopSimulation(client)
def __load_robot_urdf(self):
"""
Load the single/trifinger model from the corresponding urdf
"""
finger_base_position = [0, 0, 0.0]
finger_base_orientation = pybullet.getQuaternionFromEuler([0, 0, 0])
self.finger_id = pybullet.loadURDF(
fileName=self.finger_urdf_path,
basePosition=finger_base_position,
baseOrientation=finger_base_orientation,
useFixedBase=1,
flags=(pybullet.URDF_USE_INERTIA_FROM_FILE
| pybullet.URDF_USE_SELF_COLLISION),
)
# create a map link_name -> link_index
# Source: https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=12728.
link_name_to_index = {
pybullet.getBodyInfo(self.finger_id)[0].decode("UTF-8"): -1,
}
for joint_idx in range(pybullet.getNumJoints(self.finger_id)):
link_name = pybullet.getJointInfo(self.finger_id,
joint_idx)[12].decode("UTF-8")
link_name_to_index[link_name] = joint_idx
self.pybullet_link_indices = [
link_name_to_index[name] for name in self.link_names
]
self.pybullet_tip_link_indices = [
link_name_to_index[name] for name in self.tip_link_names
]
# joint and link indices are the same in pybullet
self.pybullet_joint_indices = self.pybullet_link_indices
def get_cube(x, y, z):
body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), x, y, z)
p.changeDynamics(body,-1, mass=1.2)#match Roboschool
part_name, _ = p.getBodyInfo(body, 0)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(part_name, bodies, 0, -1)
def register_object(self, obj, name_override=None):
"""Registers an object with the simulator.
Unless a specific name is given, the simulator will automatically assign one to the object.
If the specific name is already taken an exception will be raised.
:param obj: Object to register with the simulator.
:type obj: iai_bullet_sim.rigid_body.RigidBody, iai_bullet_sim.multibody.MultiBody
:param name_override: Name to assign to the object.
:type obj: str, NoneType
"""
if name_override is None:
if isinstance(obj, MultiBody):
base_link, bodyId = pb.getBodyInfo(
obj.bId(), physicsClientId=self.__client_id)
elif isinstance(obj, RigidBody):
bodyId = obj.type
counter = 0
bodyName = bodyId
while bodyId in self.bodies:
bodyId = '{}.{}'.format(bodyName, counter)
counter += 1
self.bodies[bodyId] = obj
self.__bId_IdMap[obj.bId()] = bodyId
return bodyId
else:
if name_override in self.bodies:
raise Exception(
'Id "{}" is already taken.'.format(name_override))
self.bodies[name_override] = obj
self.__bId_IdMap[obj.bId()] = name_override
return name_override
def get_sphere(x, y, z):
body = p.loadURDF(
os.path.join(pybullet_data.getDataPath(), "sphere2red.urdf"), x, y, z)
part_name, _ = p.getBodyInfo(body, 0)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(part_name, bodies, 0, -1)
def buildTower(blockPath, tW, tH, basePos):
#blockPath is string path to block object
#tW is width of tower (usually 3, must be odd)
#tH is height of tower
#basePos is position of center bottom block
blockList = []
BO = (tW - 1) / 2
curHeight = basePos[2]
for i in range(0, tH):
curHeight = basePos[2] + BLOCK_HEIGHT * i
curPos = [basePos[0], basePos[1], curHeight]
for j in range(0, tW):
print('(i,j) = (%d,%d)' % (i, j))
if (i % 2 == 0):
orientation = p.getQuaternionFromEuler([0, 0, 0])
yoffset = (j - BO) * BLOCK_WIDTH
curPos[1] = basePos[1] + yoffset
else:
orientation = p.getQuaternionFromEuler([0, 0, 3.14159 / 2])
xoffset = (j - BO) * BLOCK_WIDTH
curPos[0] = basePos[0] + xoffset
print('Loading block at [%f,%f,%f]' %
(curPos[0], curPos[1], curPos[2]))
blockList.append(p.loadURDF(blockPath, curPos, orientation))
print(p.getBodyInfo(blockList[0]))
return blockList
def get_robot_config(robot):
nq, nv, na, joint_id, link_id = 0, 0, 0, OrderedDict(), OrderedDict()
link_id[(p.getBodyInfo(robot)[0]).decode("utf-8")] = -1
for i in range(p.getNumJoints(robot)):
info = p.getJointInfo(robot, i)
if info[2] != p.JOINT_FIXED:
joint_id[info[1].decode("utf-8")] = info[0]
link_id[info[12].decode("utf-8")] = info[0]
nq = max(nq, info[3])
nv = max(nv, info[4])
nq += 1
nv += 1
na = len(joint_id)
print("=" * 80)
print("SimulationRobot")
print("nq: ", nq, ", nv: ", nv, ", na: ", na)
print("+" * 80)
print("Joint Infos")
util.pretty_print(joint_id)
print("+" * 80)
print("Link Infos")
util.pretty_print(link_id)
return nq, nv, na, joint_id, link_id
def get_cube(x, y, z):
body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), x, y, z)
p.changeDynamics(body,-1, mass=1.2)#match Roboschool
part_name, _ = p.getBodyInfo(body, 0)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(part_name, bodies, 0, -1)
def _reset(self):
assert(self._robot_introduced)
assert(self._scene_introduced)
debugmode = 1
if debugmode:
print("Episode: steps:{} score:{}".format(self.nframe, self.reward))
body_xyz = self.robot.body_xyz
print("[{}, {}, {}],".format(body_xyz[0], body_xyz[1], body_xyz[2]))
print("Episode count: {}".format(self.eps_count))
self.eps_count += 1
self.nframe = 0
self.eps_reward = 0
BaseEnv._reset(self)
if not self.ground_ids:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene(
[])
self.ground_ids = set(self.scene.scene_obj_list)
## Todo: (hzyjerry) this part is not working, robot_tracking_id = -1
for i in range (p.getNumBodies()):
if (p.getBodyInfo(i)[0].decode() == self.robot_body.get_name()):
self.robot_tracking_id=i
#print(p.getBodyInfo(i)[0].decode())
i = 0
eye_pos, eye_quat = self.get_eye_pos_orientation()
pose = [eye_pos, eye_quat]
observations = self.render_observations(pose)
pos = self.robot._get_scaled_position()
orn = self.robot.get_orientation()
pos = (pos[0], pos[1], pos[2] + self.tracking_camera['z_offset'])
p.resetDebugVisualizerCamera(self.tracking_camera['distance'],self.tracking_camera['yaw'], self.tracking_camera['pitch'],pos)
return observations
def robot_specific_reset(self):
WalkerBase.robot_specific_reset(self)
humanoidId = -1
numBodies = p.getNumBodies()
for i in range (numBodies):
bodyInfo = p.getBodyInfo(i)
if bodyInfo[1].decode("ascii") == 'humanoid':
humanoidId = i
## Spherical radiance/glass shield to protect the robot's camera
if self.glass_id is None:
glass_id = p.loadMJCF(os.path.join(self.physics_model_dir, "glass.xml"))[0]
#print("setting up glass", glass_id, humanoidId)
p.changeVisualShape(glass_id, -1, rgbaColor=[0, 0, 0, 0])
cid = p.createConstraint(humanoidId, -1, glass_id,-1,p.JOINT_FIXED,[0,0,0],[0,0,1.4],[0,0,1])
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 0
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, i, -1, self.scale, model_type=self.model_type)
for j in range(p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
## TODO (hzyjerry): the following is diabled due to pybullet update
#_,joint_name,joint_type, _,_,_, _,_,_,_, _,_, part_name = p.getJointInfo(bodies[i], j)
_,joint_name,joint_type, _,_,_, _,_,_,_, _,_, part_name, _,_,_,_ = p.getJointInfo(bodies[i], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
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, self.scale, model_type=self.model_type)
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, self.scale, model_type=self.model_type)
self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j, self.scale).disable_motor()
continue
if joint_name[:8] != "jointfix" and joint_type != p.JOINT_FIXED:
joints[joint_name] = Joint(joint_name, bodies, i, j, self.scale, model_type=self.model_type)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
debugmode = 0
if debugmode:
for j in ordered_joints:
print(j, j.power_coef)
return parts, joints, ordered_joints, self.robot_body
def register_object(self, body_id, urdf_path, global_scaling=1):
link_id_map = dict()
n = p.getNumJoints(body_id)
link_id_map[p.getBodyInfo(body_id)[0].decode('gb2312')] = -1
for link_id in range(0, n):
link_id_map[p.getJointInfo(body_id, link_id)[
12].decode('gb2312')] = link_id
dir_path = dirname(abspath(urdf_path))
file_name = splitext(basename(urdf_path))[0]
robot = URDF.load(urdf_path)
for link in robot.links:
link_id = link_id_map[link.name]
if len(link.visuals) > 0:
for i, link_visual in enumerate(link.visuals):
mesh_scale = [global_scaling,
global_scaling, global_scaling]\
if link_visual.geometry.mesh.scale is None \
else link_visual.geometry.mesh.scale * global_scaling
self.links.append(
PyBulletRecorder.LinkTracker(
name=file_name + f'_{body_id}_{link.name}_{i}',
body_id=body_id,
link_id=link_id,
link_origin= # If link_id == -1 then is base link,
# PyBullet will return
# inertial_origin @ visual_origin,
# so need to undo that transform
(np.linalg.inv(link.inertial.origin)
if link_id == -1
else np.identity(4)) @
link_visual.origin * global_scaling,
mesh_path=dir_path + '/' +
link_visual.geometry.mesh.filename,
mesh_scale=mesh_scale))
def getLinkNames(model_id):
_link_name_to_index = {p.getBodyInfo(model_id)[0].decode('UTF-8'):-1,}
for _id in range(p.getNumJoints(model_id)):
_name = p.getJointInfo(model_id, _id)[12].decode('UTF-8')
_link_name_to_index[_name] = _id
return _link_name_to_index
def get_cube(x, y, z):
print("get cube")
body = p.loadURDF(
os.path.join(pybullet_data.getDataPath(), "cube_small.urdf"), x, y, z)
part_name, _ = p.getBodyInfo(body, 0)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(part_name, bodies, 0, -1)
def _check_body_exist(self, body_id):
"""
Check if the body exist in the pybullet client.
Args:
body_id (int): body index.
Returns:
bool: True if the body exists, False otherwise.
"""
exist = True
try:
p.getBodyInfo(body_id, physicsClientId=self._pb_id)
except Exception:
exist = False
return exist
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 1
for i in range(len(bodies)):
print(i)
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
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])):
_, joint_name, _, _, _, _, _, _, _, _, _, _, part_name, _, _, _, _ = p.getJointInfo(
bodies[i], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
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)
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)
self.robot_body = parts[self.robot_name]
if joint_name[:8] != "jointfix":
joints[joint_name] = Joint(joint_name, bodies, i, j)
ordered_joints.append(joints[joint_name])
if joint_name[:6] == "ignore":
joints[joint_name].disable_motor()
continue
joints[joint_name].power_coef = 100.0
return parts, joints, ordered_joints, self.robot_body
def get_body_info(body):
"""
Get the body info
:param body:int
body's ID
:return: class
Return base name of the body
"""
return BodyInfo(*p.getBodyInfo(body))
def __init__(self, RobotId):
self.RobotId = RobotId
self.index_dof = {
p.getBodyInfo(self.RobotId)[0].decode('UTF-8'): -1,
}
for id in range(p.getNumJoints(self.RobotId)):
self.index_dof[p.getJointInfo(
self.RobotId, id)[12].decode('UTF-8')] = p.getJointInfo(
self.RobotId, id)[3] - 7
def robot_specific_reset(self):
"""
Humanoid robot specific reset
Add spherical radiance/glass shield to protect the robot's camera
"""
humanoidId = -1
numBodies = p.getNumBodies()
for i in range(numBodies):
bodyInfo = p.getBodyInfo(i)
if bodyInfo[1].decode("ascii") == 'humanoid':
humanoidId = i
# Spherical radiance/glass shield to protect the robot's camera
super(Humanoid, self).robot_specific_reset()
if self.glass_id is None:
glass_path = os.path.join(self.physics_model_dir,
"humanoid/glass.xml")
glass_id = p.loadMJCF(glass_path)[0]
self.glass_id = glass_id
p.changeVisualShape(self.glass_id, -1, rgbaColor=[0, 0, 0, 0])
p.createMultiBody(baseVisualShapeIndex=glass_id,
baseCollisionShapeIndex=-1)
cid = p.createConstraint(
humanoidId,
-1,
self.glass_id,
-1,
p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, self.glass_offset],
childFramePosition=[0, 0, 0])
robot_pos = list(self.get_position())
robot_pos[2] += self.glass_offset
robot_orn = self.get_orientation()
p.resetBasePositionAndOrientation(self.glass_id, robot_pos, robot_orn)
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += [
"right_hip_x", "right_hip_z", "right_hip_y", "right_knee"
]
self.motor_power += [100, 100, 300, 200]
self.motor_names += [
"left_hip_x", "left_hip_z", "left_hip_y", "left_knee"
]
self.motor_power += [100, 100, 300, 200]
self.motor_names += [
"right_shoulder1", "right_shoulder2", "right_elbow"
]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#print(visualShapes)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex<0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos=jointInfo[14]
parentCom2JointOrn=jointInfo[15]
tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def __str__(self):
robot_link_name = pb.getJointInfo(self.robot_body, self.robot_link,
physicsClientId=self.pb_client)[12].decode('ascii')
if self.self_collision:
other_link_name = pb.getJointInfo(self.other_body, self.other_link,
physicsClientId=self.pb_client)[12].decode('ascii')
return f"Collision between xarm:{robot_link_name} and xarm:{other_link_name}"
else:
other_body_name = pb.getBodyInfo(self.other_body,
physicsClientId=self.pb_client)[1].decode('ascii')
return f"Collision between xarm:{robot_link_name} and {other_body_name}"
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 0
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
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)
_,joint_name,_,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
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)
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)
self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j).disable_motor()
continue
if joint_name[:8] != "jointfix":
joints[joint_name] = Joint(joint_name, bodies, i, j)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
return parts, joints, ordered_joints, self.robot_body
def _predict(sess, env, policy_estimator, weight_path, num_episodes=5):
policy_estimator.model.load_weights(weight_path)
env.reset()
torsoId = -1
for i in range(p.getNumBodies()):
print('body_info_{}'.format(p.getBodyInfo(i)))
if p.getBodyInfo(i)[1].decode() == 'walker2d':
torsoId = i
Step = collections.namedtuple('Step', ['state', 'action', 'reward'])
scores = []
print('start_episodes...')
for i_episode in range(1, num_episodes + 1):
state = env.reset()
episode = []
score = 0
steps = 0
while True:
steps += 1
action = policy_estimator.predict(sess, state)
state_new, r, done, _ = env.step(action)
score += r
distance = 5
yaw = 0
humanPos = p.getLinkState(torsoId, 4)[0]
p.resetDebugVisualizerCamera(distance, yaw, -20, humanPos)
still_open = env.render('human')
if still_open is None:
return
episode.append(Step(state=state, action=action, reward=r))
state = state_new
if done:
print('episode_{}, score_{}'.format(i_episode, score))
scores.append(score)
break
print('{}episode avg_score_{}'.format(num_episodes, np.mean(scores)))
def __load_robot_urdf(self, robot_position_offset: typing.Sequence[float]):
"""
Load the single/trifinger model from the corresponding urdf
Args:
robot_position_offset: Position offset with which the robot is
placed in the world. Use this, for example, to change the
height of the fingers above the table.
"""
finger_base_orientation = (0, 0, 0, 1)
self.finger_id = pybullet.loadURDF(
fileName=self.finger_urdf_path,
basePosition=robot_position_offset,
baseOrientation=finger_base_orientation,
useFixedBase=1,
flags=(
pybullet.URDF_USE_INERTIA_FROM_FILE
| pybullet.URDF_USE_SELF_COLLISION
),
physicsClientId=self._pybullet_client_id,
)
# create a map link_name -> link_index
# Source: https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=12728.
link_name_to_index = {
pybullet.getBodyInfo(
self.finger_id,
physicsClientId=self._pybullet_client_id,
)[0].decode("UTF-8"): -1,
}
for joint_idx in range(
pybullet.getNumJoints(
self.finger_id,
physicsClientId=self._pybullet_client_id,
)
):
link_name = pybullet.getJointInfo(
self.finger_id,
joint_idx,
physicsClientId=self._pybullet_client_id,
)[12].decode("UTF-8")
link_name_to_index[link_name] = joint_idx
self.pybullet_link_indices = [
link_name_to_index[name] for name in self.link_names
]
self.pybullet_tip_link_indices = [
link_name_to_index[name] for name in self.tip_link_names
]
# joint and link indices are the same in pybullet
self.pybullet_joint_indices = self.pybullet_link_indices
def _check_collisions(self, jpos, gripper_mode='current'):
'''Returns True if collisions present as arm joint position
Parameters
----------
jpos : array_like
positions of arm joints in radians
gripper_mode : {'closed', 'open', 'ignore', 'current'}
how to treat gripper during collision check. if 'current' then the
gripper position will not be changed
Returns
-------
bool
True if collision was detected
dict
data about collision
'''
self._teleport_arm(jpos)
if gripper_mode == 'open':
self._teleport_gripper(state=GRIPPER_OPENED)
elif gripper_mode == 'closed':
self._teleport_gripper(state=GRIPPER_CLOSED)
# ignore base because it doesnt move and will likely be in collision
# with camera at all times
ignored_links = ['base']
if gripper_mode == 'ignore':
ignored_links.extend(['gripper_left', 'gripper_right'])
pb.performCollisionDetection(self._client)
contact_points = pb.getContactPoints(bodyA=self.robot_id,
physicsClientId=self._client)
for cont_pt in contact_points:
assert cont_pt[1] == self.robot_id
robot_link = cont_pt[3]
robot_link_name = self.link_names[robot_link]
if robot_link_name not in ignored_links:
other_body_id = cont_pt[2]
other_body_name = pb.getBodyInfo(
other_body_id,
physicsClientId=self._client)[1].decode('ascii')
return True, {
'robot_link': robot_link_name,
'other_body': other_body_name
}
return False, {}
def get_link_name(body_unique_id: int, link_index: int):
"""Returns the link name.
Args:
body_unique_id (int): The body unique id, as returned by loadURDF, etc.
link_index (int): Link index or -1 for the base.
Returns:
link_name (str): Name of the link.
"""
if link_index == -1:
link_name = p.getBodyInfo(body_unique_id)[0]
else:
link_name = p.getJointInfo(body_unique_id, link_index)[12]
return link_name.decode('UTF-8')
def construct_tables(robot):
# construct tables
# table (joint_name -> id) (link_name -> id)
link_table = {pb.getBodyInfo(robot, physicsClientId=CLIENT)[0].decode('UTF-8'):-1}
joint_table = {}
heck = lambda path: "_".join(path.split("/"))
for _id in range(pb.getNumJoints(robot, physicsClientId=CLIENT)):
joint_info = pb.getJointInfo(robot, _id, physicsClientId=CLIENT)
joint_id = joint_info[0]
joint_name = joint_info[1].decode('UTF-8')
joint_table[joint_name] = joint_id
name_ = joint_info[12].decode('UTF-8')
name = heck(name_)
link_table[name] = _id
return link_table, joint_table
def get_link_from_joint(robot_id):
"""Summary
Args:
robot_id (TYPE): Description
Returns:
TYPE: Description
"""
_link_name_to_index = {
p.getBodyInfo(robot_id)[0].decode('UTF-8'): -1,
}
for _id in range(p.getNumJoints(robot_id)):
_name = p.getJointInfo(robot_id, _id)[12].decode('UTF-8')
_link_name_to_index[_name] = _id
return _link_name_to_index
def construct_tables(robot):
# construct tables
# table (joint_name -> id) (link_name -> id)
link_table = {pb.getBodyInfo(robot)[
0].decode('UTF-8'): -1}
joint_table = {}
def heck(path): return "_".join(path.split("/"))
for _id in range(pb.getNumJoints(robot)):
joint_info = pb.getJointInfo(
robot, _id)
joint_id = joint_info[0]
joint_name = joint_info[1].decode('UTF-8')
joint_table[joint_name] = joint_id
name_ = joint_info[12].decode('UTF-8')
name = heck(name_)
link_table[name] = _id
return link_table, joint_table
def get_robot_config(robot,
initial_pos=None,
initial_quat=None,
b_print_info=False):
nq, nv, na, joint_id, link_id = 0, 0, 0, OrderedDict(), OrderedDict()
link_id[(p.getBodyInfo(robot)[0]).decode("utf-8")] = -1
for i in range(p.getNumJoints(robot)):
info = p.getJointInfo(robot, i)
if info[2] != p.JOINT_FIXED:
joint_id[info[1].decode("utf-8")] = info[0]
link_id[info[12].decode("utf-8")] = info[0]
nq = max(nq, info[3])
nv = max(nv, info[4])
nq += 1
nv += 1
na = len(joint_id)
base_pos, base_quat = p.getBasePositionAndOrientation(robot)
rot_world_com = util.quat_to_rot(base_quat)
initial_pos = [0., 0., 0.] if initial_pos is None else initial_pos
initial_quat = [0., 0., 0., 1.] if initial_quat is None else initial_quat
rot_world_basejoint = util.quat_to_rot(np.array(initial_quat))
pos_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(),
base_pos - np.array(initial_pos))
rot_basejoint_to_basecom = np.dot(rot_world_basejoint.transpose(),
rot_world_com)
if b_print_info:
print("=" * 80)
print("SimulationRobot")
print("nq: ", nq, ", nv: ", nv, ", na: ", na)
print("Vector from base joint frame to base com frame")
print(pos_basejoint_to_basecom)
print("Rotation from base joint frame to base com frame")
print(rot_basejoint_to_basecom)
print("+" * 80)
print("Joint Infos")
util.PrettyPrint(joint_id)
print("+" * 80)
print("Link Infos")
util.PrettyPrint(link_id)
return nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom
orn = e[ORIENTATION] lineFrom = pos mat = p.getMatrixFromQuaternion(orn) dir = [mat[0],mat[3],mat[6]] to = [pos[0]+dir[0]*rayLen,pos[1]+dir[1]*rayLen,pos[2]+dir[2]*rayLen] hit = p.rayTest(lineFrom,to) oldRay = pointRay color = [1,1,0] width = 3 #pointRay = p.addUserDebugLine(lineFrom,to,color,width,lifeTime=1) #if (oldRay>=0): # p.removeUserDebugItem(oldRay) if (hit): #if (hit[0][0]>=0): hitObjectUid = hit[0][0] linkIndex = hit[0][1] if (hitObjectUid>=0): objectInfo = str(hitObjectUid)+" Link Index="+str(linkIndex)+"\nBase Name:"+p.getBodyInfo(hitObjectUid)[0].decode()+"\nBody Info:"+p.getBodyInfo(hitObjectUid)[1].decode() else: objectInfo="None" if e[CONTROLLER_ID] == controllerId: # To make sure we only get the value for one of the remotes #sync the vr pr2 gripper with the vr controller position p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500) relPosTarget = 1 - e[ANALOG] #open/close the gripper, based on analogue p.changeConstraint(pr2_cid2,gearRatio=1, erp=1, relativePositionTarget=relPosTarget, maxForce=3)
to = [pos[0] + dir[0] * rayLen, pos[1] + dir[1] * rayLen, pos[2] + dir[2] * rayLen]
hit = p.rayTest(lineFrom, to)
oldRay = pointRay
color = [1, 1, 0]
width = 3
#pointRay = p.addUserDebugLine(lineFrom,to,color,width,lifeTime=1)
#if (oldRay>=0):
# p.removeUserDebugItem(oldRay)
if (hit):
#if (hit[0][0]>=0):
hitObjectUid = hit[0][0]
linkIndex = hit[0][1]
if (hitObjectUid >= 0):
objectInfo = str(hitObjectUid) + " Link Index=" + str(
linkIndex) + "\nBase Name:" + p.getBodyInfo(hitObjectUid)[0].decode(
) + "\nBody Info:" + p.getBodyInfo(hitObjectUid)[1].decode()
else:
objectInfo = "None"
if e[
CONTROLLER_ID] == controllerId: # To make sure we only get the value for one of the remotes
#sync the vr pr2 gripper with the vr controller position
p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
relPosTarget = 1 - e[ANALOG]
#open/close the gripper, based on analogue
p.changeConstraint(pr2_cid2,
gearRatio=1,
erp=1,
relativePositionTarget=relPosTarget,
maxForce=3)
useNullSpace = 0
count = 0
useOrientation = 1
useSimulation = 1
useRealTimeSimulation = 1
p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
trailDuration = 15
logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS,"LOG0002.txt",bodyUniqueIdA=2)
for i in xrange(5):
print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1])
while 1:
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second/60.)*2.*math.pi
else:
t=t+0.1
if (useSimulation and useRealTimeSimulation==0):
p.stepSimulation()
for i in range (1):
pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0,-math.pi,0])
def get_sphere(x, y, z):
body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"sphere2red_nocol.urdf"), x, y, z)
part_name, _ = p.getBodyInfo(body, 0)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(part_name, bodies, 0, -1)