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 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 fn(conf1, conf2, fluents=[]):
if teleport is True:
path = [conf1, conf2]
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
obstacles = list(obstacle_from_name.values())
attachments = parse_fluents(robot, brick_from_index, fluents,
obstacles)
set_joint_positions(robot, movable_joints, conf1)
path = plan_joint_motion(
robot,
movable_joints,
conf2,
obstacles=obstacles,
attachments=attachments,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
#weights=weights, resolutions=resolutions,
restarts=5,
iterations=50,
smooth=100)
if path is None:
return None
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_interrupt()
phi = 0
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
grasp_rotations = [sample_direction() for _ in range(25)]
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for grasp_rotation in grasp_rotations:
n1, n2 = elements[0]
length = np.linalg.norm(node_points[n2] - node_points[n1])
set_joint_positions(robot, movable_joints, sample_fn())
for t in np.linspace(-length / 2, length / 2, 10):
#element_translation = Pose(point=Point(z=-t))
#grasp_pose = multiply(grasp_rotation, element_translation)
#reverse = Pose(euler=Euler())
reverse = Pose(euler=Euler(roll=np.pi))
grasp_pose = get_grasp_pose(t, grasp_rotation, 0)
grasp_pose = multiply(grasp_pose, reverse)
element_pose = get_pose(element_body)
link_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, link_pose)
wait_for_interrupt()
def fn(conf1, conf2, fluents=[]):
#path = [conf1, conf2]
obstacles = list(obstacle_from_name.values())
attachments = []
for fact in fluents:
if fact[0] == 'atpose':
index, pose = fact[1:]
body = brick_from_index[index].body
set_pose(body, pose.value)
obstacles.append(body)
elif fact[0] == 'atgrasp':
index, grasp = fact[1:]
body = brick_from_index[index].body
attachments.append(
Attachment(robot, tool_link, grasp.attach, body))
else:
raise NotImplementedError(fact[0])
set_joint_positions(robot, movable_joints, conf1)
path = plan_joint_motion(
robot,
movable_joints,
conf2,
obstacles=obstacles,
attachments=attachments,
self_collisions=True,
disabled_collisions=disabled_collisions,
#weights=weights, resolutions=resolutions,
restarts=5,
iterations=50,
smooth=100)
if path is None:
return None
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
def test_confs(robot, num_samples=10):
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
sample_fn = get_sample_fn(robot, joints)
for i in range(num_samples):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_interrupt()
def check_trajectory_collision(robot, trajectory, bodies):
# TODO: each new addition makes collision checking more expensive
#offset = 4
movable_joints = get_movable_joints(robot)
#for q in trajectory[offset:-offset]:
collisions = [False for _ in range(len(bodies))] # TODO: batch collision detection
for q in trajectory:
set_joint_positions(robot, movable_joints, q)
for i, body in enumerate(bodies):
if not collisions[i]:
collisions[i] |= pairwise_collision(robot, body)
return collisions
def test_ik(robot, node_order, node_points):
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for n in node_order:
point = node_points[n]
set_joint_positions(robot, movable_joints, sample_fn())
conf = inverse_kinematics(robot, link, (point, None))
if conf is not None:
set_joint_positions(robot, movable_joints, conf)
continue
link_point, link_quat = get_link_pose(robot, link)
#print(point, link_point)
#user_input('Continue?')
wait_for_interrupt()
def compute_motions(robot, fixed_obstacles, element_bodies, initial_conf,
trajectories):
# TODO: can just plan to initial and then shortcut
# TODO: backoff motion
# TODO: reoptimize for the sequence that have the smallest movements given this
# TODO: sample trajectories
# TODO: more appropriate distance based on displacement/volume
if trajectories is None:
return None
weights = np.array(JOINT_WEIGHTS)
resolutions = np.divide(0.005 * np.ones(weights.shape), weights)
movable_joints = get_movable_joints(robot)
disabled_collisions = {
tuple(link_from_name(robot, link) for link in pair)
for pair in DISABLED_COLLISIONS
}
printed_elements = []
current_conf = initial_conf
all_trajectories = []
for i, print_traj in enumerate(trajectories):
set_joint_positions(robot, movable_joints, current_conf)
goal_conf = print_traj.path[0]
obstacles = fixed_obstacles + [
element_bodies[e] for e in printed_elements
]
path = plan_joint_motion(robot,
movable_joints,
goal_conf,
obstacles=obstacles,
self_collisions=True,
disabled_collisions=disabled_collisions,
weights=weights,
resolutions=resolutions,
restarts=5,
iterations=50,
smooth=100)
if path is None:
print('Failed to find a path!')
return None
motion_traj = MotionTrajectory(robot, movable_joints, path)
print('{}) {}'.format(i, motion_traj))
all_trajectories.append(motion_traj)
current_conf = print_traj.path[-1]
printed_elements.append(print_traj.element)
all_trajectories.append(print_traj)
# TODO: return to initial?
return all_trajectories
def compute_direction_path(robot, length, reverse, element_body, direction,
collision_fn):
step_size = 0.0025 # 0.005
#angle_step_size = np.pi / 128
angle_step_size = np.math.radians(0.25)
angle_deltas = [-angle_step_size, 0, angle_step_size]
#num_initial = 12
num_initial = 1
steps = np.append(np.arange(-length / 2, length / 2, step_size),
[length / 2])
#print('Length: {} | Steps: {}'.format(length, len(steps)))
#initial_angles = [wrap_angle(angle) for angle in np.linspace(0, 2*np.pi, num_initial, endpoint=False)]
initial_angles = [
wrap_angle(angle)
for angle in np.random.uniform(0, 2 * np.pi, num_initial)
]
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
set_joint_positions(robot, movable_joints, sample_fn())
link = link_from_name(robot, TOOL_NAME)
element_pose = get_pose(element_body)
current_angle, current_conf = optimize_angle(robot, link, element_pose,
steps[0], direction, reverse,
initial_angles, collision_fn)
if current_conf is None:
return None
# TODO: constrain maximum conf displacement
# TODO: alternating minimization for just position and also orientation
trajectory = [current_conf]
for translation in steps[1:]:
#set_joint_positions(robot, movable_joints, current_conf)
initial_angles = [
wrap_angle(current_angle + delta) for delta in angle_deltas
]
current_angle, current_conf = optimize_angle(robot, link, element_pose,
translation, direction,
reverse, initial_angles,
collision_fn)
if current_conf is None:
return None
trajectory.append(current_conf)
return trajectory
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 gen_fn(index, pose, grasp):
body = brick_from_index[index].body
#world_pose = get_link_pose(robot, tool_link)
#draw_pose(world_pose, length=0.04)
#set_pose(body, multiply(world_pose, grasp.attach))
#draw_pose(multiply(pose.value, invert(grasp.attach)), length=0.04)
#wait_for_interrupt()
set_pose(body, pose.value)
for _ in range(max_attempts):
set_joint_positions(robot, movable_joints,
sample_fn()) # Random seed
attach_pose = multiply(pose.value, invert(grasp.attach))
attach_conf = inverse_kinematics(robot, tool_link, attach_pose)
if attach_conf is None:
continue
approach_pose = multiply(attach_pose, ([0, 0, -0.1], unit_quat()))
#approach_pose = multiply(pose.value, invert(grasp.approach))
approach_conf = inverse_kinematics(robot, tool_link, approach_pose)
if approach_conf is None:
continue
# TODO: retreat
path = plan_waypoints_joint_motion(
robot,
movable_joints, [approach_conf, attach_conf],
obstacles=obstacle_from_name.values(),
self_collisions=True,
disabled_collisions=disabled_collisions)
if path is None:
continue
#path = [approach_conf, attach_conf]
attachment = Attachment(robot, tool_link, grasp.attach, body)
traj = MotionTrajectory(robot,
movable_joints,
path,
attachments=[attachment])
yield approach_conf, traj
break
def gen_fn(index, pose, grasp):
body = brick_from_index[index].body
set_pose(body, pose.value)
obstacles = list(obstacle_from_name.values()) # + [body]
collision_fn = get_collision_fn(
robot,
movable_joints,
obstacles=obstacles,
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits=get_custom_limits(robot))
attach_pose = multiply(pose.value, invert(grasp.attach))
approach_pose = multiply(attach_pose, (approach_vector, unit_quat()))
# approach_pose = multiply(pose.value, invert(grasp.approach))
for _ in range(max_attempts):
if IK_FAST:
attach_conf = sample_tool_ik(robot, attach_pose)
else:
set_joint_positions(robot, movable_joints,
sample_fn()) # Random seed
attach_conf = inverse_kinematics(robot, tool_link, attach_pose)
if (attach_conf is None) or collision_fn(attach_conf):
continue
set_joint_positions(robot, movable_joints, attach_conf)
if IK_FAST:
approach_conf = sample_tool_ik(robot,
approach_pose,
nearby_conf=attach_conf)
else:
approach_conf = inverse_kinematics(robot, tool_link,
approach_pose)
if (approach_conf is None) or collision_fn(approach_conf):
continue
set_joint_positions(robot, movable_joints, approach_conf)
path = plan_direct_joint_motion(
robot,
movable_joints,
attach_conf,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions)
if path is None: # TODO: retreat
continue
#path = [approach_conf, attach_conf]
attachment = Attachment(robot, tool_link, grasp.attach, body)
traj = MotionTrajectory(robot,
movable_joints,
path,
attachments=[attachment])
yield approach_conf, traj
break
def iterate(self):
for conf in self.path[1:]:
set_joint_positions(self.robot, self.joints, conf)
yield
def set_base_conf(robot, conf):
set_joint_positions(robot, get_base_joints(robot), conf)
