def compute_surface_aabb(world, surface_name):
if surface_name in ENV_SURFACES: # TODO: clean this up
# TODO: the aabb for golf is off the table
surface_body = world.environment_bodies[surface_name]
return get_aabb(surface_body)
surface_body = world.kitchen
surface_name, shape_name, _ = surface_from_name(surface_name)
surface_link = link_from_name(surface_body, surface_name)
surface_pose = get_link_pose(surface_body, surface_link)
if shape_name == SURFACE_TOP:
surface_aabb = get_aabb(surface_body, surface_link)
elif shape_name == SURFACE_BOTTOM:
data = sorted(get_collision_data(surface_body, surface_link),
key=lambda d: point_from_pose(get_data_pose(d))[2])[0]
extent = np.array(get_data_extents(data))
aabb = AABB(-extent/2., +extent/2.)
vertices = apply_affine(multiply(surface_pose, get_data_pose(data)), get_aabb_vertices(aabb))
surface_aabb = aabb_from_points(vertices)
else:
[data] = filter(lambda d: d.filename != '',
get_collision_data(surface_body, surface_link))
meshes = read_obj(data.filename)
#colors = spaced_colors(len(meshes))
#set_color(surface_body, link=surface_link, color=np.zeros(4))
mesh = meshes[shape_name]
#for i, (name, mesh) in enumerate(meshes.items()):
mesh = tform_mesh(multiply(surface_pose, get_data_pose(data)), mesh=mesh)
surface_aabb = aabb_from_points(mesh.vertices)
#add_text(surface_name, position=surface_aabb[1])
#draw_mesh(mesh, color=colors[i])
#wait_for_user()
#draw_aabb(surface_aabb)
#wait_for_user()
return surface_aabb
def test_aabb(robot):
base_link = link_from_name(robot, BASE_LINK_NAME)
region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3))
draw_aabb(region_aabb)
# bodies = get_bodies_in_region(region_aabb)
# print(len(bodies), bodies)
# for body in get_bodies():
# set_pose(body, Pose())
#step_simulation() # Need to call before get_bodies_in_region
#update_scene()
for i in range(3):
with timer(message='{:f}'):
bodies = get_bodies_in_region(
region_aabb) # This does cache some info
print(i, len(bodies), bodies)
# https://github.com/bulletphysics/bullet3/search?q=broadphase
# https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93
# https://andysomogyi.github.io/mechanica/bullet.html
# http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual
aabb = get_aabb(robot)
# aabb = get_subtree_aabb(robot, base_link)
print(aabb)
draw_aabb(aabb)
for link in [BASE_LINK, base_link]:
print(link, get_collision_data(robot, link),
pairwise_link_collision(robot, link, robot, link))
def main(use_pr2_drake=True):
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
table_path = "models/table_collision/table.urdf"
table = load_pybullet(table_path, fixed_base=True)
set_quat(table, quat_from_euler(Euler(yaw=PI / 2)))
# table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf
obstacles = [plane, table]
pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF
with HideOutput():
pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings?
dump_body(pr2)
z = base_aligned_z(pr2)
print(z)
#z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3)))
print(z)
set_point(pr2, Point(z=z))
print(get_aabb(pr2))
wait_if_gui()
base_start = (-2, -2, 0)
base_goal = (2, 2, 0)
arm_start = SIDE_HOLDING_LEFT_ARM
#arm_start = TOP_HOLDING_LEFT_ARM
#arm_start = REST_LEFT_ARM
arm_goal = TOP_HOLDING_LEFT_ARM
#arm_goal = SIDE_HOLDING_LEFT_ARM
left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
set_joint_positions(pr2, left_joints, arm_start)
set_joint_positions(pr2, right_joints,
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, torso_joints, [0.2])
open_arm(pr2, 'left')
# test_ikfast(pr2)
add_line(base_start, base_goal, color=RED)
print(base_start, base_goal)
if use_pr2_drake:
test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
else:
test_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
test_arm_motion(pr2, left_joints, arm_goal)
# test_arm_control(pr2, left_joints, arm_start)
wait_if_gui('Finish?')
disconnect()
def visualize_cartesian_path(body, pose_path):
for i, pose in enumerate(pose_path):
set_pose(body, pose)
print('{}/{}) continue?'.format(i, len(pose_path)))
wait_for_user()
handles = draw_pose(get_pose(body))
handles.extend(draw_aabb(get_aabb(body)))
print('Finish?')
wait_for_user()
for h in handles:
remove_debug(h)
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 get_contained_beads(body, beads, height_fraction=1.0, top_threshold=0.0):
#aabb = get_aabb(body)
center, extent = approximate_as_prism(body, body_pose=get_pose(body))
lower = center - extent / 2.
upper = center + extent / 2.
_, _, z1 = lower
x2, y2, z2 = upper
z_upper = z1 + height_fraction * (z2 - z1) + top_threshold
new_aabb = AABB(lower, np.array([x2, y2, z_upper]))
#handles = draw_aabb(new_aabb)
return {
bead
for bead in beads if aabb_contains_aabb(get_aabb(bead), new_aabb)
}
def read_mass(scale_body, max_height=0.5, tolerance=1e-2):
# Approximation: ignores connectivity outside box and bodies not resting on the body
# TODO: could also add a force sensor and estimate the force
scale_aabb = get_aabb(scale_body)
extent = scale_aabb.upper - scale_aabb.lower
lower = scale_aabb.lower + np.array([0, 0, extent[2]
]) - tolerance * np.ones(3)
upper = scale_aabb.upper + np.array([0, 0, max_height
]) + tolerance * np.ones(3)
above_aabb = AABB(lower, upper)
total_mass = 0.
#handles = draw_aabb(above_aabb)
for body, link in get_bodies_in_region(above_aabb):
if scale_body == body:
continue
link_aabb = get_aabb(body, link)
if aabb_contains_aabb(link_aabb, above_aabb):
total_mass += get_mass(body, link)
else:
#print(get_name(body), get_link_name(body, link))
#handles.extend(draw_aabb(link_aabb))
pass
return total_mass
def draw_forward_reachability(world, arms, grasp_type='top', color=(1, 0, 0)):
for name, body in world.perception.sim_bodies.items():
if name.startswith(TABLE):
table = body
break
else:
return False
if not DEBUG:
return True
with ClientSaver(world.perception.client):
lower, upper = get_aabb(table)
for arm in arms:
vertices = [
np.append(vertex, upper[2] + 1e-3)
for vertex in compute_forward_reachability(
world.perception.pr2, arm=arm, grasp_type=grasp_type)
]
add_segments(vertices, closed=True, color=color)
return True
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 load_world(use_floor=True):
obstacles = []
#side, height = 10, 0.01
robot = load_robot()
with HideOutput():
lower, _ = get_aabb(robot)
if use_floor:
#floor = load_model('models/short_floor.urdf', fixed_base=True)
#add_data_path()
#floor = load_pybullet('plane.urdf', fixed_base=True)
#set_color(floor, TAN)
#floor = create_box(w=side, l=side, h=height, color=apply_alpha(GROUND_COLOR))
floor = create_plane(color=apply_alpha(GROUND_COLOR))
obstacles.append(floor)
#set_point(floor, Point(z=lower[2]))
set_point(floor, Point(x=1.2, z=0.023 - 0.025)) # -0.02
else:
floor = None # TODO: make this an empty list of obstacles
#set_all_static()
return obstacles, robot
def is_safe(self, obstacles):
checked_bodies = set(self.safe_from_body) & set(obstacles)
if any(not self.safe_from_body[e] for e in checked_bodies):
return False
unchecked_bodies = randomize(set(obstacles) - checked_bodies)
if not unchecked_bodies:
return True
for tool_pose in randomize(self.tool_path):
self.extrusion.end_effector.set_pose(tool_pose)
tool_aabb = get_aabb(
self.extrusion.end_effector.body) # TODO: cache nearby_bodies
nearby_bodies = {
b
for b, _ in get_bodies_in_region(tool_aabb)
if b in unchecked_bodies
}
for body in nearby_bodies:
if pairwise_collision(self.extrusion.end_effector.body, body):
self.safe_from_body[body] = False
return False
return True
def tool_path_collision(end_effector, element_pose, translation_path,
direction, angle, reverse, obstacles):
# TODO: allow sampling in the full sphere by checking collision with an element while sliding
for tool_pose in randomize(
compute_tool_path(element_pose, translation_path, direction, angle,
reverse)):
end_effector.set_pose(tool_pose)
#bodies = obstacles
tool_aabb = get_aabb(end_effector.body) # TODO: could just translate
#handles = draw_aabb(tool_aabb)
bodies = {
b
for b, _ in get_bodies_in_region(tool_aabb) if b in obstacles
}
#print(bodies)
#for body, link in bodies:
# handles.extend(draw_aabb(get_aabb(body, link)))
#wait_for_user()
#remove_handles(handles)
if any(pairwise_collision(end_effector.body, obst) for obst in bodies):
# TODO: sort by angle with smallest violation
return True
return False
def draw_names(world, **kwargs):
# TODO: adjust the colors?
handles = []
if not DEBUG:
return handles
with ClientSaver(world.perception.client):
for name, body in world.perception.sim_bodies.items():
#add_body_name(body, **kwargs)
with PoseSaver(body):
set_pose(body, unit_pose())
lower, upper = get_aabb(
body
) # TODO: multi-link bodies doesn't seem to update when moved
handles.append(
add_text(name, position=upper, parent=body,
**kwargs)) # parent_link=0,
#handles.append(draw_pose(get_pose(body)))
#handles.extend(draw_base_limits(get_base_limits(world.pr2), color=(1, 0, 0)))
#handles.extend(draw_circle(get_point(world.pr2), MAX_VIS_DISTANCE, color=(0, 0, 1)))
#handles.extend(draw_circle(get_point(world.pr2), MAX_REG_DISTANCE, color=(0, 0, 1)))
#from plan_tools.debug import test_voxels
#test_voxels(world)
return handles
def element_collision_fn(q):
if collision_fn(q):
return True
#step_simulation() # Might only need to call this once
for robot_link in robot_links:
#bodies = obstacles
aabb = get_aabb(robot, link=robot_link)
bodies = {
b
for b, _ in get_bodies_in_region(aabb) if b in obstacles
}
#set_joint_positions(robot, joints, q) # Need to reset
#draw_aabb(aabb)
#print(robot_link, get_link_name(robot, robot_link), len(bodies), aabb)
#print(sum(pairwise_link_collision(robot, robot_link, body, link2=0) for body, _ in region_bodies))
#print(sum(pairwise_collision(robot, body) for body, _ in region_bodies))
#wait_for_user()
if any(
pairwise_link_collision(
robot, robot_link, body, link2=BASE_LINK)
for body in bodies):
#wait_for_user()
return True
return False
def get_world_aabb(self):
return aabb_union(get_aabb(body)
for body in self.fixed) # self.all_bodies
def get_base_aabb(self):
franka_links = get_link_subtree(self.robot, self.franka_link)
base_links = get_link_subtree(self.robot, self.base_link)
return aabb_union(
get_aabb(self.robot, link)
for link in set(base_links) - set(franka_links))
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-c', '--cfree', action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument('--holonomic', action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false',
help='')
parser.add_argument('-s', '--seed', default=None, type=int,
help='The random seed to use.')
parser.add_argument('-v', '--viewer', action='store_false',
help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
seed = args.seed
#seed = 0
#seed = time.time()
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
print('Random seed:', get_random_seed(), random.random())
print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1()
draw_base_limits(base_limits)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
base_link = link_from_name(robot, BASE_LINK_NAME)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
set_all_static() # Doesn't seem to affect
region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3))
draw_aabb(region_aabb)
step_simulation() # Need to call before get_bodies_in_region
#update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331
bodies = get_bodies_in_region(region_aabb)
print(len(bodies), bodies)
# https://github.com/bulletphysics/bullet3/search?q=broadphase
# https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93
# https://andysomogyi.github.io/mechanica/bullet.html
# http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual
#draw_pose(get_link_pose(robot, base_link), length=0.5)
for conf in [get_joint_positions(robot, base_joints), goal_conf]:
draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH)
aabb = get_aabb(robot)
#aabb = get_subtree_aabb(robot, base_link)
draw_aabb(aabb)
for link in [BASE_LINK, base_link]:
print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link))
#########################
saver = WorldSaver()
start_time = time.time()
profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None
profiler.save()
with LockRenderer(lock=args.lock):
path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles,
custom_limits=custom_limits, resolutions=resolutions,
use_aabb=True, cache=True, max_distance=0.,
restarts=2, iterations=20, smooth=20) # 20 | None
saver.restore()
#wait_for_duration(duration=1e-3)
profiler.restore()
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path))
print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
disconnect()
return
iterate_path(robot, base_joints, path)
disconnect()
