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 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 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 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 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()