def optimize_angle(robot,
link,
element_pose,
translation,
direction,
reverse,
initial_angles,
collision_fn,
max_error=1e-2):
movable_joints = get_movable_joints(robot)
initial_conf = get_joint_positions(robot, movable_joints)
best_error, best_angle, best_conf = max_error, None, None
for i, angle in enumerate(initial_angles):
grasp_pose = get_grasp_pose(translation, direction, angle, reverse)
target_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, target_pose)
# if conf is None:
# continue
#if pairwise_collision(robot, robot):
conf = get_joint_positions(robot, movable_joints)
if not collision_fn(conf):
link_pose = get_link_pose(robot, link)
error = get_distance(point_from_pose(target_pose),
point_from_pose(link_pose))
if error < best_error: # TODO: error a function of direction as well
best_error, best_angle, best_conf = error, angle, conf
# wait_for_interrupt()
if i != len(initial_angles) - 1:
set_joint_positions(robot, movable_joints, initial_conf)
#print(best_error, translation, direction, best_angle)
if best_conf is not None:
set_joint_positions(robot, movable_joints, best_conf)
#wait_for_interrupt()
return best_angle, best_conf
def extract_point2d(v):
if isinstance(v, Conf):
return v.values[:2]
if isinstance(v, Pose):
return point_from_pose(v.value)[:2]
if v.stream == 'sample-pose':
r, = v.values
return point_from_pose(get_pose(r))[:2]
if v.stream == 'inverse-kinematics':
p, = v.values
return extract_point2d(p)
raise ValueError(v.stream)
def extract_point2d(v):
if isinstance(v, Conf):
return v.values[:2]
if isinstance(v, Pose):
return point_from_pose(v.value)[:2]
if v.stream == 'p-sp':
r, = v.inputs
return point_from_pose(get_pose(r))[:2]
if v.stream == 'q-ik':
p, = v.inputs
return extract_point2d(p)
return ValueError(v.stream)
def test(o, p, bq):
if o in task.rooms:
return True
target_xy = point_from_pose(p.value)[:2]
base_xy = bq.values[:2]
return range[0] <= get_length(np.array(target_xy) -
base_xy) <= range[1]
def gen(o, p):
# default_conf = arm_conf(a, g.carry)
# joints = get_arm_joints(robot, a)
# TODO: check collisions with fixed links
target_point = point_from_pose(p.value)
base_generator = visible_base_generator(robot, target_point,
base_range)
while True:
for _ in range(max_attempts):
set_pose(o, p.value)
base_conf = next(base_generator)
#set_base_values(robot, base_conf)
set_joint_positions(robot, base_joints, base_conf)
if any(pairwise_collision(robot, b) for b in fixed):
continue
head_conf = inverse_visibility(robot, target_point)
if head_conf is None: # TODO: test if visible
continue
#bq = Pose(robot, get_pose(robot))
bq = Conf(robot, base_joints, base_conf)
hq = Conf(robot, head_joints, head_conf)
yield (bq, hq)
break
else:
yield None
def gen(rover, objective):
base_joints = get_base_joints(rover)
target_point = get_point(objective)
base_generator = visible_base_generator(rover, target_point,
base_range)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
attempts = 0
while True:
if max_attempts <= attempts:
attempts = 0
yield None
attempts += 1
base_conf = next(base_generator)
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in obstacles):
continue
link_pose = get_link_pose(rover,
link_from_name(rover, KINECT_FRAME))
if use_cone:
mesh, _ = get_detection_cone(rover,
objective,
camera_link=KINECT_FRAME,
depth=max_range)
if mesh is None:
continue
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
local_pose = Pose()
else:
distance = get_distance(point_from_pose(link_pose),
target_point)
if max_range < distance:
continue
cone = create_cylinder(radius=0.01,
height=distance,
color=(0, 0, 1, 0.5))
local_pose = Pose(Point(z=distance / 2.))
set_pose(cone, multiply(link_pose, local_pose))
# TODO: colors corresponding to scanned object
if any(
pairwise_collision(cone, b) for b in obstacles
if b != objective and not is_placement(objective, b)):
# TODO: ensure that this works for the rover
remove_body(cone)
continue
if not reachable_test(rover, bq):
continue
print('Visibility attempts:', attempts)
y = Ray(cone, rover, objective)
yield Output(bq, y)
def gen(o, p):
if o in task.rooms: # TODO: predicate instead
return
# default_conf = arm_conf(a, g.carry)
# joints = get_arm_joints(robot, a)
# TODO: check collisions with fixed links
target_point = point_from_pose(p.value)
base_generator = visible_base_generator(robot, target_point,
base_range)
while True:
set_pose(o, p.value) # p.assign()
bq = Conf(robot, base_joints, next(base_generator))
# bq = Pose(robot, get_pose(robot))
bq.assign()
if any(pairwise_collision(robot, b) for b in fixed):
yield None
else:
yield (bq, )
def fn(o, p, bq):
set_pose(o, p.value) # p.assign()
bq.assign()
if o in task.rooms:
waypoints = plan_scan_path(task.robot, tilt=ROOM_SCAN_TILT)
set_group_conf(robot, 'head', waypoints[0])
path = plan_waypoints_joint_motion(robot,
head_joints,
waypoints[1:],
obstacles=None,
self_collisions=False)
if path is None:
return None
ht = create_trajectory(robot, head_joints, path)
hq = ht.path[0]
else:
target_point = point_from_pose(p.value)
head_conf = inverse_visibility(robot, target_point)
if head_conf is None: # TODO: test if visible
return None
hq = Conf(robot, head_joints, head_conf)
ht = Trajectory([hq])
return (hq, ht)
def display_trajectories(ground_nodes, trajectories, time_step=0.05):
if trajectories is None:
return
connect(use_gui=True)
floor, robot = load_world()
wait_for_interrupt()
movable_joints = get_movable_joints(robot)
#element_bodies = dict(zip(elements, create_elements(node_points, elements)))
#for body in element_bodies.values():
# set_color(body, (1, 0, 0, 0))
connected = set(ground_nodes)
for trajectory in trajectories:
if isinstance(trajectory, PrintTrajectory):
print(trajectory, trajectory.n1 in connected, trajectory.n2
in connected, is_ground(trajectory.element, ground_nodes),
len(trajectory.path))
connected.add(trajectory.n2)
#wait_for_interrupt()
#set_color(element_bodies[element], (1, 0, 0, 1))
last_point = None
handles = []
for conf in trajectory.path:
set_joint_positions(robot, movable_joints, conf)
if isinstance(trajectory, PrintTrajectory):
current_point = point_from_pose(
get_link_pose(robot, link_from_name(robot, TOOL_NAME)))
if last_point is not None:
color = (0, 0, 1) if is_ground(trajectory.element,
ground_nodes) else (1, 0, 0)
handles.append(
add_line(last_point, current_point, color=color))
last_point = current_point
wait_for_duration(time_step)
#wait_for_interrupt()
#user_input('Finish?')
wait_for_interrupt()
disconnect()
def __repr__(self):
return '{}({},{})'.format(self.__class__.__name__, self.index,
str(np.array(point_from_pose(self.value))))
