def test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
ik_joints = get_ik_joints(robot, info, tool_link)
start_pose = get_link_pose(robot, tool_link)
end_pose = multiply(start_pose, Pose(Point(z=-distance)))
handles = [
add_line(point_from_pose(start_pose),
point_from_pose(end_pose),
color=BLUE)
]
#handles.extend(draw_pose(start_pose))
#handles.extend(draw_pose(end_pose))
path = []
pose_path = list(
interpolate_poses(start_pose, end_pose, pos_step_size=0.01))
for i, pose in enumerate(pose_path):
print('Waypoint: {}/{}'.format(i + 1, len(pose_path)))
handles.extend(draw_pose(pose))
conf = next(
either_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
None)
if conf is None:
print('Failure!')
path = None
wait_for_user()
break
set_joint_positions(robot, ik_joints, conf)
path.append(conf)
wait_for_user()
# for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1):
# set_joint_positions(robot, joints[:len(conf)], conf)
# wait_for_user()
remove_handles(handles)
return path
def are_visible(world):
ray_names = []
rays = []
for name in world.movable:
for camera, info in world.cameras.items():
camera_pose = get_pose(info.body)
camera_point = point_from_pose(camera_pose)
point = point_from_pose(get_pose(world.get_body(name)))
if is_visible_point(CAMERA_MATRIX,
KINECT_DEPTH,
point,
camera_pose=camera_pose):
ray_names.append(name)
rays.append(Ray(camera_point, point))
ray_results = batch_ray_collision(rays)
visible_indices = [
idx for idx, (name, result) in enumerate(zip(ray_names, ray_results))
if result.objectUniqueId == world.get_body(name)
]
visible_names = {ray_names[idx] for idx in visible_indices}
print('Detected:', sorted(visible_names))
if has_gui():
handles = [
add_line(rays[idx].start, rays[idx].end, color=BLUE)
for idx in visible_indices
]
wait_for_duration(1.0)
remove_handles(handles)
# TODO: the object drop seems to happen around here
return visible_names
def score_fn(plan):
assert plan is not None
initial_distance = get_distance(
point_from_pose(initial_pose)[:2], goal_pos2d)
final_pose = world.perception.get_pose(block_name)
final_distance = get_distance(
point_from_pose(final_pose)[:2], goal_pos2d)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print(
'Initial: {:.5f} m | Final: {:.5f} | Rotation: {:.5f} rads'.format(
initial_distance, final_distance, quat_distance))
# TODO: compare orientation to final predicted orientation
# TODO: record simulation time in the event that the controller gets stuck
score = {
'initial_distance': initial_distance,
'final_distance': final_distance,
'rotation': quat_distance,
DYNAMICS: parameters_from_name,
}
#_, args = find_unique(lambda a: a[0] == 'push', plan)
#control = args[-1]
return score
def score_fn(plan):
assert plan is not None
final_pose = world.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
bowl_beads = get_contained_beads(bowl_body, init_beads)
fraction_bowl = float(
len(bowl_beads)) / len(init_beads) if init_beads else 0
mass_in_bowl = sum(map(get_mass, bowl_beads))
spoon_beads = get_contained_beads(world.get_body(spoon_name),
init_beads)
fraction_spoon = float(
len(spoon_beads)) / len(init_beads) if init_beads else 0
mass_in_spoon = sum(map(get_mass, spoon_beads))
print('In Bowl: {:.3f} | In Spoon: {:.3f}'.format(
fraction_bowl, fraction_spoon))
# TODO: measure change in roll/pitch
# TODO: could make latent parameters field
score = {
# Displacements
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
# Masses
'total_mass': init_mass,
'mass_in_bowl': mass_in_bowl,
'mass_in_spoon': mass_in_spoon,
'spoon_mass_capacity':
(init_mass / len(init_beads)) * spoon_capacity,
# Counts
'num_beads': len(init_beads),
'bowl_beads': len(bowl_beads),
'spoon_beads': len(spoon_beads),
'spoon_capacity': spoon_capacity,
# Fractions
'fraction_in_bowl': fraction_bowl,
'fraction_in_spoon': fraction_spoon,
# Latent
'bead_radius': bead_radius,
DYNAMICS: parameters_from_name
}
fraction_filled = float(score['spoon_beads']) / score['spoon_capacity']
spilled_beads = score['num_beads'] - (score['bowl_beads'] +
score['spoon_beads'])
fraction_spilled = float(spilled_beads) / score['num_beads']
print('Fraction Filled: {} | Fraction Spilled: {}'.format(
fraction_filled, fraction_spilled))
#_, args = find_unique(lambda a: a[0] == 'scoop', plan)
#control = args[-1]
return score
def main():
connect(use_gui=True)
with HideOutput():
pr2 = load_model(
"models/drake/pr2_description/urdf/pr2_simplified.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))
# head_link = link_from_name(pr2, HEAD_LINK)
head_joints = joints_from_names(pr2, PR2_GROUPS['head'])
head_link = head_joints[-1]
#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)
p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)),
target_point,
lineColorRGB=(1, 0, 0)) # addUserDebugText
p.addUserDebugLine(point_from_pose(
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))),
target_point,
lineColorRGB=(0, 0, 1)) # addUserDebugText
# head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, )
head_conf = inverse_visibility(pr2, target_point)
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_for_user()
remove_body(detect_cone)
remove_body(view_cone)
disconnect()
def get_water_path(bowl_body, bowl_pose, cup_body, pose_waypoints):
cup_pose = pose_waypoints[0]
bowl_origin_from_base = get_urdf_from_base(
bowl_body) # TODO: reference pose
cup_origin_from_base = get_urdf_from_base(cup_body)
ray_start = point_from_pose(multiply(cup_pose, cup_origin_from_base))
ray_end = point_from_pose(multiply(bowl_pose, bowl_origin_from_base))
water_path = interpolate_poses((ray_start, unit_quat()),
(ray_end, unit_quat()),
pos_step_size=0.01)
return water_path
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 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 relative_detections(belief, detections):
world = belief.world
rel_detections = {}
world_aabb = world.get_world_aabb()
for name in detections:
if name == belief.holding:
continue
body = world.get_body(name)
for observed_pose in detections[name]:
world_z_axis = np.array([0, 0, 1])
local_z_axis = tform_point(observed_pose, world_z_axis)
if np.pi / 2 < angle_between(world_z_axis, local_z_axis):
observed_pose = multiply(observed_pose,
Pose(euler=Euler(roll=np.pi)))
if not aabb_contains_point(point_from_pose(observed_pose),
world_aabb):
continue
set_pose(body, observed_pose)
support = world.get_supporting(name)
#assert support is not None
# Could also fix as relative to the world
if support is None:
# TODO: prune if nowhere near a surface (e.g. on the robot)
relative_pose = create_world_pose(world, name, init=True)
else:
relative_pose = create_relative_pose(world,
name,
support,
init=True)
rel_detections.setdefault(name, []).append(relative_pose)
# relative_pose.assign()
return rel_detections
def load_json_problem(problem_filename):
reset_simulation()
json_path = os.path.join(get_json_directory(), problem_filename)
with open(json_path, 'r') as f:
problem_json = json.loads(f.read())
set_camera_pose(point_from_pose(parse_pose(problem_json['camera'])))
task_json = problem_json['task']
important_bodies = []
for field in BODY_FIELDS:
important_bodies.extend(task_json[field])
robots = {
robot['name']: parse_robot(robot)
for robot in problem_json['robots']
}
bodies = {
body['name']: parse_body(body, (body['name'] in important_bodies))
for body in problem_json['bodies']
}
regions = {
region['name']: parse_region(region)
for region in task_json['regions']
}
#print(get_image())
return parse_task(task_json, robots, bodies, regions)
def fix_pose(self, name, pose=None, fraction=0.5):
body = self.get_body(name)
if pose is None:
pose = get_pose(body)
else:
set_pose(body, pose)
# TODO: can also drop in simulation
x, y, z = point_from_pose(pose)
roll, pitch, yaw = euler_from_quat(quat_from_pose(pose))
quat = quat_from_euler(Euler(yaw=yaw))
set_quat(body, quat)
surface_name = self.get_supporting(name)
if surface_name is None:
return None, None
if fraction == 0:
new_pose = (Point(x, y, z), quat)
return new_pose, surface_name
surface_aabb = compute_surface_aabb(self, surface_name)
new_z = (1 - fraction) * z + fraction * stable_z_on_aabb(
body, surface_aabb)
point = Point(x, y, new_z)
set_point(body, point)
print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format(
name, roll, pitch, new_z - z))
new_pose = (point, quat)
return new_pose, surface_name
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 at(self, time_from_start):
current_conf = super(PrintTrajectory, self).at(time_from_start)
if current_conf is None:
if self.last_point is not None:
#set_configuration(self.robot, INITIAL_CONF) # TODO: return here
end_point = point_from_pose(self.tool_path[-1])
self.handles.append(
add_line(self.last_point, end_point, color=RED))
self.last_point = None
else:
if self.last_point is None:
self.last_point = point_from_pose(self.tool_path[0])
current_point = point_from_pose(self.end_effector.get_tool_pose())
self.handles.append(
add_line(self.last_point, current_point, color=RED))
self.last_point = current_point
return current_conf
def score_fn(plan):
assert plan is not None
final_pose = world.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
# TODO: lift the bowl up (with particles around) to prevent scale detections
final_bowl_beads = get_contained_beads(bowl_body, init_beads)
fraction_bowl = safe_ratio(len(final_bowl_beads),
len(init_beads),
undefined=0)
mass_in_bowl = sum(map(get_mass, final_bowl_beads))
final_cup_beads = get_contained_beads(cup_body, init_beads)
fraction_cup = safe_ratio(len(final_cup_beads),
len(init_beads),
undefined=0)
mass_in_cup = sum(map(get_mass, final_cup_beads))
print('In Bowl: {} | In Cup: {}'.format(fraction_bowl, fraction_cup))
score = {
# Displacements
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
# Masses
'mass_in_bowl': mass_in_bowl,
'mass_in_cup': mass_in_cup,
# Counts
'bowl_beads': len(final_bowl_beads),
'cup_beads': len(final_cup_beads),
# Fractions
'fraction_in_bowl': fraction_bowl,
'fraction_in_cup': fraction_cup,
}
score.update(latent)
# TODO: store the cup path length to bias towards shorter paths
#_, args = find_unique(lambda a: a[0] == 'pour', plan)
#control = args[-1]
#feature = control['feature']
#parameter = control['parameter']
return score
def get_nearby(self, target_pose, radius=NEARBY_RADIUS):
# TODO: could instead use the probability density
target_point = np.array(
point_from_pose(target_pose.get_reference_from_body()))
draw_circle(target_point,
radius,
parent=target_pose.reference_body,
parent_link=target_pose.reference_link)
poses = set()
for pose in self.dist.support():
if target_pose.support != pose.support:
continue
point = point_from_pose(pose.get_reference_from_body())
delta = target_point - point
if np.linalg.norm(delta[:2]) < radius:
poses.add(pose)
prob = sum(map(self.discrete_prob, poses))
#poses = {target_pose}
return Neighborhood(poses, prob)
def compute_detectable(poses, camera_pose):
detectable_poses = set()
for pose in poses:
point = point_from_pose(pose.get_world_from_body())
if is_visible_point(CAMERA_MATRIX,
KINECT_DEPTH,
point,
camera_pose=camera_pose):
detectable_poses.add(pose)
return detectable_poses
def extract_point(loc):
if isinstance(loc, str):
name = loc.split('-')[0]
conf = initial_confs[name]
conf.assign()
robot = index_from_name(robots, name)
return point_from_pose(
get_link_pose(robot, link_from_name(robot, TOOL_LINK)))
else:
return node_points[loc]
def score_fn(plan):
assert plan is not None
with ClientSaver(world.client):
rgb_image = take_image(world, bowl_body, beads_per_color)
values = score_image(rgb_image, bead_colors, beads_per_color)
final_pose = perception.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
all_beads = list(flatten(beads_per_color))
bowl_beads = get_contained_beads(bowl_body, all_beads)
fraction_bowl = float(
len(bowl_beads)) / len(all_beads) if all_beads else 0
print('In Bowl: {}'.format(fraction_bowl))
with ClientSaver(world.client):
final_dispersion = compute_dispersion(bowl_body, beads_per_color)
print('Initial Dispersion: {:.3f} | Final Dispersion {:.3f}'.format(
initial_distance, final_dispersion))
score = {
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
'fraction_in_bowl': fraction_bowl,
'initial_dispersion': initial_distance,
'final_dispersion': final_dispersion,
'num_beads': len(all_beads), # Beads per color
DYNAMICS: parameters_from_name,
}
# TODO: include time required for stirring
# TODO: change in dispersion
#wait_for_user()
#_, args = find_unique(lambda a: a[0] == 'stir', plan)
#control = args[-1]
return score
def compute_visible(body, poses, camera_pose, draw=True):
ordered_poses = list(poses)
rays = []
camera_point = point_from_pose(camera_pose)
for pose in ordered_poses:
point = point_from_pose(pose.get_world_from_body())
rays.append(Ray(camera_point, point))
ray_results = batch_ray_collision(rays)
if draw:
with LockRenderer():
handles = []
for ray, result in zip(rays, ray_results):
handles.extend(draw_ray(ray, result))
# Blocking objects will likely be known with high probability
# TODO: move objects out of the way?
return {
pose
for pose, result in zip(ordered_poses, ray_results)
if result.objectUniqueId in (body, -1)
}
def score_poses(problem, task, ros_world):
cup_name, bowl_name = REQUIREMENT_FNS[problem](ros_world)
name_from_type = {'cup': cup_name, 'bowl': bowl_name}
initial_poses = {
ty: ros_world.get_pose(name)
for ty, name in name_from_type.items()
}
final_stackings = wait_until_observation(problem, ros_world)
#final_stackings = None
if final_stackings is None:
# TODO: only do this for the bad bowl
point_distances = {ty: INF for ty in name_from_type}
quat_distances = {ty: 2 * np.pi
for ty in name_from_type} # Max rotation
else:
final_poses = {
ty: ros_world.get_pose(name)
for ty, name in name_from_type.items()
}
point_distances = {
ty: get_distance(point_from_pose(initial_poses[ty]),
point_from_pose(final_poses[ty]))
for ty in name_from_type
}
# TODO: wrap around distance for symmetric things
quat_distances = {
ty: quat_angle_between(quat_from_pose(initial_poses[ty]),
quat_from_pose(final_poses[ty]))
for ty in name_from_type
}
score = {}
for ty in sorted(name_from_type):
print(
'{} | translation (m): {:.3f} | rotation (degrees): {:.3f}'.format(
ty, point_distances[ty], math.degrees(quat_distances[ty])))
score.update({
'{}_translation'.format(ty): point_distances[ty],
'{}_rotation'.format(ty): quat_distances[ty],
})
return score
def fn(obj_name, pose):
# TODO: incorporate probability mass
# Ether sample observation (control) or target belief (next state)
body = world.get_body(obj_name)
open_surface_joints(world, pose.support)
for camera_name in world.cameras:
camera_body, camera_matrix, camera_depth = world.cameras[
camera_name]
camera_pose = get_pose(camera_body)
camera_point = point_from_pose(camera_pose)
obj_point = point_from_pose(pose.get_world_from_body())
aabb = get_view_aabb(body, camera_pose)
center = get_aabb_center(aabb)
extent = np.multiply([detect_scale, detect_scale, 1],
get_aabb_extent(aabb))
view_aabb = (center - extent / 2, center + extent / 2)
# print(is_visible_aabb(view_aabb, camera_matrix=camera_matrix))
obj_points = apply_affine(
camera_pose, support_from_aabb(view_aabb)) + [obj_point]
# obj_points = [obj_point]
if not all(
is_visible_point(camera_matrix, camera_depth, point,
camera_pose) for point in obj_points):
continue
rays = [Ray(camera_point, point) for point in obj_points]
detect = Detect(world, camera_name, obj_name, pose, rays)
if ray_trace:
# TODO: how should doors be handled?
move_occluding(world)
open_surface_joints(world, pose.support)
detect.pose.assign()
if obstacles & detect.compute_occluding():
continue
#detect.draw()
#wait_for_user()
return (detect, )
return None
def inverse_kinematics(arm, pose, torso, upper):
from pybullet_tools.pr2_ik.ikLeft import leftIK
from pybullet_tools.pr2_ik.ikRight import rightIK
arm_ik = {
'left': leftIK,
'right': rightIK,
}
ik_fn = arm_ik[arm]
pos = point_from_pose(pose)
rot = matrix_from_quat(quat_from_pose(pose)).tolist()
solutions = ik_fn(list(rot), list(pos), [torso, upper])
if solutions is None:
return []
return solutions
def optimize_angle(end_effector,
element_pose,
translation,
direction,
reverse,
candidate_angles,
collision_fn,
nearby=True,
max_error=TRANSLATION_TOLERANCE):
robot = end_effector.robot
movable_joints = get_movable_joints(robot)
best_error, best_angle, best_conf = max_error, None, None
initial_conf = get_joint_positions(robot, movable_joints)
for angle in candidate_angles:
grasp_pose = get_grasp_pose(translation, direction, angle, reverse)
# Pose_{world,EE} = Pose_{world,element} * Pose_{element,EE}
# = Pose_{world,element} * (Pose_{EE,element})^{-1}
target_pose = multiply(element_pose, invert(grasp_pose))
set_pose(end_effector.body,
multiply(target_pose, end_effector.tool_from_ee))
if nearby:
set_joint_positions(robot, movable_joints, initial_conf)
conf = solve_ik(end_effector, target_pose, nearby=nearby)
if (conf is None) or collision_fn(conf):
continue
set_joint_positions(robot, movable_joints, conf)
link_pose = get_link_pose(robot, end_effector.tool_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
#break
if best_conf is not None:
set_joint_positions(robot, movable_joints, best_conf)
return best_angle, best_conf
def test(object_name, pose, base_conf):
if object_name in ALL_SURFACES:
surface_name = object_name
if surface_name not in vertices_from_surface:
vertices_from_surface[surface_name] = grow_polygon(
map(point_from_pose,
load_inverse_placements(world, surface_name)),
radius=GROW_INVERSE_BASE)
if not vertices_from_surface[surface_name]:
return False
base_conf.assign()
pose.assign()
surface = surface_from_name(surface_name)
world_from_surface = get_link_pose(
world.kitchen, link_from_name(world.kitchen, surface.link))
world_from_base = get_link_pose(world.robot, world.base_link)
surface_from_base = multiply(invert(world_from_surface),
world_from_base)
#result = is_point_in_polygon(point_from_pose(surface_from_base), vertices_from_surface[surface_name])
#if not result:
# draw_pose(surface_from_base)
# points = [Point(x, y, 0) for x, y, in vertices_from_surface[surface_name]]
# add_segments(points, closed=True)
# wait_for_user()
return is_point_in_polygon(point_from_pose(surface_from_base),
vertices_from_surface[surface_name])
else:
if not base_from_objects:
return False
base_conf.assign()
pose.assign()
world_from_base = get_link_pose(world.robot, world.base_link)
world_from_object = pose.get_world_from_body()
base_from_object = multiply(invert(world_from_base),
world_from_object)
return is_point_in_polygon(point_from_pose(base_from_object),
base_from_objects)
def test(joint_name, base_conf):
if not DOOR_PROXIMITY:
return True
if joint_name not in vertices_from_joint:
base_confs = list(load_pull_base_poses(world, joint_name))
vertices_from_joint[joint_name] = grow_polygon(
base_confs, radius=GROW_INVERSE_BASE)
if not vertices_from_joint[joint_name]:
return False
# TODO: can't open hitman_drawer_top_joint any more
# Likely due to conservative carter geometry
base_conf.assign()
base_point = point_from_pose(
get_link_pose(world.robot, world.base_link))
return is_point_in_polygon(base_point[:2],
vertices_from_joint[joint_name])
def is_robot_visible(world, links):
for link in links:
link_point = point_from_pose(get_link_pose(world.robot, link))
visible = False
for camera_body, camera_matrix, camera_depth in world.cameras.values():
camera_pose = get_pose(camera_body)
#camera_point = point_from_pose(camera_pose)
#add_line(link_point, camera_point)
if is_visible_point(camera_matrix, camera_depth, link_point,
camera_pose):
visible = True
break
if not visible:
return False
#wait_for_user()
return True
def gen_fn(obj, pose1, surface, region=None):
start_point = point_from_pose(pose1)
distance_range = (0.15, 0.2) if region is None else (0.15, 0.3)
obj_body = world.bodies[obj]
while True:
theta = random.uniform(-np.pi, np.pi)
distance = random.uniform(*distance_range)
end_point2d = np.array(start_point[:2]) + distance * unit_from_theta(theta)
end_pose = (np.append(end_point2d, [start_point[2]]), quat_from_pose(pose1))
set_pose(obj_body, end_pose)
if not is_center_stable(obj_body, world.bodies[surface], above_epsilon=np.inf):
continue
if region is not None:
assert region in ARMS
if not reachable_test(region, obj, end_pose):
continue
yield end_point2d,
def get_push_feature(world, arm, block_name, initial_pose, goal_pos2d):
block_body = world.get_body(block_name)
block_reference = get_reference_pose(block_name)
_, (block_w, block_l, block_h) = approximate_as_prism(block_body, body_pose=block_reference)
goal_pose = get_end_pose(initial_pose, goal_pos2d)
difference_initial = point_from_pose(multiply(invert(initial_pose), goal_pose))
feature = {
'arm_name': arm,
'block_name': block_name,
'block_type': get_type(block_name),
'block_width': block_w,
'block_length': block_l,
'block_height': block_h,
'push_yaw': get_yaw(difference_initial),
'push_distance': get_length(difference_initial)
}
return feature
def pose_from_pose2d(self, pose2d, surface):
#assert surface in self.poses_from_surface
#reference_pose = self.poses_from_surface[surface][0]
body = self.world.get_body(self.name)
surface_aabb = compute_surface_aabb(self.world, surface)
world_from_surface = get_surface_reference_pose(
self.world.kitchen, surface)
if DIM == 2:
x, y = pose2d[:DIM]
yaw = np.random.uniform(*CIRCULAR_LIMITS)
else:
x, y, yaw = pose2d
z = stable_z_on_aabb(
body,
surface_aabb) + Z_EPSILON - point_from_pose(world_from_surface)[2]
point = Point(x, y, z)
surface_from_body = Pose(point, Euler(yaw=yaw))
set_pose(body, multiply(world_from_surface, surface_from_body))
return create_relative_pose(self.world, self.name, surface)
def compute_assignments(robots, elements, node_points, initial_confs):
# TODO: print direction might influence the assignment
assignments = {name: set() for name in initial_confs}
for element in elements:
point = get_midpoint(node_points, element) # min/max
closest_robot, closest_distance = None, INF
for i, robot in enumerate(robots):
base_pose = get_pose(robot)
base_point = point_from_pose(base_pose)
point_base = tform_point(invert(base_pose), point)
distance = get_yaw(point_base) # which side its on
#distance = abs((base_point - point)[0]) # x distance
#distance = get_length((base_point - point)[:2]) # xy distance
if distance < closest_distance:
closest_robot, closest_distance = ROBOT_TEMPLATE.format(
i), distance
assert closest_robot is not None
# TODO: assign to several robots if close to the best distance
assignments[closest_robot].add(element)
return assignments
