def command_collision(end_effector, command, bodies):
# TODO: each new addition makes collision checking more expensive
#offset = 4
#for robot_conf in trajectory[offset:-offset]:
collisions = [False for _ in range(len(bodies))]
# Orientation remains the same for the extrusion trajectory
idx_from_body = dict(zip(bodies, range(len(bodies))))
# TODO: separate into another method. Sort paths by tool poses first
for trajectory in command.trajectories:
for tool_pose in randomize(trajectory.get_link_path()): # TODO: bisect
end_effector.set_pose(tool_pose)
#for body, _ in get_bodies_in_region(tool_aabb): # TODO
for i, body in enumerate(bodies):
if body not in idx_from_body: # Robot
continue
idx = idx_from_body[body]
if not collisions[idx]:
collisions[idx] |= pairwise_collision(
end_effector.body, body)
for trajectory in command.trajectories:
for robot_conf in randomize(trajectory.path):
set_joint_positions(trajectory.robot, trajectory.joints,
robot_conf)
for i, body in enumerate(bodies):
if not collisions[i]:
collisions[i] |= pairwise_collision(trajectory.robot, body)
#for element, unsafe in zip(elements, collisions):
# command.safe_per_element[element] = unsafe
return collisions
def test_caching(robot, obstacles):
with timer(message='{:f}'):
#update_scene() # 5.19752502441e-05
step_simulation() # 0.000210046768188
with timer(message='{:f}'):
#print(get_aabb(robot, link=None, only_collision=True))
print(contact_collision()) # 2.50339508057e-05
for _ in range(3):
with timer(message='{:f}'):
#print(get_aabb(robot, link=None, only_collision=True)) # Recomputes each time
print(contact_collision()) # 1.69277191162e-05
print()
obstacle = obstacles[-1]
#for link in get_all_links(robot):
# set_collision_pair_mask(robot, obstacle, link1=link, enable=False) # Doesn't seem to affect pairwise_collision
with timer('{:f}'):
print(pairwise_collision(robot, obstacle)) # 0.000031
links = get_all_links(robot)
links = [link for link in get_all_links(robot) if can_collide(robot, link)]
#links = randomize(links)
with timer('{:f}'):
print(
any(
pairwise_collision(robot, obstacle, link1=link)
for link in links # 0.000179
))
#if aabb_overlap(get_aabb(robot, link), get_aabb(obstacles[-1]))))
#if can_collide(robot, link)))
with timer('{:f}'):
print(pairwise_collision(robot, obstacle))
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 element_collision_fn(q):
if collision_fn(q):
return True
#for body in get_bodies_in_region(get_aabb(robot)): # Perform per link?
# if (element_from_body.get(body, None) in printed_elements) and pairwise_collision(robot, body):
# return True
for hull, bodies in hulls.items():
if pairwise_collision(robot, hull) and any(pairwise_collision(robot, body) for body in bodies):
return True
return False
def lower_spoon(world, bowl_name, spoon_name, min_spoon_z, max_spoon_z):
assert min_spoon_z <= max_spoon_z
bowl_body = world.get_body(bowl_name)
bowl_urdf_from_center = get_urdf_from_base(
bowl_body) # get_urdf_from_center
spoon_body = world.get_body(spoon_name)
spoon_quat = lookup_orientation(spoon_name, STIR_QUATS)
spoon_urdf_from_center = get_urdf_from_base(
spoon_body, reference_quat=spoon_quat) # get_urdf_from_center
# Keeping the orientation consistent for generalization purposes
# TODO: what happens when the base isn't flat (e.g. bowl)
bowl_pose = get_pose(bowl_body)
stir_z = None
for z in np.arange(max_spoon_z, min_spoon_z, step=-0.005):
bowl_from_stirrer = multiply(bowl_urdf_from_center, Pose(Point(z=z)),
invert(spoon_urdf_from_center))
set_pose(spoon_body, multiply(bowl_pose, bowl_from_stirrer))
#wait_for_user()
if pairwise_collision(bowl_body, spoon_body):
# Want to be careful not to make contact with the base
break
stir_z = z
return stir_z
def sample_placements(body_surfaces, obstacles=None, savers=[], min_distances={}):
if obstacles is None:
obstacles = OrderedSet(get_bodies()) - set(body_surfaces)
obstacles = list(obstacles)
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:
obstacles.append(body)
break
for saver in savers:
saver.restore()
return True
def sample_placement(world,
entity_name,
surface_name,
min_distance=0.05,
robust_radius=0.025,
**kwargs):
entity_body = world.get_body(entity_name)
placement_gen = get_stable_gen(world,
pos_scale=0,
rot_scale=0,
robust_radius=robust_radius,
**kwargs)
for pose, in placement_gen(entity_name, surface_name):
x, y, z = point_from_pose(pose.get_world_from_body())
if x < MIN_PLACEMENT_X:
continue
pose.assign()
if not any(
pairwise_collision(
entity_body, obst_body, max_distance=min_distance)
for obst_body in world.body_from_name.values()
if entity_body != obst_body):
return pose
raise RuntimeError(
'Unable to find a pose for object {} on surface {}'.format(
entity_name, surface_name))
def compute_cfree(body, poses, obstacles=[]):
cfree_poses = set()
for pose in poses:
pose.assign()
if not any(pairwise_collision(body, obst) for obst in obstacles):
cfree_poses.add(pose)
return cfree_poses
def test(o1, rp1, o2, rp2, s):
if not collisions or (o1 == o2):
return True
if isinstance(rp1, SurfaceDist) or isinstance(rp2, SurfaceDist):
return True # TODO: perform this probabilistically
rp1.assign()
rp2.assign()
return not pairwise_collision(world.get_body(o1), world.get_body(o2))
def fn(body, pose, grasp):
obstacles = [body] + fixed
gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose)
approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose)
draw_pose(gripper_pose, length=0.04)
draw_pose(approach_pose, length=0.04)
for _ in range(num_attempts):
if USE_IKFAST:
q_approach = sample_tool_ik(robot, approach_pose)
if q_approach is not None:
set_joint_positions(robot, movable_joints, q_approach)
else:
set_joint_positions(robot, movable_joints, sample_fn()) # Random seed
q_approach = inverse_kinematics(robot, grasp.link, approach_pose)
if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles):
continue
conf = BodyConf(robot, joints=movable_joints)
if USE_IKFAST:
q_grasp = sample_tool_ik(robot, gripper_pose, closest_only=True)
if q_grasp is not None:
set_joint_positions(robot, movable_joints, q_grasp)
else:
q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose)
if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles):
continue
if teleport:
path = [q_approach, q_grasp]
else:
conf.assign()
#direction, _ = grasp.approach_pose
#path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction,
# quat_from_pose(approach_pose))
path = plan_direct_joint_motion(robot, conf.joints, q_grasp, obstacles=obstacles, \
self_collisions=self_collisions)
if path is None:
if DEBUG_FAILURE: user_input('Approach motion failed')
continue
command = Command([BodyPath(robot, path, joints=movable_joints),
Attach(body, robot, grasp.link),
BodyPath(robot, path[::-1], joints=movable_joints, attachments=[grasp])])
return (conf, command)
# TODO: holding collisions
return None
def compute_door_paths(world,
joint_name,
door_conf1,
door_conf2,
obstacles=set(),
teleport=False):
door_paths = []
if door_conf1 == door_conf2:
return door_paths
door_joint = joint_from_name(world.kitchen, joint_name)
door_joints = [door_joint]
# TODO: could unify with grasp path
door_extend_fn = get_extend_fn(world.kitchen,
door_joints,
resolutions=[DOOR_RESOLUTION])
door_path = [door_conf1.values] + list(
door_extend_fn(door_conf1.values, door_conf2.values))
if teleport:
door_path = [door_conf1.values, door_conf2.values]
# TODO: open until collision for the drawers
sign = world.get_door_sign(door_joint)
pull = (sign * door_path[0][0] < sign * door_path[-1][0])
# door_obstacles = get_descendant_obstacles(world.kitchen, door_joint)
for handle_grasp in get_handle_grasps(world, door_joint, pull=pull):
link, grasp, pregrasp = handle_grasp
handle_path = []
for door_conf in door_path:
set_joint_positions(world.kitchen, door_joints, door_conf)
# if any(pairwise_collision(door_obst, obst)
# for door_obst, obst in product(door_obstacles, obstacles)):
# return
handle_path.append(get_link_pose(world.kitchen, link))
# Collide due to adjacency
# TODO: check pregrasp path as well
# TODO: check gripper self-collisions with the robot
set_configuration(world.gripper, world.open_gq.values)
tool_path = [
multiply(handle_pose, invert(grasp)) for handle_pose in handle_path
]
for i, tool_pose in enumerate(tool_path):
set_joint_positions(world.kitchen, door_joints, door_path[i])
set_tool_pose(world, tool_pose)
# handles = draw_pose(handle_path[i], length=0.25)
# handles.extend(draw_aabb(get_aabb(world.kitchen, link=link)))
# wait_for_user()
# for handle in handles:
# remove_debug(handle)
if any(
pairwise_collision(world.gripper, obst)
for obst in obstacles):
break
else:
door_paths.append(
DoorPath(door_path, handle_path, handle_grasp, tool_path))
return door_paths
def test(o1, wp1):
if isinstance(wp1, SurfaceDist):
return True
if not collisions or (wp1.support not in DRAWERS):
return True
body = world.get_body(o1)
wp1.assign()
obstacles = world.static_obstacles
if any(pairwise_collision(body, obst) for obst in obstacles):
return False
return True
def is_pull_safe(world, door_joint, door_plan):
obstacles = get_descendant_obstacles(world.kitchen, door_joint)
door_path, handle_path, handle_plan, tool_path = door_plan
for door_conf in [door_path[0], door_path[-1]]:
# TODO: check the whole door trajectory
set_joint_positions(world.kitchen, [door_joint], door_conf)
# TODO: just check collisions with the base of the robot
if any(pairwise_collision(world.robot, b) for b in obstacles):
if PRINT_FAILURES: print('Door start/end failure')
return False
return True
def test(bq, o2, wp2):
if not collisions:
return True
if isinstance(wp2, SurfaceDist):
return True # TODO: perform this probabilistically
bq.assign()
world.carry_conf.assign()
wp2.assign()
obstacles = get_link_obstacles(world, o2)
return not any(
pairwise_collision(world.robot, obst) for obst in obstacles)
def test_base_conf(world, bq, obstacles, min_distance=0.0):
robot_links = [world.franka_link, world.gripper_link
] if world.is_real() else []
bq.assign()
for conf in world.special_confs:
# Could even sample a special visible conf for this base_conf
conf.assign()
if not is_robot_visible(world, robot_links) or any(
pairwise_collision(world.robot, b, max_distance=min_distance)
for b in obstacles):
return False
return True
def test_supported(world, body, surface_name, collisions=True):
# TODO: is_center_on_aabb or is_placed_on_aabb
surface_aabb = compute_surface_aabb(world, surface_name)
# TODO: epsilon thresholds?
if not is_placed_on_aabb(body, surface_aabb): # , above_epsilon=z_offset+1e-3):
return False
obstacles = world.static_obstacles | get_surface_obstacles(world, surface_name)
if not collisions:
obstacles = set()
#print([get_link_name(obst[0], list(obst[1])[0]) for obst in obstacles
# if pairwise_collision(body, obst)])
return not any(pairwise_collision(body, obst) for obst in obstacles)
def test(o1, wp1, o2, wp2):
if isinstance(wp1, SurfaceDist) or isinstance(wp2, SurfaceDist):
return True
if not collisions or (o1 == o2) or (o2 == wp1.support): # DRAWERS
return True
body = world.get_body(o1)
wp1.assign()
wp2.assign()
if any(
pairwise_collision(body, obst)
for obst in get_surface_obstacles(world, o2)):
return False
return True
def create_inverse_reachability(robot, body, table, arm, grasp_type, num_samples=500):
link = get_gripper_link(robot, arm)
movable_joints = get_movable_joints(robot)
default_conf = get_joint_positions(robot, movable_joints)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
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))
set_joint_positions(robot, movable_joints, default_conf)
base_conf = next(uniform_pose_generator(robot, gripper_pose))
set_base_values(robot, base_conf)
if pairwise_collision(robot, table):
continue
conf = inverse_kinematics(robot, link, gripper_pose)
if (conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(invert(get_link_pose(robot, link)), get_pose(robot))
gripper_from_base_list.append(gripper_from_base)
print('{} / {}'.format(len(gripper_from_base_list), num_samples))
filename = IR_FILENAME.format(grasp_type, arm)
path = get_database_file(filename)
data = {
'filename': filename,
'robot': get_body_name(robot),
'grasp_type': grasp_type,
'arg': arm,
'carry_conf': get_carry_conf(arm, grasp_type),
'gripper_link': link,
'gripper_from_base': gripper_from_base_list,
}
write_pickle(path, data)
return path
def sample(self, discrete=True):
# TODO: timeout if unable to find
while True:
poses = {}
for name, pose_dist in randomize(self.pose_dists.items()):
body = self.world.get_body(name)
pose = pose_dist.sample_discrete(
) if discrete else pose_dist.sample()
pose.assign()
if any(
pairwise_collision(body, self.world.get_body(other))
for other in poses):
break
poses[name] = pose
else:
return poses
def test(detect, obj_name, pose):
if (detect.name == obj_name) or (detect.surface_name
== obj_name) or isinstance(
pose, SurfaceDist):
return True
move_occluding(world)
detect.pose.assign()
pose.assign()
body = world.get_body(detect.name)
obstacles = get_link_obstacles(world, obj_name)
if any(pairwise_collision(body, obst) for obst in obstacles):
return False
visible = not obstacles & detect.compute_occluding()
#if not visible:
# handles = detect.draw()
# wait_for_user()
# remove_handles(handles)
return visible
def is_approach_safe(world, obj_name, pose, grasp, obstacles):
assert pose.support is not None
obj_body = world.get_body(obj_name)
pose.assign() # May set the drawer confs as well
set_joint_positions(world.gripper, get_movable_joints(world.gripper),
world.open_gq.values)
#set_renderer(enable=True)
for _ in iterate_approach_path(world, pose, grasp, body=obj_body):
#for link in get_all_links(world.gripper):
# set_color(world.gripper, apply_alpha(np.zeros(3)), link)
#wait_for_user()
if any(
pairwise_collision(world.gripper, obst
) # or pairwise_collision(obj_body, obst)
for obst in obstacles):
print('Unsafe approach!')
#wait_for_user()
return False
return True
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 test(o1, wp1, g1, o2, wp2):
# o1 will always be a movable object
if isinstance(wp2, SurfaceDist):
return True # TODO: perform this probabilistically
if not collisions or (o1 == o2) or (o2 == wp1.support):
return True
# TODO: could define these on sets of samples to prune all at once
body = world.get_body(o1)
wp2.assign()
obstacles = get_link_obstacles(world, o2) # - {body}
if not obstacles:
return True
for _ in iterate_approach_path(world, wp1, g1, body=body):
if any(
pairwise_collision(part, obst)
for part in [world.gripper, body] for obst in obstacles):
# TODO: some collisions the bottom drawer and the top drawer handle
#print(o1, wp1.support, o2, wp2.support)
#wait_for_user()
return False
return True
def test(at, o, wp):
if not collisions:
return True
# TODO: check door collisions
# TODO: still need to check static links at least once
if isinstance(wp, SurfaceDist):
return True # TODO: perform this probabilistically
wp.assign()
state = at.context.copy()
state.assign()
all_bodies = {
body
for command in at.commands for body in command.bodies
}
for command in at.commands:
obstacles = get_link_obstacles(world, o) - all_bodies
# TODO: why did I previously remove o at p?
#obstacles = get_link_obstacles(world, o) - command.bodies # - p.bodies # Doesn't include o at p
if not obstacles:
continue
if isinstance(command, DoorTrajectory):
[door_joint] = command.door_joints
surface_name = get_link_name(world.kitchen,
child_link_from_joint(door_joint))
if wp.support == surface_name:
return True
for _ in command.iterate(state):
state.derive()
#for attachment in state.attachments.values():
# if any(pairwise_collision(attachment.child, obst) for obst in obstacles):
# return False
# TODO: just check collisions with moving links
if any(
pairwise_collision(world.robot, obst)
for obst in obstacles):
#print(at, o, p)
#wait_for_user()
return False
return True
def plan_workspace(world,
tool_path,
obstacles,
randomize=True,
teleport=False):
# Assuming that pairs of fixed things aren't in collision at this point
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
distance_fn = get_distance_fn(world.robot, world.arm_joints)
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()
arm_path = []
for i, tool_pose in enumerate(tool_path):
#set_joint_positions(world.kitchen, [door_joint], door_path[i])
tolerance = INF if i == 0 else NEARBY_PULL
full_arm_conf = world.solve_inverse_kinematics(
tool_pose, nearby_tolerance=tolerance)
if full_arm_conf is None:
# TODO: this fails when teleport=True
if PRINT_FAILURES: print('Workspace kinematic failure')
return None
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
if PRINT_FAILURES: print('Workspace collision failure')
return None
arm_conf = get_joint_positions(world.robot, world.arm_joints)
if arm_path and not teleport:
distance = distance_fn(arm_path[-1], arm_conf)
if MAX_CONF_DISTANCE < distance:
if PRINT_FAILURES:
print(
'Workspace proximity failure (distance={:.5f})'.format(
distance))
return None
arm_path.append(arm_conf)
# wait_for_user()
return arm_path
def validate_trajectories(element_bodies, fixed_obstacles, trajectories):
if trajectories is None:
return False
# TODO: combine all validation procedures
remove_all_debug()
for body in element_bodies.values():
set_color(body, np.zeros(4))
print('Trajectories:', len(trajectories))
obstacles = list(fixed_obstacles)
for i, trajectory in enumerate(trajectories):
for _ in trajectory.iterate():
#wait_for_user()
if any(pairwise_collision(trajectory.robot, body) for body in obstacles):
if has_gui():
print('Collision on trajectory {}'.format(i))
wait_for_user()
return False
if isinstance(trajectory, PrintTrajectory):
body = element_bodies[trajectory.element]
set_color(body, apply_alpha(RED))
obstacles.append(body)
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 test_bounding(q):
set_joint_positions(robot, joints, q)
collision = (bounding is not None) and pairwise_collision(robot, bounding, max_distance=buffer) # body_collision
return q, collision
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 body_pair_collision(body1, body2, collision_buffer=COLLISION_BUFFER):
if body1 == body2:
return False
return pairwise_collision(body1, body2, max_distance=collision_buffer)
