def test_push(visualize):
arms = [LEFT_ARM]
block_name = create_name('purpleblock', 1) # greenblock | purpleblock
world, table_body = create_world([block_name], visualize=visualize)
with ClientSaver(world.perception.client):
block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON
#pos = (0.6, 0, block_z)
pos = (0.5, 0.2, block_z)
world.perception.set_pose(block_name, pos, quat_from_euler(Euler(yaw=-np.pi/6)))
update_world(world, table_body)
# TODO: push into reachable region
goal_pos2d = np.array(pos[:2]) + np.array([0.025, 0.1])
#goal_pos2d = np.array(pos[:2]) + np.array([0.025, -0.1])
#goal_pos2d = np.array(pos[:2]) + np.array([-0.1, -0.05])
#goal_pos2d = np.array(pos[:2]) + np.array([0.15, -0.05])
#goal_pos2d = np.array(pos[:2]) + np.array([0, 0.7]) # Intentionally not feasible
draw_point(np.append(goal_pos2d, [block_z]), size=0.05, color=(1, 0, 0))
init = [
('CanPush', block_name, goal_pos2d),
]
goal = [
('InRegion', block_name, goal_pos2d),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def save_inverse_reachability(robot, arm, grasp_type, tool_link,
gripper_from_base_list):
# TODO: store value of torso and roll joint for the IK database. Sample the roll joint.
# TODO: hash the pr2 urdf as well
filename = IR_FILENAME.format(grasp_type, arm)
path = get_database_file(filename)
data = {
'filename': filename,
'robot': get_body_name(robot),
'grasp_type': grasp_type,
'arm': arm,
'torso': get_group_conf(robot, 'torso'),
'carry_conf': get_carry_conf(arm, grasp_type),
'tool_link': tool_link,
'ikfast': is_ik_compiled(),
'gripper_from_base': gripper_from_base_list,
}
write_pickle(path, data)
if has_gui():
handles = []
for gripper_from_base in gripper_from_base_list:
handles.extend(
draw_point(point_from_pose(gripper_from_base),
color=(1, 0, 0)))
wait_for_user()
return path
def draw_push_goal(world, block_name, pos2d):
block_z = get_point(world.get_body(block_name))[2]
handles = draw_point(np.append(pos2d, [block_z]),
size=0.05,
color=(1, 0, 0))
lower, upper = POSE2D_RANGE
handles.extend(
add_segments([
(lower[0], lower[1], block_z),
(upper[0], lower[1], block_z),
(upper[0], upper[1], block_z),
(lower[0], upper[1], block_z),
],
closed=True))
return handles
def visualize_learned_pours(learner, bowl_type='blue_bowl', cup_type='red_cup', num_steps=None):
learner.query_type = REJECTION # BEST | REJECTION | STRADDLE
learner.validity_learner = None
world = create_world()
#add_obstacles()
#environment = get_bodies()
world.bodies[bowl_type] = load_cup_bowl(bowl_type)
world.bodies[cup_type] = load_cup_bowl(cup_type)
feature = get_pour_feature(world, bowl_type, cup_type)
draw_pose(get_pose(world.get_body(bowl_type)), length=0.2)
# TODO: sample different sizes
print('Feature:', feature)
for parameter in learner.parameter_generator(world, feature, valid=False):
handles = []
print('Parameter:', parameter)
valid = is_valid_pour(world, feature, parameter)
score = learner.score(feature, parameter)
x = learner.func.x_from_feature_parameter(feature, parameter)
[prediction] = learner.predict_x(np.array([x]), noise=True)
print('Valid: {} | Mean: {:.3f} | Var: {:.3f} | Score: {:.3f}'.format(
valid, prediction['mean'], prediction['variance'], score))
#fraction = rescale(clip(prediction['mean'], *DEFAULT_INTERVAL), DEFAULT_INTERVAL, new_interval=(0, 1))
#color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN)
#set_color(world.get_body(cup_type), apply_alpha(color, alpha=0.25))
# TODO: overlay many cups at different poses
bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter)
#handles.extend(draw_pose(bowl_from_pivot))
handles.extend(draw_point(point_from_pose(bowl_from_pivot), color=BLACK))
# TODO: could label bowl, cup dimensions
path, _ = pour_path_from_parameter(world, feature, parameter, cup_yaw=0)
for p1, p2 in safe_zip(path[:-1], path[1:]):
handles.append(add_line(point_from_pose(p1), point_from_pose(p2), color=RED))
if num_steps is not None:
path = path[:num_steps]
for i, pose in enumerate(path[::-1]):
#step_handles = draw_pose(pose, length=0.1)
#step_handles = draw_point(point_from_pose(pose), color=BLACK)
set_pose(world.get_body(cup_type), pose)
print('Step {}/{}'.format(i+1, len(path)))
wait_for_user()
#remove_handles(step_handles)
remove_handles(handles)
def visualize_stiffness(extrusion_path):
if not has_gui():
return
#label_elements(element_bodies)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
elements = list(element_from_id.values())
#draw_model(elements, node_points, ground_nodes)
# Freeform Assembly Planning
# TODO: https://arxiv.org/pdf/1801.00527.pdf
# Though assembly sequencing is often done by finding a disassembly sequence and reversing it, we will use a forward search.
# Thus a low-cost state will usually be correctly identified by considering only the deflection of the cantilevered beam path
# and approximating the rest of the beams as being infinitely stiff
reaction_from_node = compute_node_reactions(extrusion_path, elements)
#reaction_from_node = deformation.displacements # For visualizing displacements
#test_node_forces(node_points, reaction_from_node)
force_from_node = {
node: sum(
np.linalg.norm(force_from_reaction(reaction))
for reaction in reactions)
for node, reactions in reaction_from_node.items()
}
sorted_nodes = sorted(reaction_from_node,
key=lambda n: force_from_node[n],
reverse=True)
for i, node in enumerate(sorted_nodes):
print('{}) node={}, point={}, magnitude={:.3E}'.format(
i, node, node_points[node], force_from_node[node]))
#max_force = max(force_from_node.values())
max_force = max(
np.linalg.norm(reaction[:3])
for reactions in reaction_from_node.values() for reaction in reactions)
print('Max force:', max_force)
neighbors_from_node = get_node_neighbors(elements)
colors = sample_colors(len(sorted_nodes))
handles = []
for node, color in zip(sorted_nodes, colors):
#color = (0, 0, 0)
reactions = reaction_from_node[node]
#print(np.array(reactions))
start = node_points[node]
handles.extend(draw_point(start, color=color))
for reaction in reactions[:1]:
handles.append(
draw_reaction(start, reaction, max_force=max_force, color=RED))
for reaction in reactions[1:]:
handles.append(
draw_reaction(start,
reaction,
max_force=max_force,
color=GREEN))
print(
'Node: {} | Ground: {} | Neighbors: {} | Reactions: {} | Magnitude: {:.3E}'
.format(node,
(node in ground_nodes), len(neighbors_from_node[node]),
len(reactions), force_from_node[node]))
print('Total:', np.sum(reactions, axis=0))
wait_for_user()
#for handle in handles:
# remove_debug(handle)
#handles = []
#remove_all_debug()
# TODO: could compute the least balanced node with respect to original forces
# TODO: sum the norms of all the forces in the structure
#draw_sequence(sequence, node_points)
wait_for_user()
def main(floor_width=2.0):
# Creates a pybullet world and a visualizer for it
connect(use_gui=True)
set_camera_pose(camera_point=[1, -1, 1], target_point=unit_point()) # Sets the camera's position
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(TURTLEBOT_URDF) # TURTLEBOT_URDF | ROOMBA_URDF # Loads a robot from a *.urdf file
assign_link_colors(robot)
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 turtlebots 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_point(center))
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_if_gui() # Like raw_input() but also updates the viewer
# Destroys the pybullet world
disconnect()
def draw(self, **kwargs):
point_reference = point_from_pose(self.get_reference_from_body())
if self.reference_body is None:
return draw_point(point_reference, **kwargs)
return draw_point(point_reference, parent=self.reference_body,
parent_link=self.reference_link, **kwargs)
def solve_tsp(all_elements,
ground_nodes,
node_points,
printed,
initial_point,
final_point,
bidirectional=False,
layers=True,
max_time=30,
visualize=True,
verbose=False):
# https://developers.google.com/optimization/routing/tsp
# https://developers.google.com/optimization/reference/constraint_solver/routing/RoutingModel
# http://www.math.uwaterloo.ca/tsp/concorde.html
# https://developers.google.com/optimization/reference/python/constraint_solver/pywrapcp
# AddDisjunction
# TODO: pick up and delivery
# TODO: time window for skipping elements
# TODO: Minimum Spanning Tree (MST) bias
# TODO: time window constraint to ensure connected
# TODO: reuse by simply swapping out the first vertex
# arc-routing
from ortools.constraint_solver import routing_enums_pb2, pywrapcp
from extrusion.visualization import draw_ordered, draw_model
start_time = time.time()
assert initial_point is not None
remaining = all_elements - printed
#printed_nodes = compute_printed_nodes(ground_nodes, printed)
if not remaining:
cost = get_distance(initial_point, final_point)
return [], cost
# TODO: use as a lower bound
total_distance = compute_element_distance(node_points, remaining)
# TODO: some of these are invalid still
level_from_node, cost_from_edge, sequence = greedily_plan(
all_elements, node_points, ground_nodes, remaining, initial_point)
if sequence is None:
return None, INF
extrusion_edges = set()
point_from_vertex = {INITIAL_NODE: initial_point, FINAL_NODE: final_point}
frame_nodes = nodes_from_elements(remaining)
min_level = min(level_from_node[n] for n in frame_nodes)
max_level = max(level_from_node[n] for n in frame_nodes)
frame_keys = set()
keys_from_node = defaultdict(set)
for element in remaining:
mid = (element, element)
point_from_vertex[mid] = get_midpoint(node_points, element)
for node in element:
key = (element, node)
point_from_vertex[key] = node_points[node]
frame_keys.add(key)
keys_from_node[node].add(key)
for reverse in [True, False]:
directed = reverse_element(element) if reverse else element
node1, node2 = directed
#delta = node_points[node2] - node_points[node1]
#pitch = get_pitch(delta)
#upward = -SUPPORT_THETA <= pitch
# theta = angle_between(delta, [0, 0, -1])
# upward = theta < (np.pi / 2 - SUPPORT_THETA)
#if (directed in tree_elements): # or upward:
if bidirectional or (directed in cost_from_edge):
# Add edges from anything that is roughly the correct cost
start = (element, node1)
end = (element, node2)
extrusion_edges.update({(start, mid), (mid, end)})
for node in keys_from_node:
for edge in product(keys_from_node[node], repeat=2):
extrusion_edges.add(edge)
# Key thing is partial order on buckets of elements to adhere to height
# Connect v2 to v1 if v2 is the same level
# Traversing an edge might move to a prior level (but at most one)
transit_edges = set()
for directed in product(frame_keys, repeat=2):
key1, key2 = directed
element1, node1 = key1
#level1 = min(level_from_node[n] for n in element1)
level1 = level_from_node[get_other_node(node1, element1)]
_, node2 = key2
level2 = level_from_node[node2]
if level2 in [level1, level1 + 1]: # TODO: could bucket more coarsely
transit_edges.add(directed)
for key in frame_keys:
_, node = key
if level_from_node[node] == min_level:
transit_edges.add((INITIAL_NODE, key))
if level_from_node[node] in [max_level, max_level - 1]:
transit_edges.add((key, FINAL_NODE))
# TODO: can also remove restriction that elements are printed in a single direction
if not layers:
transit_edges.update(
product(frame_keys | {INITIAL_NODE, FINAL_NODE},
repeat=2)) # TODO: apply to greedy as well
key_from_index = list(
{k
for pair in extrusion_edges | transit_edges for k in pair})
edge_weights = {
pair: INVALID
for pair in product(key_from_index, repeat=2)
}
for k1, k2 in transit_edges | extrusion_edges:
p1, p2 = point_from_vertex[k1], point_from_vertex[k2]
edge_weights[k1, k2] = get_distance(p1, p2)
#edge_weights.update({e: 0. for e in extrusion_edges}) # frame edges are free
edge_weights[FINAL_NODE, INITIAL_NODE] = 0.
edge_weights[INITIAL_NODE,
FINAL_NODE] = INVALID # Otherwise might be backward
print(
'Elements: {} | Vertices: {} | Edges: {} | Structure: {:.3f} | Min Level {} | Max Level: {}'
.format(len(remaining), len(key_from_index), len(edge_weights),
total_distance, min_level, max_level))
index_from_key = dict(map(reversed, enumerate(key_from_index)))
num_vehicles, depot = 1, index_from_key[INITIAL_NODE]
manager = pywrapcp.RoutingIndexManager(len(key_from_index), num_vehicles,
depot)
#[depot], [depot])
solver = pywrapcp.RoutingModel(manager)
cost_from_index = {}
for (k1, k2), weight in edge_weights.items():
i1, i2 = index_from_key[k1], index_from_key[k2]
cost_from_index[i1, i2] = int(math.ceil(SCALE * weight))
solver.SetArcCostEvaluatorOfAllVehicles(
solver.RegisterTransitCallback(
lambda i1, i2: cost_from_index[manager.IndexToNode(
i1), manager.IndexToNode(i2)])) # from -> to
# sequence = plan_stiffness(None, None, node_points, ground_nodes, elements,
# initial_position=initial_point, stiffness=False, max_backtrack=INF)
initial_order = []
#initial_order = [INITIAL_NODE] # Start and end automatically included
for directed in sequence:
node1, node2 = directed
element = get_undirected(remaining, directed)
initial_order.extend([
(element, node1),
(element, element),
(element, node2),
])
initial_order.append(FINAL_NODE)
#initial_order.append(INITIAL_NODE)
initial_route = [index_from_key[key] for key in initial_order]
#index = initial_route.index(0)
#initial_route = initial_route[index:] + initial_route[:index] + [0]
initial_solution = solver.ReadAssignmentFromRoutes(
[initial_route], ignore_inactive_indices=True)
assert initial_solution is not None
#print_solution(manager, solver, initial_solution)
#print(solver.GetAllDimensionNames())
#print(solver.ComputeLowerBound())
objective = initial_solution.ObjectiveValue() / SCALE
invalid = int(objective / INVALID)
order = parse_solution(solver, manager, key_from_index,
initial_solution)[:-1]
ordered_pairs = get_pairs(order)
cost = sum(edge_weights[pair] for pair in ordered_pairs)
#print('Initial solution | Invalid: {} | Objective: {:.3f} | Cost: {:.3f} | Duration: {:.3f}s'.format(
# invalid, objective, cost, elapsed_time(start_time)))
if False and visualize: # and invalid
remove_all_debug()
draw_model(printed, node_points, None, color=BLACK)
draw_point(initial_point, color=BLACK)
draw_point(final_point, color=GREEN)
for pair in ordered_pairs:
if edge_weights[pair] == INVALID:
for key in pair:
draw_point(point_from_vertex[key], color=RED)
draw_ordered(ordered_pairs, point_from_vertex)
wait_for_user() # TODO: pause only if viewer
#sequence = extract_sequence(level_from_node, remaining, ordered_pairs)
#print(compute_sequence_distance(node_points, sequence, start=initial_point, end=final_point), total_distance+cost)
#print([cost_from_edge[edge] for edge in sequence])
#return sequence, cost
start_time = time.time()
search_parameters = pywrapcp.DefaultRoutingSearchParameters()
#search_parameters.solution_limit = 1
search_parameters.time_limit.seconds = int(max_time)
search_parameters.log_search = verbose
# AUTOMATIC | PATH_CHEAPEST_ARC | LOCAL_CHEAPEST_ARC | GLOBAL_CHEAPEST_ARC | LOCAL_CHEAPEST_INSERTION
#search_parameters.first_solution_strategy = (
# routing_enums_pb2.FirstSolutionStrategy.AUTOMATIC)
# AUTOMATIC | GREEDY_DESCENT | GUIDED_LOCAL_SEARCH | SIMULATED_ANNEALING | TABU_SEARCH | OBJECTIVE_TABU_SEARCH
#search_parameters.local_search_metaheuristic = (
# routing_enums_pb2.LocalSearchMetaheuristic.GREEDY_DESCENT)
#solution = solver.SolveWithParameters(search_parameters)
solution = solver.SolveFromAssignmentWithParameters(
initial_solution, search_parameters)
#print_solution(manager, solver, solution)
print('Status: {} | Text: {}'.format(solver.status(),
STATUS[solver.status()]))
if not solution:
print('Failure! Duration: {:.3f}s'.format(elapsed_time(start_time)))
return None, INF
objective = solution.ObjectiveValue() / SCALE
invalid = int(objective / INVALID)
order = parse_solution(solver, manager, key_from_index, solution)[:-1]
ordered_pairs = get_pairs(order) # + [(order[-1], order[0])]
#cost = compute_element_distance(point_from_vertex, ordered_pairs)
cost = sum(edge_weights[pair] for pair in ordered_pairs)
print(
'Final solution | Invalid: {} | Objective: {:.3f} | Cost: {:.3f} | Duration: {:.3f}s'
.format(invalid, objective, cost, elapsed_time(start_time)))
sequence = extract_sequence(level_from_node, remaining, ordered_pairs)
#print(compute_sequence_distance(node_points, sequence, start=initial_point, end=final_point)) #, total_distance+cost)
#print(sequence)
violations = 0
printed_nodes = compute_printed_nodes(ground_nodes, printed)
for directed in sequence:
if directed[0] not in printed_nodes:
#print(directed)
violations += 1
printed_nodes.update(directed)
print('Violations:', violations)
if visualize:
# TODO: visualize by weight
remove_all_debug()
draw_model(printed, node_points, None, color=BLACK)
draw_point(initial_point, color=BLACK)
draw_point(final_point, color=GREEN)
#tour_pairs = ordered_pairs + get_pairs(list(reversed(order)))
#draw_model(extrusion_edges - set(tour_pairs), point_from_vertex, ground_nodes, color=BLACK)
draw_ordered(ordered_pairs, point_from_vertex)
wait_for_user()
if sequence is None:
return None, INF
#print([cost_from_edge[edge] for edge in sequence])
return sequence, cost
def main():
# TODO: update this example
connect(use_gui=True)
with HideOutput():
pr2 = load_model(DRAKE_PR2_URDF)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']),
REST_LEFT_ARM)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']),
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']),
[0.2])
dump_body(pr2)
block = load_model(BLOCK_URDF, fixed_base=False)
set_point(block, [2, 0.5, 1])
target_point = point_from_pose(get_pose(block))
draw_point(target_point)
head_joints = joints_from_names(pr2, PR2_GROUPS['head'])
#head_link = child_link_from_joint(head_joints[-1])
#head_name = get_link_name(pr2, head_link)
head_name = 'high_def_optical_frame' # HEAD_LINK_NAME | high_def_optical_frame | high_def_frame
head_link = link_from_name(pr2, head_name)
#max_detect_distance = 2.5
max_register_distance = 1.0
distance_range = (max_register_distance / 2, max_register_distance)
base_generator = visible_base_generator(pr2, target_point, distance_range)
base_joints = joints_from_names(pr2, PR2_GROUPS['base'])
for i in range(5):
base_conf = next(base_generator)
set_joint_positions(pr2, base_joints, base_conf)
handles = [
add_line(point_from_pose(get_link_pose(pr2, head_link)),
target_point,
color=RED),
add_line(point_from_pose(
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))),
target_point,
color=BLUE),
]
# head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, )
head_conf = inverse_visibility(pr2,
target_point,
head_name=head_name,
head_joints=head_joints)
assert head_conf is not None
set_joint_positions(pr2, head_joints, head_conf)
print(get_detections(pr2))
# TODO: does this detect the robot sometimes?
detect_mesh, z = get_detection_cone(pr2, block)
detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5))
set_pose(detect_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25))
set_pose(view_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
wait_if_gui()
remove_body(detect_cone)
remove_body(view_cone)
disconnect()