def create_gripper(robot, arm):
# gripper = load_pybullet(os.path.join(get_data_path(), 'pr2_gripper.urdf'))
# gripper = load_pybullet(os.path.join(get_models_path(), 'pr2_description/pr2_l_gripper.urdf'), fixed_base=False)
links = get_link_subtree(robot, link_from_name(robot, GRIPPER_LINKS[arm]))
#print([get_link_name(robot, link) for link in links])
gripper = clone_body(robot, links=links, visual=True,
collision=True) # TODO: joint limits
return gripper
def test_clone_arm(pr2):
first_joint_name = PR2_GROUPS['left_arm'][0]
first_joint = joint_from_name(pr2, first_joint_name)
parent_joint = get_link_parent(pr2, first_joint)
print(get_link_name(pr2, parent_joint), parent_joint, first_joint_name,
first_joint)
print(get_link_descendants(pr2,
first_joint)) # TODO: least common ancestor
links = [first_joint] + get_link_descendants(pr2, first_joint)
new_arm = clone_body(pr2, links=links, collision=False)
dump_world()
set_base_values(pr2, (-2, 0, 0))
wait_for_interrupt()
def create_gripper(robot, visual=False):
gripper_link = link_from_name(robot, get_gripper_link(robot))
links = get_link_descendants(robot, gripper_link) # get_link_descendants | get_link_subtree
with LockRenderer():
# Actually uses the parent of the first link
gripper = clone_body(robot, links=links, visual=False, collision=True) # TODO: joint limits
if not visual:
for link in get_all_links(gripper):
set_color(gripper, np.zeros(4), link)
#dump_body(robot)
#dump_body(gripper)
#user_input()
return gripper
def __init__(self, robot, ee_link, tool_link, color=TRANSPARENT, **kwargs):
self.robot = robot
self.ee_link = ee_link
self.tool_link = tool_link
self.tool_from_ee = get_relative_pose(self.robot, self.ee_link,
self.tool_link)
tool_links = get_link_subtree(robot, self.ee_link)
self.body = clone_body(robot,
links=tool_links,
visual=False,
collision=True,
**kwargs)
set_static(self.body)
if color is not None:
for link in get_all_links(self.body):
set_color(self.body, color, link) # None = white
def test_clone_robot(pr2):
# TODO: j toggles frames, p prints timings, w is wire, a is boxes
new_pr2 = clone_body(pr2, visual=True, collision=False)
#new_pr2 = clone_body_editor(pr2, visual=True, collision=True)
dump_world()
#print(load_srdf_collisions())
#print(load_dae_collisions())
# TODO: some unimportant quats are off for both URDF and other
# TODO: maybe all the frames are actually correct when I load things this way?
# TODO: the visual geometries are off but not the collision frames?
"""
import numpy as np
for link in get_links(pr2):
if not get_visual_data(new_pr2, link): # pybullet.error: Error receiving visual shape info?
continue
#print(get_link_name(pr2, link))
data1 = VisualShapeData(*get_visual_data(pr2, link)[0])
data2 = VisualShapeData(*get_visual_data(new_pr2, link)[0])
pose1 = (data1.localVisualFrame_position, data1.localVisualFrame_orientation)
pose2 = (data2.localVisualFrame_position, data2.localVisualFrame_orientation)
#pose1 = get_link_pose(pr2, link) # Links are fine
#pose2 = get_link_pose(new_pr2, link)
#pose1 = get_joint_parent_frame(pr2, link)
#pose2 = get_joint_parent_frame(new_pr2, link)
#pose1 = get_joint_inertial_pose(pr2, link) # Inertia is fine
#pose2 = get_joint_inertial_pose(new_pr2, link)
if not np.allclose(pose1[0], pose2[0], rtol=0, atol=1e-3):
print('Point', get_link_name(pr2, link), link, pose1[0], pose2[0])
#print(data1)
#print(data2)
#print(get_joint_parent_frame(pr2, link), get_joint_parent_frame(new_pr2, link))
print(get_joint_inertial_pose(pr2, link)) #, get_joint_inertial_pose(new_pr2, link))
print()
if not np.allclose(euler_from_quat(pose1[1]), euler_from_quat(pose2[1]), rtol=0, atol=1e-3):
print('Quat', get_link_name(pr2, link), link, euler_from_quat(pose1[1]), euler_from_quat(pose2[1]))
joint_info1 = get_joint_info(pr2, link)
joint_info2 = get_joint_info(new_pr2, link)
# TODO: the axis is off for some of these
if not np.allclose(joint_info1.jointAxis, joint_info2.jointAxis, rtol=0, atol=1e-3):
print('Axis', get_link_name(pr2, link), link, joint_info1.jointAxis, joint_info2.jointAxis)
"""
set_base_values(new_pr2, (2, 0, 0))
wait_for_interrupt()
def _load_bodies(self, real_world):
with ClientSaver(self.client):
assert not self.bodies
#for real_body in self.bodies.values():
# remove_body(real_body)
self.bodies = {}
self.initial_poses = {}
with HideOutput():
for name, real_body in real_world.perception.sim_bodies.items(
):
if name in real_world.perception.get_surfaces():
self.add_body(name, fixed=True)
elif name in real_world.perception.get_items():
with real_world:
model_info = get_model_info(real_body)
# self.bodies[item] = clone_body(real_world.perception.get_body(item), client=self.client)
if 'wall' in name:
with real_world:
self.bodies[name] = clone_body(
real_body, client=self.client)
elif 'cylinder' in name:
self.bodies[name] = create_cylinder(
*model_info.path)
elif 'box' in name:
self.bodies[name] = create_box(*model_info.path)
elif model_info is None:
self.add_body(name, fixed=False)
else:
self.bodies[name] = load_model_info(model_info)
else:
raise ValueError(name)
# TODO: the floor might not be added which causes the indices to misalign
self.body_mapping[self.bodies[name]] = real_body
if name not in self.get_held():
self.initial_poses[name] = Pose(
real_world.perception.get_pose(name))
self.set_initial()
