def create_inverse_reachability(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500):
tool_link = get_gripper_link(robot, arm)
robot_saver = BodySaver(robot)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
start_time = time.time()
while len(gripper_from_base_list) < num_samples:
box_pose = sample_placement(body, table)
set_pose(body, box_pose)
grasp_pose = random.choice(grasps)
gripper_pose = multiply(box_pose, invert(grasp_pose))
for attempt in range(max_attempts):
robot_saver.restore()
base_conf = next(uniform_pose_generator(robot, gripper_pose)) #, reachable_range=(0., 1.)))
#set_base_values(robot, base_conf)
set_group_conf(robot, 'base', base_conf)
if pairwise_collision(robot, table):
continue
grasp_conf = pr2_inverse_kinematics(robot, arm, gripper_pose) #, nearby_conf=USE_CURRENT)
#conf = inverse_kinematics(robot, link, gripper_pose)
if (grasp_conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot))
#wait_if_gui()
gripper_from_base_list.append(gripper_from_base)
print('{} / {} | {} attempts | [{:.3f}]'.format(
len(gripper_from_base_list), num_samples, attempt, elapsed_time(start_time)))
wait_if_gui()
break
else:
print('Failed to find a kinematic solution after {} attempts'.format(max_attempts))
return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
def get_grasps(world, name, grasp_types=GRASP_TYPES, pre_distance=APPROACH_DISTANCE, **kwargs):
use_width = world.robot_name == FRANKA_CARTER
body = world.get_body(name)
#fraction = 0.25
obj_type = type_from_name(name)
body_pose = REFERENCE_POSE.get(obj_type, unit_pose())
center, extent = approximate_as_prism(body, body_pose)
for grasp_type in grasp_types:
if not implies(world.is_real(), is_valid_grasp_type(name, grasp_type)):
continue
#assert is_valid_grasp_type(name, grasp_type)
if grasp_type == TOP_GRASP:
grasp_length = 1.5 * FINGER_EXTENT[2] # fraction = 0.5
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
post_direction = unit_point()
generator = get_top_grasps(body, under=True, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, max_width=np.inf, **kwargs)
elif grasp_type == SIDE_GRASP:
# Take max of height and something
grasp_length = 1.75 * FINGER_EXTENT[2] # No problem if pushing a little
x, z = pre_distance * get_unit_vector([3, -1])
pre_direction = [0, 0, x]
post_direction = [0, 0, z]
top_offset = extent[2] / 2 if obj_type in MID_SIDE_GRASPS else 1.0*FINGER_EXTENT[0]
# Under grasps are actually easier for this robot
# TODO: bug in under in that it grasps at the bottom
generator = get_side_grasps(body, under=False, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, top_offset=top_offset, max_width=np.inf, **kwargs)
# if world.robot_name == FRANKA_CARTER else unit_pose()
generator = (multiply(Pose(euler=Euler(yaw=yaw)), grasp)
for grasp in generator for yaw in [0, np.pi])
else:
raise ValueError(grasp_type)
grasp_poses = randomize(list(generator))
if obj_type in CYLINDERS:
# TODO: filter first
grasp_poses = (multiply(grasp_pose, Pose(euler=Euler(
yaw=random.uniform(-math.pi, math.pi)))) for grasp_pose in cycle(grasp_poses))
for i, grasp_pose in enumerate(grasp_poses):
pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose,
Pose(point=post_direction))
grasp = Grasp(world, name, grasp_type, i, grasp_pose, pregrasp_pose)
with BodySaver(body):
grasp.get_attachment().assign()
with BodySaver(world.robot):
grasp.grasp_width = close_until_collision(
world.robot, world.gripper_joints, bodies=[body])
#print(get_joint_positions(world.robot, world.arm_joints)[-1])
#draw_pose(unit_pose(), parent=world.robot, parent_link=world.tool_link)
#grasp.get_attachment().assign()
#wait_for_user()
##for value in get_joint_limits(world.robot, world.arm_joints[-1]):
#for value in [-1.8973, 0, +1.8973]:
# set_joint_position(world.robot, world.arm_joints[-1], value)
# grasp.get_attachment().assign()
# wait_for_user()
if use_width and (grasp.grasp_width is None):
continue
yield grasp
def sample_placements(body_surfaces,
obstacles=None,
savers=[],
min_distances={}):
if obstacles is None:
obstacles = set(get_bodies()) - set(body_surfaces)
savers = list(savers) + [BodySaver(obstacle) for obstacle in obstacles]
if not isinstance(min_distances, dict):
min_distances = {body: min_distances for body in body_surfaces}
# TODO: max attempts here
for body, surface in body_surfaces.items(): # TODO: shuffle
min_distance = min_distances.get(body, 0.)
while True:
pose = sample_placement(body, surface)
if pose is None:
for saver in savers:
saver.restore()
return False
for saver in savers:
obstacle = saver.body
if obstacle in [body, surface]:
continue
saver.restore()
if pairwise_collision(body,
obstacle,
max_distance=min_distance):
break
else:
savers.append(BodySaver(body))
break
for saver in savers:
saver.restore()
return True
def gen_fn(arm, obj_name, pose):
open_width = get_max_limit(world.robot,
get_gripper_joints(world.robot, arm)[0])
obj_body = world.bodies[obj_name]
#grasps = cycle([(grasp,)])
grasps = cycle(grasp_gen_fn(obj_name))
for attempt in range(max_attempts):
try:
grasp, = next(grasps)
except StopIteration:
break
# TODO: if already successful for a grasp, continue
set_pose(obj_body, pose)
set_gripper_position(world.robot, arm, open_width)
robot_saver = BodySaver(
world.robot) # TODO: robot might be at the wrong conf
body_saver = BodySaver(obj_body)
pretool_pose = multiply(pose, invert(grasp.pregrasp_pose))
tool_pose = multiply(pose, invert(grasp.grasp_pose))
grip_conf = solve_inverse_kinematics(world.robot,
arm,
tool_pose,
obstacles=obstacles)
if grip_conf is None:
continue
#attachment = Attachment(world.robot, tool_link, get_grasp_pose(grasp), world.bodies[obj])
# Attachments cause table collisions
post_path = plan_waypoint_motion(
world.robot,
arm,
pretool_pose,
obstacles=obstacles,
attachments=[], #attachments=[attachment],
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(post_path[-1])
pre_path = post_path[::-1]
post_conf = pre_conf
control = Control({
'action':
'pick',
'objects': [obj_name],
'context':
Context(savers=[robot_saver, body_saver], attachments={}),
'commands': [
ArmTrajectory(arm, pre_path, dialation=2.),
GripperCommand(arm, grasp.grasp_width,
effort=grasp.effort),
Attach(arm, obj_name, effort=grasp.effort),
ArmTrajectory(arm, post_path, dialation=2.),
],
})
yield (grasp, pre_conf, post_conf, control)
def has_grasp(world, name, max_attempts=4):
with BodySaver(world.get_body(name)):
try:
next(get_grasp_gen_fn(world, max_attempts=max_attempts)(name))
except StopIteration:
return False
return True
def update(self,
belief,
observation,
n_samples=25,
verbose=False,
**kwargs):
if verbose:
print('Prior:', self.dist)
body = self.world.get_body(self.name)
obstacles = [
self.world.get_body(name) for name in belief.pose_dists
if name != self.name
]
dists = []
for _ in range(n_samples):
belief.sample(discrete=True) # Trouble if no support
with BodySaver(body):
new_dist = self.update_dist(observation, obstacles, **kwargs)
#new_pose_dist = self.__class__(self.world, self.name, new_dist).resample()
dists.append(new_dist)
#remove_all_debug()
#new_pose_dist.draw(color=belief.color_from_name[self.name])
#wait_for_user()
posterior = mixDDists({dist: 1. / len(dists) for dist in dists})
if verbose:
print('Posterior:', posterior)
pose_dist = self.__class__(self.world, self.name, posterior)
if RESAMPLE:
pose_dist = pose_dist.resample()
return pose_dist
def stabilize(world, duration=0.1):
# TODO: apply to simulated_problems
with ClientSaver(world.client):
world.controller.set_gravity()
with BodySaver(
world.robot): # Otherwise the robot starts in self-collision
world.controller.rest_for_duration(
duration) # Let's the objects stabilize
def extract_full_path(robot, path_joints, path, all_joints):
with BodySaver(robot):
new_path = []
for conf in path:
set_joint_positions(robot, path_joints, conf)
new_path.append(get_joint_positions(
robot, all_joints)) # TODO: do without assigning
return new_path
def bowl_path_collision(bowl_body, body, body_path_bowl):
bowl_pose = get_pose(bowl_body)
with BodySaver(body):
for cup_pose_bowl in body_path_bowl:
cup_pose_world = multiply(bowl_pose, cup_pose_bowl)
set_pose(body, cup_pose_world)
if body_pair_collision(bowl_body, body): #, collision_buffer=0.0):
return True
return False
def fill_with_beads(world, bowl_name, beads, **kwargs):
with LockRenderer(lock=True):
with BodySaver(world.robot):
contained_beads = pour_beads(world, bowl_name, list(beads),
**kwargs)
print('Contained: {} out of {} beads ({:.3f}%)'.format(
len(contained_beads), len(beads),
100 * safe_ratio(len(contained_beads), len(beads), undefined=0)))
return contained_beads
def compute_grasps(world,
name,
grasp_length=(HAND_LENGTH - 0.02),
pre_distance=0.1,
max_attempts=INF):
body = world.get_body(name)
reference_pose = get_reference_pose(name)
# TODO: add z offset in the world frame
pre_direction = pre_distance * TOP_DIRECTION
ty = get_type(name)
if is_obj_type(name, 'block'):
generator = get_top_grasps(body,
under=False,
tool_pose=TOOL_POSE,
body_pose=reference_pose,
grasp_length=grasp_length,
max_width=np.inf)
elif is_obj_type(name, 'cup'):
#pre_direction = pre_distance*get_unit_vector([1, 0, -2]) # -x, -y, -z
pre_direction = pre_distance * get_unit_vector([3, 0, -1
]) # -x, -y, -z
# Cannot pick if top_offset is too large(0.03 <=)
top_offset = 3 * FINGER_WIDTH / 4 if ty in CUP_WITH_LIP else FINGER_WIDTH / 4
generator = get_side_cylinder_grasps(body,
under=False,
tool_pose=TOOL_POSE,
body_pose=reference_pose,
grasp_length=grasp_length,
top_offset=top_offset,
max_width=np.inf)
elif is_obj_type(name, 'stirrer'):
generator = get_top_cylinder_grasps(body,
tool_pose=TOOL_POSE,
body_pose=reference_pose,
grasp_length=grasp_length,
max_width=np.inf)
elif is_obj_type(name, 'bowl'):
generator = get_edge_cylinder_grasps(body,
tool_pose=TOOL_POSE,
body_pose=reference_pose,
grasp_length=0.02)
elif is_obj_type(name, 'spoon'):
generator = get_spoon_grasps(name, body)
else:
raise NotImplementedError(name)
effort = 25 if ty in (OLIVE_CUPS + THREE_D_CUPS) else 50
for index, grasp_pose in enumerate(
islice(generator, None if max_attempts is INF else max_attempts)):
with BodySaver(world.robot):
grasp_width = compute_grasp_width(world.robot, 'left', body,
grasp_pose)
#print(index, grasp_width)
if grasp_width is not None:
yield Grasp(name, index, grasp_pose, pre_direction, grasp_width,
effort)
def get_link_path(self, link_name=TOOL_LINK):
link = link_from_name(self.robot, link_name)
if link not in self.path_from_link:
with BodySaver(self.robot):
self.path_from_link[link] = []
for conf in self.path:
set_joint_positions(self.robot, self.joints, conf)
self.path_from_link[link].append(
get_link_pose(self.robot, link))
return self.path_from_link[link]
def gen_fn(arm, bowl_name, bowl_pose, stirrer_name, grasp):
if bowl_name == stirrer_name:
return
bowl_body = world.bodies[bowl_name]
attachment = get_grasp_attachment(world, arm, grasp)
feature = get_stir_feature(world, bowl_name, stirrer_name)
parameter_generator = parameter_fn(world, feature)
if revisit:
parameter_generator = cycle(parameter_generator)
for parameter in parameter_generator:
for _ in range(max_attempts):
set_gripper_position(world.robot, arm, grasp.grasp_width)
set_pose(bowl_body, bowl_pose)
stirrer_path_bowl = sample_stir_trajectory(
world, feature, parameter)
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
stirrer_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl)
for cup_pose_bowl in stirrer_path_bowl
]
#visualize_cartesian_path(world.get_body(stirrer_name), stirrer_path)
arm_path = plan_attachment_motion(
world.robot,
arm,
attachment,
stirrer_path,
obstacles=set(obstacles) - {bowl_body},
self_collisions=collisions,
disabled_collisions=disabled_collisions,
collision_buffer=collision_buffer,
attachment_collisions=False)
if arm_path is None:
continue
pre_conf = Conf(arm_path[0])
post_conf = Conf(arm_path[-1])
control = Control({
'action':
'stir',
'objects': [bowl_name, stirrer_name],
'feature':
feature,
'parameter':
parameter,
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments={stirrer_name: attachment}),
'commands': [
ArmTrajectory(arm, arm_path),
],
})
yield (pre_conf, post_conf, control)
def gen_fn(arm, button):
gripper_joint = get_gripper_joints(world.robot, arm)[0]
closed_width, open_width = get_joint_limits(world.robot, gripper_joint)
pose = world.initial_poses[button]
body = world.bodies[button]
presses = cycle(get_top_presses(body, tool_pose=TOOL_POSE))
for attempt in range(max_attempts):
try:
press = next(presses)
except StopIteration:
break
set_gripper_position(world.robot, arm, closed_width)
tool_pose = multiply(pose, invert(press))
grip_conf = solve_inverse_kinematics(
world.robot,
arm,
tool_pose,
obstacles=obstacle_bodies,
collision_buffer=collision_buffer)
if grip_conf is None:
continue
pretool_pose = multiply(tool_pose, Pose(point=pre_direction))
post_path = plan_waypoint_motion(
world.robot,
arm,
pretool_pose,
obstacles=obstacle_bodies,
collision_buffer=collision_buffer,
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
post_conf = Conf(post_path[-1])
pre_conf = post_conf
pre_path = post_path[::-1]
control = Control({
'action':
'press',
'objects': [],
'context':
Context( # TODO: robot might be at the wrong conf
savers=[BodySaver(world.robot)
], # TODO: start with open instead
attachments={}),
'commands': [
GripperCommand(arm, closed_width),
ArmTrajectory(arm, pre_path),
ArmTrajectory(arm, post_path),
GripperCommand(arm, open_width),
],
})
yield (pre_conf, post_conf, control)
def get_aabbs(self):
#traj.aabb = aabb_union(map(get_turtle_traj_aabb, traj.iterate())) # TODO: union
if self.aabbs is not None:
return self.aabbs
self.aabbs = []
links = get_all_links(self.robot)
with BodySaver(self.robot):
for conf in self.path:
set_joint_positions(self.robot, self.joints, conf)
self.aabbs.append(
{link: get_aabb(self.robot, link)
for link in links})
return self.aabbs
def fn(bq, *args): #, aq):
# TODO: include if holding anything?
bq.assign()
aq = world.carry_conf
#aq.assign() # TODO: could sample aq instead achieve it by move actions
#world.open_gripper()
robot_saver = BodySaver(world.robot)
cmd = Sequence(
State(world, savers=[robot_saver]),
commands=[
#Trajectory(world, world.robot, world.arm_joints, approach_path),
# TODO: calibrate command
],
name='calibrate')
return (cmd, )
def fn(bq, aq1, aq2, fluents=[]):
#if aq1 == aq2:
# return None
bq.assign()
aq1.assign()
attachments, obstacles = parse_fluents(world, fluents)
obstacles.update(world.static_obstacles)
if not collisions:
obstacles = set()
robot_saver = BodySaver(world.robot)
if teleport:
path = [aq1.values, aq2.values]
else:
path = plan_joint_motion(
world.robot,
aq2.joints,
aq2.values,
attachments=attachments,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=world.disabled_collisions,
custom_limits=world.custom_limits,
resolutions=resolutions,
restarts=2,
iterations=50,
smooth=50)
if path is None:
print('Failed to find an arm motion plan for {}->{}'.format(
aq1, aq2))
if PAUSE_MOTION_FAILURES:
set_renderer(enable=True)
print(fluents)
for bq in [aq1, aq2]:
bq.assign()
wait_for_user()
set_renderer(enable=False)
return None
cmd = Sequence(State(world, savers=[robot_saver]),
commands=[
Trajectory(world, world.robot, world.arm_joints,
path),
],
name='arm')
return (cmd, )
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 get_intersecting(self):
if self.intersecting:
return self.intersecting
robot_links = get_all_links(self.robot)
# TODO: might need call step_simulation
with BodySaver(self.robot):
for conf in self.path:
set_joint_positions(self.robot, self.joints, conf)
intersecting = {}
for robot_link in robot_links:
for body, _ in get_bodies_in_region(
get_aabb(self.robot, link=robot_link)):
if body != self.robot:
intersecting.setdefault(robot_link,
set()).add(body)
#print(get_bodies_in_region(get_aabb(self.robot, robot_link)))
#draw_aabb(get_aabb(self.robot, robot_link))
#wait_for_user()
self.intersecting.append(intersecting)
return self.intersecting
def create_surface_pose_dist(world, obj_name, surface_dist, n=NUM_PARTICLES):
# TODO: likely easier to just make a null surface below ground
placement_gen = get_stable_gen(world,
max_attempts=100,
learned=True,
pos_scale=1e-3,
rot_scale=1e-2)
poses = []
with LockRenderer():
with BodySaver(world.get_body(obj_name)):
while len(poses) < n:
surface_name = surface_dist.sample()
assert surface_name is not ELSEWHERE
result = next(placement_gen(obj_name, surface_name), None)
if result is None:
surface_dist = surface_dist.condition(
lambda s: s != surface_name)
else:
(rel_pose, ) = result
rel_pose.init = True
poses.append(rel_pose)
return PoseDist(world, obj_name, UniformDist(poses))
def gen_fn(arm, obj_name, pose1, region):
# TODO: reachability test here
if region is None:
goals = push_goal_gen_fn(obj_name, pose1, surface)
elif isinstance(region, str):
goals = push_goal_gen_fn(obj_name, pose1, surface, region=region)
else:
goals = [(region,)]
if repeat:
goals = cycle(goals)
arm_joints = get_arm_joints(world.robot, arm)
open_width = get_max_limit(world.robot, get_gripper_joints(world.robot, arm)[0])
body = world.bodies[obj_name]
for goal_pos2d, in islice(goals, max_samples):
pose2 = get_end_pose(pose1, goal_pos2d)
body_path = list(interpolate_poses(pose1, pose2))
if cartesian_path_collision(body, body_path, set(obstacles) - {surface_body}) or \
cartesian_path_unsupported(body, body_path, surface_body):
continue
set_pose(body, pose1)
push_direction = np.array(point_from_pose(pose2)) - np.array(point_from_pose(pose1))
backoff_tform = Pose(-backoff_distance * get_unit_vector(push_direction)) # World coordinates
feature = get_push_feature(world, arm, obj_name, pose1, goal_pos2d)
for parameter in parameter_fn(world, feature):
push_contact = next(iter(sample_push_contact(world, feature, parameter, under=False)))
gripper_path = [multiply(pose, invert(multiply(TOOL_POSE, push_contact))) for pose in body_path]
set_gripper_position(world.robot, arm, open_width)
for _ in range(max_attempts):
start_conf = solve_inverse_kinematics(world.robot, arm, gripper_path[0], obstacles=obstacles)
if start_conf is None:
continue
set_pose(body, pose1)
body_saver = BodySaver(body)
#attachment = create_attachment(world.robot, arm, body)
push_path = plan_waypoint_motion(world.robot, arm, gripper_path[-1],
obstacles=obstacles, #attachments=[attachment],
self_collisions=collisions, disabled_collisions=disabled_collisions)
if push_path is None:
continue
pre_backoff_pose = multiply(backoff_tform, gripper_path[0])
pre_approach_pose = multiply(pre_backoff_pose, approach_tform)
set_joint_positions(world.robot, arm_joints, push_path[0])
pre_path = plan_waypoints_motion(world.robot, arm, [pre_backoff_pose, pre_approach_pose],
obstacles=obstacles, attachments=[],
self_collisions=collisions, disabled_collisions=disabled_collisions)
if pre_path is None:
continue
pre_path = pre_path[::-1]
post_backoff_pose = multiply(backoff_tform, gripper_path[-1])
post_approach_pose = multiply(post_backoff_pose, approach_tform)
set_joint_positions(world.robot, arm_joints, push_path[-1])
post_path = plan_waypoints_motion(world.robot, arm, [post_backoff_pose, post_approach_pose],
obstacles=obstacles, attachments=[],
self_collisions=collisions, disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(pre_path[0])
set_joint_positions(world.robot, arm_joints, pre_conf)
robot_saver = BodySaver(world.robot)
post_conf = Conf(post_path[-1])
control = Control({
'action': 'push',
'objects': [obj_name],
'feature': feature,
'parameter': None,
'context': Context(
savers=[robot_saver, body_saver],
attachments={}),
'commands': [
ArmTrajectory(arm, pre_path),
Push(arm, obj_name),
ArmTrajectory(arm, push_path),
Detach(arm, obj_name),
ArmTrajectory(arm, post_path),
],
})
yield (pose2, pre_conf, post_conf, control)
break
def gen(bowl_name, wp, cup_name, grasp, bq):
# https://github.mit.edu/Learning-and-Intelligent-Systems/ltamp_pr2/blob/d1e6024c5c13df7edeab3a271b745e656a794b02/plan_tools/samplers/pour.py
if bowl_name == cup_name:
return
#attachment = get_grasp_attachment(world, arm, grasp)
bowl_body = world.get_body(bowl_name)
#cup_body = world.get_body(cup_name)
obstacles = (world.static_obstacles
| {bowl_body}) if collisions else set()
cup_path_bowl = pour_path_from_parameter(world, bowl_name, cup_name)
while True:
for _ in range(max_attempts):
bowl_pose = wp.get_world_from_body()
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
rotate_cup = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
cup_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl,
rotate_cup) for cup_pose_bowl in cup_path_bowl
]
#visualize_cartesian_path(cup_body, cup_path)
#if cartesian_path_collision(cup_body, cup_path, obstacles + [bowl_body]):
# continue
tool_path = [
multiply(p, invert(grasp.grasp_pose)) for p in cup_path
]
# TODO: extra collision test for visibility
# TODO: orientation constraint while moving
bq.assign()
grasp.set_gripper()
world.carry_conf.assign()
arm_path = plan_workspace(world,
tool_path,
obstacles,
randomize=True) # tilt to upright
if arm_path is None:
continue
assert MOVE_ARM
aq = FConf(world.robot, world.arm_joints, arm_path[-1])
robot_saver = BodySaver(world.robot)
obj_type = type_from_name(cup_name)
duration = 5.0 if obj_type in [MUSTARD] else 1.0
objects = [bowl_name, cup_name]
cmd = Sequence(State(world, savers=[robot_saver]),
commands=[
ApproachTrajectory(objects, world,
world.robot,
world.arm_joints,
arm_path[::-1]),
Wait(world, duration=duration),
ApproachTrajectory(objects, world,
world.robot,
world.arm_joints,
arm_path),
],
name='pour')
yield (
aq,
cmd,
)
break
else:
yield None
def plan_pull(world,
door_joint,
door_plan,
base_conf,
randomize=True,
collisions=True,
teleport=False,
**kwargs):
door_path, handle_path, handle_plan, tool_path = door_plan
handle_link, handle_grasp, handle_pregrasp = handle_plan
door_obstacles = get_descendant_obstacles(
world.kitchen, door_joint) # if collisions else set()
obstacles = (world.static_obstacles | door_obstacles
) # if collisions else set()
base_conf.assign()
world.open_gripper()
world.carry_conf.assign()
robot_saver = BodySaver(world.robot) # TODO: door_saver?
if not is_pull_safe(world, door_joint, door_plan):
return
arm_path = plan_workspace(world,
tool_path,
world.static_obstacles,
randomize=randomize,
teleport=collisions)
if arm_path is None:
return
approach_paths = []
for index in [0, -1]:
set_joint_positions(world.kitchen, [door_joint], door_path[index])
set_joint_positions(world.robot, world.arm_joints, arm_path[index])
tool_pose = multiply(handle_path[index], invert(handle_pregrasp))
approach_path = plan_approach(world,
tool_pose,
obstacles=obstacles,
teleport=teleport,
**kwargs)
if approach_path is None:
return
approach_paths.append(approach_path)
if MOVE_ARM:
aq1 = FConf(world.robot, world.arm_joints, approach_paths[0][0])
aq2 = FConf(world.robot, world.arm_joints, approach_paths[-1][0])
else:
aq1 = world.carry_conf
aq2 = aq1
set_joint_positions(world.kitchen, [door_joint], door_path[0])
set_joint_positions(world.robot, world.arm_joints, arm_path[0])
grasp_width = close_until_collision(world.robot,
world.gripper_joints,
bodies=[(world.kitchen, [handle_link])
])
gripper_motion_fn = get_gripper_motion_gen(world,
teleport=teleport,
collisions=collisions,
**kwargs)
gripper_conf = FConf(world.robot, world.gripper_joints,
[grasp_width] * len(world.gripper_joints))
finger_cmd, = gripper_motion_fn(world.open_gq, gripper_conf)
objects = []
commands = [
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
approach_paths[0]),
DoorTrajectory(world, world.robot, world.arm_joints, arm_path,
world.kitchen, [door_joint], door_path),
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
reversed(approach_paths[-1])),
]
door_path, _, _, _ = door_plan
sign = world.get_door_sign(door_joint)
pull = (sign * door_path[0][0] < sign * door_path[-1][0])
if pull:
commands.insert(1, finger_cmd.commands[0])
commands.insert(3, finger_cmd.commands[0].reverse())
cmd = Sequence(State(world, savers=[robot_saver]), commands, name='pull')
yield (
aq1,
aq2,
cmd,
)
def plan_pick(world,
obj_name,
pose,
grasp,
base_conf,
obstacles,
randomize=True,
**kwargs):
# TODO: check if within database convex hull
# TODO: flag to check if initially in collision
obj_body = world.get_body(obj_name)
pose.assign()
base_conf.assign()
world.open_gripper()
robot_saver = BodySaver(world.robot)
obj_saver = BodySaver(obj_body)
if randomize:
sample_fn = get_sample_fn(world.robot, world.arm_joints)
set_joint_positions(world.robot, world.arm_joints, sample_fn())
else:
world.carry_conf.assign()
world_from_body = pose.get_world_from_body()
gripper_pose = multiply(world_from_body, invert(
grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1
full_grasp_conf = world.solve_inverse_kinematics(gripper_pose)
if full_grasp_conf is None:
if PRINT_FAILURES: print('Grasp kinematic failure')
return
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
#robot_obstacle = get_descendant_obstacles(world.robot, child_link_from_joint(world.arm_joints[0]))
#robot_obstacle = world.robot
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
if PRINT_FAILURES: print('Grasp collision failure')
#set_renderer(enable=True)
#wait_for_user()
#set_renderer(enable=False)
return
approach_pose = multiply(world_from_body, invert(grasp.pregrasp_pose))
approach_path = plan_approach(
world,
approach_pose, # attachments=[grasp.get_attachment()],
obstacles=obstacles,
**kwargs)
if approach_path is None:
if PRINT_FAILURES: print('Approach plan failure')
return
if MOVE_ARM:
aq = FConf(world.robot, world.arm_joints, approach_path[0])
else:
aq = world.carry_conf
gripper_motion_fn = get_gripper_motion_gen(world, **kwargs)
finger_cmd, = gripper_motion_fn(world.open_gq, grasp.get_gripper_conf())
attachment = create_surface_attachment(world, obj_name, pose.support)
objects = [obj_name]
cmd = Sequence(State(world,
savers=[robot_saver, obj_saver],
attachments=[attachment]),
commands=[
ApproachTrajectory(objects, world, world.robot,
world.arm_joints, approach_path),
finger_cmd.commands[0],
Detach(world, attachment.parent, attachment.parent_link,
attachment.child),
AttachGripper(world, obj_body, grasp=grasp),
ApproachTrajectory(objects, world, world.robot,
world.arm_joints,
reversed(approach_path)),
],
name='pick')
yield (
aq,
cmd,
)
def gen_fn(arm, bowl_name, bowl_pose, cup_name, grasp):
if bowl_name == cup_name:
return
attachment = get_grasp_attachment(world, arm, grasp)
bowl_body = world.get_body(bowl_name)
cup_body = world.get_body(cup_name)
feature = get_pour_feature(world, bowl_name, cup_name)
# TODO: this may be called several times with different grasps
for parameter in parameter_generator(feature):
for i in range(max_attempts):
set_pose(bowl_body,
bowl_pose) # Reset because might have changed
set_gripper_position(world.robot, arm, grasp.grasp_width)
cup_path_bowl, times_from_start = pour_path_from_parameter(
world, feature, parameter)
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
cup_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl)
for cup_pose_bowl in cup_path_bowl
]
#if world.visualize:
# visualize_cartesian_path(cup_body, cup_path)
if cartesian_path_collision(cup_body, cup_path,
obstacles + [bowl_body]):
print('Attempt {}: Pour path collision!'.format(i))
continue
tool_waypoints = [
multiply(p, invert(grasp.grasp_pose)) for p in cup_path
]
grip_conf = solve_inverse_kinematics(world.robot,
arm,
tool_waypoints[0],
obstacles=obstacles)
if grip_conf is None:
continue
if water_robot_collision(world, bowl_body, bowl_pose, cup_body,
cup_path):
print('Attempt {}: Water robot collision'.format(i))
continue
if water_obstacle_collision(world, bowl_body, bowl_pose,
cup_body, cup_path):
print('Attempt {}: Water obstacle collision'.format(i))
continue
post_path = plan_workspace_motion(
world.robot,
arm,
tool_waypoints,
obstacles=obstacles + [bowl_body],
attachments=[attachment],
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(post_path[-1])
pre_path = post_path[::-1]
post_conf = pre_conf
control = Control({
'action':
'pour',
'feature':
feature,
'parameter':
parameter,
'objects': [bowl_name, cup_name],
'times':
times_from_start,
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments={cup_name: attachment}),
'commands': [
ArmTrajectory(arm, pre_path, dialation=2.),
Rest(duration=2.0),
ArmTrajectory(arm, post_path, dialation=2.),
],
})
yield (pre_conf, post_conf, control)
break
else:
yield None
def gen_fn(arm, conf1, conf2, fluents=[]):
arm_confs, object_grasps, object_poses, contains_liquid = parse_fluents(
world, fluents)
for a, q in arm_confs.items():
#print(a, q) # TODO: some optimistic values are getting through
set_joint_positions(world.robot, get_arm_joints(world.robot, a), q)
for name, pose in object_poses.items():
set_pose(world.bodies[name], pose)
obstacle_names = list(world.get_fixed()) + list(object_poses)
obstacles = [world.bodies[name] for name in obstacle_names]
attachments = {}
holding_water = None
water_attachment = None
for arm2, (obj, grasp) in object_grasps.items():
set_gripper_position(world.robot, arm, grasp.grasp_width)
attachment = get_grasp_attachment(world, arm2, grasp)
attachment.assign()
if arm == arm2: # The moving arm
if obj in contains_liquid:
holding_water = obj
water_attachment = attachment
attachments[obj] = attachment
else: # The stationary arm
obstacles.append(world.get_body(obj))
if not collisions:
obstacles = []
# TODO: dynamically adjust step size to be more conservative near longer movements
arm_joints = get_arm_joints(world.robot, arm)
set_joint_positions(world.robot, arm_joints, conf1)
weights, resolutions = get_weights_resolutions(world.robot, arm)
#print(holding_water, attachments, [get_body_name(body) for body in obstacles])
if teleport:
path = [conf1, conf2]
elif holding_water is None:
# TODO(caelan): unify these two methods
path = plan_joint_motion(world.robot,
arm_joints,
conf2,
resolutions=resolutions,
weights=weights,
obstacles=obstacles,
attachments=attachments.values(),
self_collisions=collisions,
disabled_collisions=disabled_collisions,
max_distance=COLLISION_BUFFER,
custom_limits=custom_limits,
restarts=5,
iterations=50,
smooth=smooth)
else:
reference_pose = (unit_point(), get_liquid_quat(holding_water))
path = plan_water_motion(world.robot,
arm_joints,
conf2,
water_attachment,
resolutions=resolutions,
weights=weights,
obstacles=obstacles,
attachments=attachments.values(),
self_collisions=collisions,
disabled_collisions=disabled_collisions,
max_distance=COLLISION_BUFFER,
custom_limits=custom_limits,
reference_pose=reference_pose,
restarts=7,
iterations=75,
smooth=smooth)
if path is None:
return None
control = Control({
'action':
'move-arm',
#'objects': [],
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments=attachments),
'commands': [
ArmTrajectory(arm, path),
],
})
return (control, )
def gen_fn(arm, bowl_name, bowl_pose, spoon_name, grasp):
if bowl_name == spoon_name:
return
bowl_body = world.get_body(bowl_name)
attachment = get_grasp_attachment(world, arm, grasp)
feature = get_scoop_feature(world, bowl_name, spoon_name)
for parameter in parameter_generator(feature):
spoon_path_bowl = sample_scoop_trajectory(world,
feature,
parameter,
collisions=collisions)
if spoon_path_bowl is None:
continue
for _ in range(max_attempts):
set_gripper_position(world.robot, arm, grasp.grasp_width)
set_pose(bowl_body, bowl_pose)
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
spoon_path = [
multiply(bowl_pose, invert(rotate_bowl), spoon_pose_bowl)
for spoon_pose_bowl in spoon_path_bowl
]
#visualize_cartesian_path(world.get_body(spoon_name), spoon_path)
# TODO: pass in tool collision pairs here
arm_path = plan_attachment_motion(
world.robot,
arm,
attachment,
spoon_path,
obstacles=set(obstacles) - {bowl_body},
self_collisions=collisions,
disabled_collisions=disabled_collisions,
collision_buffer=collision_buffer,
attachment_collisions=False)
if arm_path is None:
continue
pre_conf = Conf(arm_path[0])
set_joint_positions(world.robot,
get_arm_joints(world.robot, arm), pre_conf)
attachment.assign()
if pairwise_collision(world.robot, bowl_body):
# TODO: ensure no robot/bowl collisions for the full path
continue
robot_saver = BodySaver(world.robot)
post_conf = Conf(arm_path[-1])
control = Control({
'action':
'scoop',
'objects': [bowl_name, spoon_name],
'feature':
feature,
'parameter':
parameter,
'context':
Context(savers=[robot_saver],
attachments={spoon_name: attachment}),
'commands': [
ArmTrajectory(arm, arm_path, dialation=4.0),
],
})
yield (pre_conf, post_conf, control)
break
else:
yield None
def plan_press(world,
knob_name,
pose,
grasp,
base_conf,
obstacles,
randomize=True,
**kwargs):
base_conf.assign()
world.close_gripper()
robot_saver = BodySaver(world.robot)
if randomize:
sample_fn = get_sample_fn(world.robot, world.arm_joints)
set_joint_positions(world.robot, world.arm_joints, sample_fn())
else:
world.carry_conf.assign()
gripper_pose = multiply(pose, invert(
grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1
#set_joint_positions(world.gripper, get_movable_joints(world.gripper), world.closed_gq.values)
#set_tool_pose(world, gripper_pose)
full_grasp_conf = world.solve_inverse_kinematics(gripper_pose)
#wait_for_user()
if full_grasp_conf is None:
# if PRINT_FAILURES: print('Grasp kinematic failure')
return
robot_obstacle = (world.robot,
frozenset(get_moving_links(world.robot,
world.arm_joints)))
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
#if PRINT_FAILURES: print('Grasp collision failure')
return
approach_pose = multiply(pose, invert(grasp.pregrasp_pose))
approach_path = plan_approach(world,
approach_pose,
obstacles=obstacles,
**kwargs)
if approach_path is None:
return
aq = FConf(world.robot, world.arm_joints,
approach_path[0]) if MOVE_ARM else world.carry_conf
#gripper_motion_fn = get_gripper_motion_gen(world, **kwargs)
#finger_cmd, = gripper_motion_fn(world.open_gq, world.closed_gq)
objects = []
cmd = Sequence(
State(world, savers=[robot_saver]),
commands=[
#finger_cmd.commands[0],
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
approach_path),
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
reversed(approach_path)),
#finger_cmd.commands[0].reverse(),
Wait(world, duration=1.0),
],
name='press')
yield (
aq,
cmd,
)
def fn(printed, directed, position, conf):
# Queue minimizes the statistic
element = get_undirected(all_elements, directed)
n1, n2 = directed
# forward adds, backward removes
structure = printed | {element} if forward else printed - {element}
structure_ids = get_extructed_ids(element_from_id, structure)
normalizer = 1
#normalizer = len(structure)
#normalizer = compute_element_distance(node_points, all_elements)
reduce_op = sum # sum | max | average
reaction_fn = force_from_reaction # force_from_reaction | torque_from_reaction
first_node, second_node = directed if forward else reverse_element(directed)
layer = sign * layer_from_edge[element]
#layer = sign * layer_from_directed.get(directed, INF)
tool_point = position
tool_distance = 0.
if heuristic in COST_HEURISTICS:
if conf is not None:
if hash_or_id(conf) not in ee_cache:
with BodySaver(robot):
set_configuration(robot, conf)
ee_cache[hash_or_id(conf)] = get_tool_position(robot)
tool_point = ee_cache[hash_or_id(conf)]
tool_distance = get_distance(tool_point, node_points[first_node])
# TODO: weighted average to balance cost and bias
if heuristic == 'none':
return 0
if heuristic == 'random':
return random.random()
elif heuristic == 'degree':
# TODO: other graph statistics
#printed_nodes = {n for e in printed for n in e}
#node = n1 if n2 in printed_nodes else n2
#if node in ground_nodes:
# return 0
raise NotImplementedError()
elif heuristic == 'length':
# Equivalent to mass if uniform density
return get_element_length(element, node_points)
elif heuristic == 'distance':
return tool_distance
elif heuristic == 'layered-distance':
return (layer, tool_distance)
# elif heuristic == 'components':
# # Ground nodes intentionally omitted
# # TODO: this is broken
# remaining = all_elements - printed if forward else printed - {element}
# vertices = nodes_from_elements(remaining)
# components = get_connected_components(vertices, remaining)
# #print('Components: {} | Distance: {:.3f}'.format(len(components), tool_distance))
# return (len(components), tool_distance)
elif heuristic == 'fixed-tsp':
# TODO: layer_from_edge[element]
# TODO: score based on current distance from the plan in the tour
# TODO: factor in the distance to the next element in a more effective way
if order is None:
return (INF, tool_distance)
return (sign*order[element], tool_distance) # Chooses least expensive direction
elif heuristic == 'tsp':
if printed not in tsp_cache: # not last_plan and
# TODO: seed with the previous solution
#remaining = all_elements - printed if forward else printed
#assert element in remaining
#printed_nodes = compute_printed_nodes(ground_nodes, printed) if forward else ground_nodes
tsp_cache[printed] = solve_tsp(all_elements, ground_nodes, node_points, printed, tool_point, initial_point,
bidirectional=True, layers=True, max_time=30, visualize=False, verbose=True)
#print(tsp_cache[printed])
if not last_plan:
last_plan[:] = tsp_cache[printed][0]
plan, cost = tsp_cache[printed]
#plan = last_plan[len(printed):]
if plan is None:
#return tool_distance
return (layer, INF)
transit = compute_transit_distance(node_points, plan, start=tool_point, end=initial_point)
assert forward
first = plan[0] == directed
#return not first # No info if the best isn't possible
index = None
for i, directed2 in enumerate(plan):
undirected2 = get_undirected(all_elements, directed2)
if element == undirected2:
assert index is None
index = i
assert index is not None
# Could also break ties by other in the plan
# Two plans might have the same cost but the swap might be detrimental
new_plan = [directed] + plan[:index] + plan[index+1:]
assert len(plan) == len(new_plan)
new_transit = compute_transit_distance(node_points, new_plan, start=tool_point, end=initial_point)
#print(layer, cost, transit + compute_element_distance(node_points, plan),
# new_transit + compute_element_distance(node_points, plan))
#return new_transit
return (layer, not first, new_transit) # Layer important otherwise it shortcuts
elif heuristic == 'online-tsp':
if forward:
_, tsp_distance = solve_tsp(all_elements-structure, ground_nodes, node_points, printed,
node_points[second_node], initial_point, visualize=False)
else:
_, tsp_distance = solve_tsp(structure, ground_nodes, node_points, printed, initial_point,
node_points[second_node], visualize=False)
total = tool_distance + tsp_distance
return total
# elif heuristic == 'mst':
# # TODO: this is broken
# mst_distance = compute_component_mst(node_points, ground_nodes, remaining,
# initial_position=node_points[second_node])
# return tool_distance + mst_distance
elif heuristic == 'x':
return sign * get_midpoint(node_points, element)[0]
elif heuristic == 'z':
return sign * compute_z_distance(node_points, element)
elif heuristic == 'pitch':
#delta = node_points[second_node] - node_points[first_node]
delta = node_points[n2] - node_points[n1]
return get_pitch(delta)
elif heuristic == 'dijkstra': # offline
# TODO: sum of all element path distances
return sign*np.average([distance_from_node[node].cost for node in element]) # min, max, average
elif heuristic == 'online-dijkstra':
if printed not in distance_cache:
distance_cache[printed] = compute_distance_from_node(printed, node_points, ground_nodes)
return sign*min(distance_cache[printed][node].cost
if node in distance_cache[printed] else INF
for node in element)
elif heuristic == 'plan-stiffness':
if order is None:
return None
return (sign*order[element], directed not in order)
elif heuristic == 'load':
nodal_loads = checker.get_nodal_loads(existing_ids=structure_ids, dof_flattened=False) # get_self_weight_loads
return reduce_op(np.linalg.norm(force_from_reaction(reaction)) for reaction in nodal_loads.values())
elif heuristic == 'fixed-forces':
#printed = all_elements # disable to use most up-to-date
# TODO: relative to the load introduced
if printed not in reaction_cache:
reaction_cache[printed] = compute_all_reactions(extrusion_path, all_elements, checker=checker)
force = reduce_op(np.linalg.norm(reaction_fn(reaction)) for reaction in reaction_cache[printed].reactions[element])
return force / normalizer
elif heuristic == 'forces':
reactions_from_nodes = compute_node_reactions(extrusion_path, structure, checker=checker)
#torque = sum(np.linalg.norm(np.sum([torque_from_reaction(reaction) for reaction in reactions], axis=0))
# for reactions in reactions_from_nodes.values())
#return torque / normalizer
total = reduce_op(np.linalg.norm(reaction_fn(reaction)) for reactions in reactions_from_nodes.values()
for reaction in reactions)
return total / normalizer
#return max(sum(np.linalg.norm(reaction_fn(reaction)) for reaction in reactions)
# for reactions in reactions_from_nodes.values())
elif heuristic == 'stiffness':
# TODO: add different variations
# TODO: normalize by initial stiffness, length, or degree
# Most unstable or least unstable first
# Gets faster with fewer all_elements
#old_stiffness = score_stiffness(extrusion_path, element_from_id, printed, checker=checker)
stiffness = score_stiffness(extrusion_path, element_from_id, structure, checker=checker) # lower is better
return stiffness / normalizer
#return stiffness / old_stiffness
elif heuristic == 'fixed-stiffness':
# TODO: invert the sign for regression/progression?
# TODO: sort FastDownward by the (fixed) action cost
return stiffness_cache[element] / normalizer
elif heuristic == 'relative-stiffness':
stiffness = score_stiffness(extrusion_path, element_from_id, structure, checker=checker) # lower is better
if normalizer == 0:
return 0
return stiffness / normalizer
#return stiffness / stiffness_cache[element]
raise ValueError(heuristic)
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
