def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table1, Point(y=+0.5))
table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table2, Point(y=-0.5))
tables = [table1, table2]
#set_euler(table1, Euler(yaw=np.pi/2))
with HideOutput():
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
block_z = stable_z(block1, table1)
set_point(block1, Point(y=-0.5, z=block_z))
block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))
block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))
blocks = [block1, block2, block3]
set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z))
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)),
top_offset=0.02, grasp_length=0.03, under=False)[1:2]
for grasp in grasps:
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user()
wait_for_user('Finish?')
disconnect()
def pick_problem(arm='left', grasp_type='side'):
other_arm = get_other_arm(arm)
initial_conf = get_carry_conf(arm, grasp_type)
pr2 = create_pr2()
set_base_values(pr2, (0, -1.2, np.pi / 2))
set_arm_conf(pr2, arm, initial_conf)
open_arm(pr2, arm)
set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
close_arm(pr2, other_arm)
plane = create_floor()
table = load_pybullet(TABLE_URDF)
# table = create_table(height=0.8)
# table = load_pybullet("table_square/table_square.urdf")
box = create_box(.07, .05, .25)
set_point(box, (-0.25, -0.3, TABLE_MAX_Z + .25 / 2))
# set_point(box, (0.2, -0.2, 0.8 + .25/2 + 0.1))
return Problem(robot=pr2,
movable=[box],
arms=[arm],
grasp_types=[grasp_type],
surfaces=[table],
goal_conf=get_pose(pr2),
goal_holding=[(arm, box)])
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-arm', required=True)
parser.add_argument('-grasp', required=True)
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
arm = args.arm
other_arm = get_other_arm(arm)
grasp_type = args.grasp
connect(use_gui=args.viewer)
add_data_path()
with HideOutput():
robot = load_pybullet(DRAKE_PR2_URDF)
set_group_conf(robot, 'torso', [0.2])
set_arm_conf(robot, arm, get_carry_conf(arm, grasp_type))
set_arm_conf(robot, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
#plane = p.loadURDF("plane.urdf")
#table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
table = create_table()
box = create_box(.07, .07, .14)
#create_inverse_reachability(robot, box, table, arm=arm, grasp_type=grasp_type)
create_inverse_reachability2(robot,
box,
table,
arm=arm,
grasp_type=grasp_type)
disconnect()
def main(group=SE3):
connect(use_gui=True)
set_camera_pose(camera_point=SIZE*np.array([1., -1., 1.]))
# TODO: can also create all links and fix some joints
# TODO: SE(3) motion planner (like my SE(3) one) where some dimensions are fixed
obstacle = create_box(w=SIZE, l=SIZE, h=SIZE, color=RED)
#robot = create_cylinder(radius=0.025, height=0.1, color=BLUE)
obstacles = [obstacle]
collision_id, visual_id = create_shape(get_cylinder_geometry(radius=0.025, height=0.1), color=BLUE)
robot = create_flying_body(group, collision_id, visual_id)
body_link = get_links(robot)[-1]
joints = get_movable_joints(robot)
joint_from_group = dict(zip(group, joints))
print(joint_from_group)
#print(get_aabb(robot, body_link))
dump_body(robot, fixed=False)
custom_limits = {joint_from_group[j]: l for j, l in CUSTOM_LIMITS.items()}
# sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits)
# for i in range(10):
# conf = sample_fn()
# set_joint_positions(robot, joints, conf)
# wait_for_user('Iteration: {}'.format(i))
# return
initial_point = SIZE*np.array([-1., -1., 0])
#initial_point = -1.*np.ones(3)
final_point = -initial_point
initial_euler = np.zeros(3)
add_line(initial_point, final_point, color=GREEN)
initial_conf = np.concatenate([initial_point, initial_euler])
final_conf = np.concatenate([final_point, initial_euler])
set_joint_positions(robot, joints, initial_conf)
#print(initial_point, get_link_pose(robot, body_link))
#set_pose(robot, Pose(point=-1.*np.ones(3)))
path = plan_joint_motion(robot, joints, final_conf, obstacles=obstacles,
self_collisions=False, custom_limits=custom_limits)
if path is None:
disconnect()
return
for i, conf in enumerate(path):
set_joint_positions(robot, joints, conf)
point, quat = get_link_pose(robot, body_link)
#euler = euler_from_quat(quat)
euler = intrinsic_euler_from_quat(quat)
print(conf)
print(point, euler)
wait_for_user('Step: {}/{}'.format(i, len(path)))
wait_for_user('Finish?')
disconnect()
def add_box(world, color_name, idx=0, **kwargs):
name = name_from_type(color_name, idx)
# TODO: geometry type
body = create_box(w=0.07,
l=0.07,
h=0.14,
color=COLOR_FROM_NAME[color_name])
world.add(name, body)
# pose2d_on_surface(world, name, COUNTERS[0], **kwargs)
return name
def create_elements_bodies(node_points,
elements,
color=apply_alpha(RED, alpha=1),
diameter=ELEMENT_DIAMETER,
shrink=ELEMENT_SHRINK):
# TODO: could scale the whole environment
# TODO: create a version without shrinking for transit planning
# URDF_USE_IMPLICIT_CYLINDER
element_bodies = []
for (n1, n2) in elements:
p1, p2 = node_points[n1], node_points[n2]
height = max(np.linalg.norm(p2 - p1) - 2 * shrink, 0)
#if height == 0: # Cannot keep this here
# continue
center = (p1 + p2) / 2
# extents = (p2 - p1) / 2
delta = p2 - p1
x, y, z = delta
phi = np.math.atan2(y, x)
theta = np.math.acos(z / np.linalg.norm(delta))
quat = quat_from_euler(Euler(pitch=theta, yaw=phi))
# p1 is z=-height/2, p2 is z=+height/2
# Much smaller than cylinder
# Also faster, C_shape 177.398 vs 400
body = create_box(diameter,
diameter,
height,
color=color,
mass=STATIC_MASS)
set_color(body, color)
set_point(body, center)
set_quat(body, quat)
#dump_body(body, fixed=True)
# Visually, smallest diameter is 2e-3
# The geometries and bounding boxes seem correct though
# TODO: create_cylinder takes in a radius not diameter
#body = create_cylinder(ELEMENT_DIAMETER, height, color=color, mass=STATIC_MASS)
#print('Diameter={:.5f} | Height={:.5f}'.format(ELEMENT_DIAMETER/2., height))
#print(get_aabb_extent(get_aabb(body)).round(6).tolist())
#print(get_visual_data(body))
#print(get_collision_data(body))
#set_point(body, center)
#set_quat(body, quat)
#draw_aabb(get_aabb(body))
element_bodies.append(body)
#wait_for_user()
return element_bodies
def create_object(shape, side=0.1, mass=1, color=BLUE):
# TODO: simulation parameters
# TODO: object dynamics parameters
if shape == 'box':
obj = create_box(w=side, l=side, h=side, mass=mass, color=color)
elif shape == 'sphere':
obj = create_sphere(radius=side, mass=mass, color=color)
elif shape == 'cylinder':
obj = create_cylinder(radius=side, height=side, mass=mass, color=color)
elif shape == 'capsule':
obj = create_capsule(radius=side, height=side, mass=mass, color=color)
else:
raise ValueError(shape)
return obj
def add_block(world, idx=0, **kwargs):
# TODO: automatically produce a unique name
color = 'green'
#block_type = '{}_block'.format(color)
block_type = 'block'.format(color)
#block_type = BLOCK_TEMPLATE.format(BLOCK_SIZES[-1], BLOCK_COLORS[0])
#block_type = 'potted_meat_can'
name = name_from_type(block_type, idx)
#world.add_body(name)
#print(get_aabb_extent(get_aabb(world.get_body(name))))
side = BIG_BLOCK_SIDE
body = create_box(w=side, l=side, h=side, color=COLOR_FROM_NAME[color])
world.add(name, body)
pose2d_on_surface(world, name, COUNTERS[0], **kwargs)
return name
def add_walls(ros_world):
thickness = 0.01 # 0.005 | 0.01 | 0.02
height = 0.11 # 0.11 | 0.12
[table_name] = ros_world.perception.get_surfaces()
#table_body = ros_world.get_body(table_name)
table_info = ros_world.perception.info_from_name[table_name]
#draw_pose(table_info.pose, length=1)
with ros_world:
#aabb = aabb_from_points(apply_affine(table_info.pose, table_info.type.vertices))
aabb = aabb_from_points(table_info.type.vertices)
draw_aabb(aabb)
# pose = get_pose(table_body)
# pose = ros_world.get_pose(table_name)
# aabb = approximate_as_prism(table_body, pose)
x, y, z = get_aabb_center(aabb)
l, w, h = get_aabb_extent(aabb)
#right_wall = create_box(l, thickness, height)
#set_pose(right_wall, multiply(table_info.pose, (Pose(point=[x, y - (w + thickness) / 2., z + (h + height) / 2.]))))
#bottom_wall = create_box(thickness, w / 2., height)
#set_point(bottom_wall, [x - (l + thickness) / 2., y - w / 4., z + (h + height) / 2.])
top_wall = create_box(thickness, w, height)
set_pose(
top_wall,
multiply(table_info.pose, (Pose(
point=[x + (l + thickness) / 2., y, z + (h + height) / 2.]))))
walls = [top_wall]
#walls = [right_wall, top_wall] # , bottom_wall]
for i, body in enumerate(walls):
name = create_name('wall', i + 1)
ros_world.perception.sim_bodies[name] = body
ros_world.perception.sim_items[name] = None
#wait_for_user()
return walls # Return names?
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()
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5):
floor_extent = 5.0
base_limits = (-floor_extent/2.*np.ones(2),
+floor_extent/2.*np.ones(2))
floor_height = 0.001
floor = create_box(floor_extent, floor_extent, floor_height, color=TAN)
set_point(floor, Point(z=-floor_height/2.))
wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY)
set_point(wall1, Point(y=floor_extent/2., z=wall_side/2.))
wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY)
set_point(wall2, Point(y=-floor_extent/2., z=wall_side/2.))
wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY)
set_point(wall3, Point(x=floor_extent/2., z=wall_side/2.))
wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY)
set_point(wall4, Point(x=-floor_extent/2., z=wall_side/2.))
walls = [wall1, wall2, wall3, wall4]
initial_surfaces = OrderedDict()
for _ in range(n_obstacles):
body = create_box(obst_width, obst_width, obst_height, color=GREY)
initial_surfaces[body] = floor
obstacles = walls + list(initial_surfaces)
initial_conf = np.array([+floor_extent/3, -floor_extent/3, 3*PI/4])
goal_conf = -initial_conf
with HideOutput():
robot = load_model(TURTLEBOT_URDF)
base_joints = joints_from_names(robot, BASE_JOINTS)
# base_link = child_link_from_joint(base_joints[-1])
base_link = link_from_name(robot, BASE_LINK_NAME)
set_all_color(robot, BLUE)
dump_body(robot)
set_point(robot, Point(z=stable_z(robot, floor)))
draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5)
set_joint_positions(robot, base_joints, initial_conf)
sample_placements(initial_surfaces, obstacles=[robot] + walls,
savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)],
min_distances=10e-2)
return robot, base_limits, goal_conf, obstacles
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-arm', required=True)
parser.add_argument('-grasp', required=True)
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
arm = args.arm
other_arm = get_other_arm(arm)
grasp_type = args.grasp
connect(use_gui=args.viewer)
add_data_path()
robot = p.loadURDF("models/pr2_description/pr2_fixed_torso.urdf")
set_arm_conf(robot, arm, get_carry_conf(arm, grasp_type))
set_arm_conf(robot, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
#plane = p.loadURDF("plane.urdf")
table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
box = create_box(.07, .05, .15)
create_inverse_reachability(robot, box, table, arm=arm, grasp_type=grasp_type)
disconnect()
def main(num_iterations=10):
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
side = 0.05
block = create_box(w=side, l=side, h=side, color=RED)
start_time = time.time()
with LockRenderer():
with HideOutput():
# TODO: MOVO must be loaded last
robot = load_model(MOVO_URDF, fixed_base=True)
#set_all_color(robot, color=MOVO_COLOR)
assign_link_colors(robot)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
print('Load time: {:.3f}'.format(elapsed_time(start_time)))
dump_body(robot)
#print(get_colliding(robot))
#for arm in ARMS:
# test_close_gripper(robot, arm)
#test_grasps(robot, block)
arm = RIGHT
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_info = MOVO_INFOS[arm]
print_ik_warning(ik_info)
ik_joints = get_ik_joints(robot, ik_info, tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
wait_if_gui('Start?')
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(num_iterations):
conf = sample_fn()
print('Iteration: {}/{} | Conf: {}'.format(i + 1, num_iterations,
np.array(conf)))
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_if_gui()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_if_gui()
test_retraction(robot,
ik_info,
tool_link,
fixed_joints=fixed_joints,
max_time=0.05,
max_candidates=100)
disconnect()
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
#ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB)
#ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box'))
#print(ycb_path)
#load_pybullet(ycb_path)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table = create_table(width=TABLE_WIDTH,
length=TABLE_WIDTH / 2,
height=TABLE_WIDTH / 2,
top_color=TAN,
cylinder=False)
#set_euler(table, Euler(yaw=np.pi/2))
with HideOutput(False):
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(
WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
#robot_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf')
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
#robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper
block1 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=RED)
block_z = stable_z(block1, table)
set_point(block1, Point(z=block_z))
block2 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=GREEN)
set_point(block2, Point(x=+0.25, z=block_z))
block3 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=BLUE)
set_point(block3, Point(x=-0.15, z=block_z))
blocks = [block1, block2, block3]
add_line(Point(x=-TABLE_WIDTH / 2,
z=block_z - BLOCK_SIDE / 2 + EPSILON),
Point(x=+TABLE_WIDTH / 2,
z=block_z - BLOCK_SIDE / 2 + EPSILON),
color=RED)
set_camera_pose(camera_point=Point(y=-1, z=block_z + 1),
target_point=Point(z=block_z))
wait_for_user()
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
y_grasp, x_grasp = get_top_grasps(block1,
tool_pose=unit_pose(),
grasp_length=0.03,
under=False)
grasp = y_grasp # fingers won't collide
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user('Finish?')
disconnect()
def add_obstacles():
ceiling = create_box(w=0.2, l=0.2, h=0.01, color=RED)
set_point(ceiling, Point(z=0.2))
pole = create_cylinder(radius=0.025, height=0.1, color=RED)
set_point(pole, Point(z=0.15))
def main(floor_width=2.0):
# Creates a pybullet world and a visualizer for it
connect(use_gui=True)
identity_pose = (unit_point(), unit_quat())
origin_handles = draw_pose(
identity_pose, length=1.0
) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE)
# Bodies are described by an integer index
floor = create_box(w=floor_width, l=floor_width, h=0.001,
color=TAN) # Creates a tan box object for the floor
set_point(floor,
[0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor
obstacle = create_box(w=0.5, l=0.5, h=0.1,
color=RED) # Creates a red box obstacle
set_point(
obstacle,
[0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle
print('Position:', get_point(obstacle))
set_euler(obstacle,
[0, 0, np.pi / 4
]) # Sets the [roll,pitch,yaw] orientation of the obstacle
print('Orientation:', get_euler(obstacle))
with LockRenderer(
): # Temporarily prevents the renderer from updating for improved loading efficiency
with HideOutput(): # Temporarily suppresses pybullet output
robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file
robot_z = stable_z(
robot, floor
) # Returns the z offset required for robot to be placed on floor
set_point(robot,
[0, 0, robot_z]) # Sets the z position of the robot
dump_body(robot) # Prints joint and link information about robot
set_all_static()
# Joints are also described by an integer index
# The turtlebot has explicit joints representing x, y, theta
x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x'
y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y'
theta_joint = joint_from_name(
robot, 'theta') # Looks up the robot joint named 'theta'
joints = [x_joint, y_joint, theta_joint]
base_link = link_from_name(
robot, 'base_link') # Looks up the robot link named 'base_link'
world_from_obstacle = get_pose(
obstacle
) # Returns the pose of the origin of obstacle wrt the world frame
obstacle_aabb = get_subtree_aabb(obstacle)
draw_aabb(obstacle_aabb)
random.seed(0) # Sets the random number generator state
handles = []
for i in range(10):
for handle in handles:
remove_debug(handle)
print('\nIteration: {}'.format(i))
x = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, x_joint,
x) # Sets the current value of the x joint
y = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, y_joint,
y) # Sets the current value of the y joint
yaw = random.uniform(-np.pi, np.pi)
set_joint_position(robot, theta_joint,
yaw) # Sets the current value of the theta joint
values = get_joint_positions(
robot,
joints) # Obtains the current values for the specified joints
print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values))
world_from_robot = get_link_pose(
robot,
base_link) # Returns the pose of base_link wrt the world frame
position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion)
x, y, z = position # Decomposing position into x, y, z
print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format(
x, y, z))
euler = euler_from_quat(
quaternion) # Converting from quaternion to euler angles
roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw
print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'.
format(roll, pitch, yaw))
handles.extend(
draw_pose(world_from_robot, length=0.5)
) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE)
obstacle_from_robot = multiply(
invert(world_from_obstacle),
world_from_robot) # Relative transformation from robot to obstacle
robot_aabb = get_subtree_aabb(
robot,
base_link) # Computes the robot's axis-aligned bounding box (AABB)
lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema
center = (lower + upper) / 2. # Computing the center of the AABB
extent = upper - lower # Computing the dimensions of the AABB
handles.extend(draw_aabb(robot_aabb))
collision = pairwise_collision(
robot, obstacle
) # Checks whether robot is currently colliding with obstacle
print('Collision: {}'.format(collision))
wait_for_duration(1.0) # Like sleep() but also updates the viewer
wait_for_user() # Like raw_input() but also updates the viewer
# Destroys the pybullet world
disconnect()
def eval_task_with_seed(domain,
seed,
learner,
sample_strategy=DIVERSELK,
task_lengthscale=None,
obstacle=True):
if task_lengthscale is not None:
learner.task_lengthscale = task_lengthscale
print('sample a new task')
current_wd, trial_wd = start_task()
### fill in additional object
item_ranges = {}
###
sim_world, collector, task, feature, evalfunc, saver = sample_task_with_seed(
domain.skill, seed=seed, visualize=False, item_ranges=item_ranges)
context = domain.context_from_feature(feature)
print('context=', *context)
# create coffee machine
if obstacle:
bowl_name = 'bowl'
cup_name = 'cup'
if domain.skill == 'scoop':
cup_name = 'spoon'
for key in sim_world.perception.sim_bodies:
if bowl_name in key:
bowl_name = key
if cup_name in key:
cup_name = key
cylinder_size = (.01, .03)
dim_bowl = get_aabb_extent(
p.getAABB(sim_world.perception.sim_bodies[bowl_name]))
dim_cup = get_aabb_extent(
p.getAABB(sim_world.perception.sim_bodies[cup_name]))
bowl_point = get_point(sim_world.perception.sim_bodies[bowl_name])
bowl_point = np.array(bowl_point)
cylinder_point = bowl_point.copy()
cylinder_offset = 1.2
if domain.skill == 'scoop':
cylinder_offset = 1.6
cylinder_point[2] += (dim_bowl[2] + dim_cup[2]) * (np.random.random_sample() * .4 + cylinder_offset) \
+ cylinder_size[1] / 2.
# TODO figure out the task space
cylinder_name = create_name('cylinder', 1)
obstacle = create_cylinder(*cylinder_size, color=(0, 0, 0, 1))
INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(cylinder_name,
cylinder_size, 1, 1)
sim_world.perception.sim_bodies[cylinder_name] = obstacle
sim_world.perception.sim_items[cylinder_name] = None
set_pose(obstacle, (cylinder_point, unit_quat()))
box_name = create_name('box', 1)
box1_size = (max(.17,
dim_bowl[0] / 2 + cylinder_size[0] * 2), 0.01, 0.01)
if domain.skill == 'scoop':
box1_size = (max(.23,
dim_bowl[0] / 2 + cylinder_size[0] * 2 + 0.05),
0.01, 0.01)
obstacle = create_box(*box1_size)
INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box1_size, 1,
1)
sim_world.perception.sim_bodies[box_name] = obstacle
sim_world.perception.sim_items[box_name] = None
box1_point = cylinder_point.copy()
box1_point[2] += cylinder_size[1] / 2 + box1_size[2] / 2
box1_point[0] += box1_size[0] / 2 - cylinder_size[0] * 2
set_pose(obstacle, (box1_point, unit_quat()))
box_name = create_name('box', 2)
box2_size = (0.01, .01,
box1_point[2] - bowl_point[2] + box1_size[2] / 2)
obstacle = create_box(*box2_size)
INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box2_size, 1,
1)
sim_world.perception.sim_bodies[box_name] = obstacle
sim_world.perception.sim_items[box_name] = None
box2_point = bowl_point.copy()
box2_point[2] += box2_size[2] / 2
box2_point[0] = box1_point[0] + box1_size[0] / 2
set_pose(obstacle, (box2_point, unit_quat()))
result = get_sample_strategy_result(sample_strategy, learner, context,
sim_world, collector, task, feature,
evalfunc, saver)
print('context=', *context)
complete_task(sim_world, current_wd, trial_wd)
return result
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5):
floor_extent = 5.0
base_limits = (-floor_extent / 2. * np.ones(2),
+floor_extent / 2. * np.ones(2))
floor_height = 0.001
floor = create_box(floor_extent, floor_extent, floor_height, color=TAN)
set_point(floor, Point(z=-floor_height / 2.))
wall1 = create_box(floor_extent + wall_side,
wall_side,
wall_side,
color=GREY)
set_point(wall1, Point(y=floor_extent / 2., z=wall_side / 2.))
wall2 = create_box(floor_extent + wall_side,
wall_side,
wall_side,
color=GREY)
set_point(wall2, Point(y=-floor_extent / 2., z=wall_side / 2.))
wall3 = create_box(wall_side,
floor_extent + wall_side,
wall_side,
color=GREY)
set_point(wall3, Point(x=floor_extent / 2., z=wall_side / 2.))
wall4 = create_box(wall_side,
floor_extent + wall_side,
wall_side,
color=GREY)
set_point(wall4, Point(x=-floor_extent / 2., z=wall_side / 2.))
wall5 = create_box(obst_width, obst_width, obst_height, color=GREY)
set_point(wall5, Point(z=obst_height / 2.))
walls = [wall1, wall2, wall3, wall4, wall5]
initial_surfaces = OrderedDict()
for _ in range(n_obstacles - 1):
body = create_box(obst_width, obst_width, obst_height, color=GREY)
initial_surfaces[body] = floor
pillars = list(initial_surfaces)
obstacles = walls + pillars
initial_conf = np.array([+floor_extent / 3, -floor_extent / 3, 3 * PI / 4])
goal_conf = -initial_conf
robot = load_pybullet(TURTLEBOT_URDF,
fixed_base=True,
merge=True,
sat=False)
base_joints = joints_from_names(robot, BASE_JOINTS)
# base_link = child_link_from_joint(base_joints[-1])
base_link = link_from_name(robot, BASE_LINK_NAME)
set_all_color(robot, BLUE)
dump_body(robot)
set_point(robot, Point(z=stable_z(robot, floor)))
#set_point(robot, Point(z=base_aligned_z(robot)))
draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5)
set_joint_positions(robot, base_joints, initial_conf)
sample_placements(
initial_surfaces,
obstacles=[robot] + walls,
savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)],
min_distances=10e-2)
# The first calls appear to be the slowest
# times = []
# for body1, body2 in combinations(pillars, r=2):
# start_time = time.time()
# colliding = pairwise_collision(body1, body2)
# runtime = elapsed_time(start_time)
# print(colliding, runtime)
# times.append(runtime)
# print(times)
return robot, base_limits, goal_conf, obstacles
def main(use_turtlebot=True):
parser = argparse.ArgumentParser()
parser.add_argument('-sim', action='store_true')
parser.add_argument('-video', action='store_true')
args = parser.parse_args()
video = 'video.mp4' if args.video else None
connect(use_gui=True, mp4=video)
#set_renderer(enable=False)
# print(list_pybullet_data())
# print(list_pybullet_robots())
draw_global_system()
set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5),
target_point=Point(-1.5, +1.5, 0))
plane = load_plane()
#door = load_pybullet('models/door.urdf', fixed_base=True) # From drake
#set_point(door, Point(z=-.1))
door = create_door()
#set_position(door, z=base_aligned_z(door))
set_point(door, base_aligned(door))
#set_collision_margin(door, link=0, margin=0.)
set_configuration(door, [math.radians(-5)])
dump_body(door)
door_joint = get_movable_joints(door)[0]
door_link = child_link_from_joint(door_joint)
#draw_pose(get_com_pose(door, door_link), parent=door, parent_link=door_link)
draw_pose(Pose(), parent=door, parent_link=door_link)
wait_if_gui()
##########
start_x = +2
target_x = -start_x
if not use_turtlebot:
side = 0.25
robot = create_box(w=side, l=side, h=side, mass=5., color=BLUE)
set_position(robot, x=start_x)
#set_velocity(robot, linear=Point(x=-1))
else:
turtlebot_urdf = os.path.abspath(TURTLEBOT_URDF)
print(turtlebot_urdf)
#print(read(turtlebot_urdf))
robot = load_pybullet(turtlebot_urdf, merge=True, fixed_base=True)
robot_joints = get_movable_joints(robot)[:3]
set_joint_positions(robot, robot_joints, [start_x, 0, PI])
set_all_color(robot, BLUE)
set_position(robot, z=base_aligned_z(robot))
dump_body(robot)
##########
set_renderer(enable=True)
#test_door(door)
if args.sim:
test_simulation(robot, target_x, video)
else:
assert use_turtlebot # TODO: extend to the block
test_kinematic(robot, door, target_x)
disconnect()
def main():
# TODO: teleporting kuka arm
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
client = connect(use_gui=args.viewer)
add_data_path()
#planeId = p.loadURDF("plane.urdf")
table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
box = create_box(.07, .05, .15)
# boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
#pr2 = p.loadURDF("pr2_description/pr2.urdf")
pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf")
#pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",)
#useFixedBase=0,)
#flags=p.URDF_USE_SELF_COLLISION)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
#pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False)
initially_colliding = get_colliding_links(pr2)
print len(initially_colliding)
origin = (0, 0, 0)
print p.getNumConstraints()
# TODO: no way of controlling the base position by itself
# TODO: PR2 seems to collide with itself frequently
# real_time = False
# p.setRealTimeSimulation(real_time)
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# while True:
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# if not real_time:
# p.stepSimulation()
# A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints.
# GetRigidlyAttachedLinks
print pr2
# for i in range (10000):
# p.stepSimulation()
# time.sleep(1./240.)
#print get_joint_names(pr2)
print[get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)]
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER))
print self_collision(pr2)
"""
print p.getNumConstraints()
constraint = fixed_constraint(pr2, -1, box, -1) # table
p.changeConstraint(constraint)
print p.getNumConstraints()
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
p.stepSimulation()
raw_input('Continue?')
set_point(pr2, (-2, 0, 0))
p.stepSimulation()
p.changeConstraint(constraint)
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
raw_input('Continue?')
print get_point(pr2)
raw_input('Continue?')
"""
# TODO: would be good if we could set the joint directly
print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME),
0.2) # Updates automatically
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#return
left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES]
print set_joint_positions(
pr2, left_joints,
TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
print set_joint_positions(
pr2, right_joints,
REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM
print get_body_name(pr2)
print get_body_names()
# print p.getBodyUniqueId(pr2)
print get_joint_names(pr2)
#for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM):
# set_joint_position(pr2, joint, value)
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# #print name, joint, get_joint_position(pr2, joint), value
# print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint)
# set_joint_position(pr2, joint, value)
# #print name, joint, get_joint_position(pr2, joint), value
# for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM):
# set_joint_position(pr2, joint_from_name(pr2, name), value)
print p.getNumJoints(pr2)
jointId = 0
print p.getJointInfo(pr2, jointId)
print p.getJointState(pr2, jointId)
# for i in xrange(10):
# #lower, upper = BASE_LIMITS
# #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower
# q = np.random.uniform(*BASE_LIMITS)
# theta = np.random.uniform(*REVOLUTE_LIMITS)
# quat = z_rotation(theta)
# print q, theta, quat, env_collision(pr2)
# #set_point(pr2, q)
# set_pose(pr2, q, quat)
# #p.getMouseEvents()
# #p.getKeyboardEvents()
# raw_input('Continue?') # Stalls because waiting for input
#
# # TODO: self collisions
# for i in xrange(10):
# for name in LEFT_JOINT_NAMES:
# joint = joint_from_name(pr2, name)
# value = np.random.uniform(*get_joint_limits(pr2, joint))
# set_joint_position(pr2, joint, value)
# raw_input('Continue?')
#start = (-2, -2, 0)
#set_base_values(pr2, start)
# #start = get_base_values(pr2)
# goal = (2, 2, 0)
# p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText
# print start, goal
# raw_input('Plan?')
# path = plan_base_motion(pr2, goal)
# print path
# if path is None:
# return
# print len(path)
# for bq in path:
# set_base_values(pr2, bq)
# raw_input('Continue?')
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# for joint in left_joints:
# print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \
# is_circular(pr2, joint), get_joint_position(pr2, joint)
#
# #goal = np.zeros(len(left_joints))
# goal = []
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# goal.append(wrap_joint(pr2, joint, value))
#
# path = plan_joint_motion(pr2, left_joints, goal)
# print path
# for q in path:s
# set_joint_positions(pr2, left_joints, q)
# raw_input('Continue?')
print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6
movable_joints = get_movable_joints(pr2)
print len(movable_joints)
for joint in xrange(get_num_joints(pr2)):
if is_movable(pr2, joint):
print joint, get_joint_name(pr2, joint), get_joint_type(
pr2, joint), get_joint_limits(pr2, joint)
#joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
#set_joint_positions(pr2, joints, sample_joints(pr2, joints))
#print get_joint_positions(pr2, joints) # Need to print before the display updates?
# set_base_values(pr2, (1, -1, -np.pi/4))
# movable_joints = get_movable_joints(pr2)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# print gripper_pose
# print get_joint_positions(pr2, movable_joints)
# p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0))
# p.stepSimulation()
# raw_input('Pre2 IK')
# set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
# print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Pre IK')
# conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons
# print conf
# print get_joint_positions(pr2, movable_joints)
# set_joint_positions(pr2, movable_joints, conf)
# print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Post IK')
# return
# print pose_from_tform(TOOL_TFORM)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #gripper_pose = multiply(gripper_pose, TOOL_POSE)
# #gripper_pose = get_gripper_pose(pr2)
# for i, grasp_pose in enumerate(get_top_grasps(box)):
# grasp_pose = multiply(TOOL_POSE, grasp_pose)
# box_pose = multiply(gripper_pose, grasp_pose)
# set_pose(box, *box_pose)
# print get_pose(box)
# raw_input('Grasp {}'.format(i))
# return
torso = joint_from_name(pr2, TORSO_JOINT_NAME)
torso_point, torso_quat = get_link_pose(pr2, torso)
#torso_constraint = p.createConstraint(pr2, torso, -1, -1,
# p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED
# parentFramePosition=torso_point,
# childFramePosition=torso_quat)
create_inverse_reachability(pr2, box, table)
ir_database = load_inverse_reachability()
print len(ir_database)
return
link = link_from_name(pr2, LEFT_ARM_LINK)
point, quat = get_link_pose(pr2, link)
print point, quat
p.addUserDebugLine(origin, point,
lineColorRGB=(1, 1, 0)) # addUserDebugText
raw_input('Continue?')
current_conf = get_joint_positions(pr2, movable_joints)
#ik_conf = p.calculateInverseKinematics(pr2, link, point)
#ik_conf = p.calculateInverseKinematics(pr2, link, point, quat)
min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints]
max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints]
max_velocities = [
get_max_velocity(pr2, joint) for joint in movable_joints
] # Range of Jacobian
print min_limits
print max_limits
print max_velocities
ik_conf = p.calculateInverseKinematics(pr2,
link,
point,
quat,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=max_velocities,
restPoses=current_conf)
value_from_joint = dict(zip(movable_joints, ik_conf))
print[value_from_joint[joint] for joint in joints]
#print len(ik_conf), ik_conf
set_joint_positions(pr2, movable_joints, ik_conf)
#print len(movable_joints), get_joint_positions(pr2, movable_joints)
print get_joint_positions(pr2, joints)
raw_input('Finish?')
p.disconnect()
