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 USE_IKFAST:
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 USE_IKFAST:
# 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 plan_approach(end_effector,
print_traj,
collision_fn,
approach_distance=APPROACH_DISTANCE):
# TODO: collisions at the ends of elements
if approach_distance == 0:
return Command([print_traj])
robot = end_effector.robot
joints = get_movable_joints(robot)
extend_fn = get_extend_fn(robot,
joints,
resolutions=0.25 * JOINT_RESOLUTIONS)
tool_link = link_from_name(robot, TOOL_LINK)
approach_pose = Pose(Point(z=-approach_distance))
#element = print_traj.element
#midpoint = get_point(element)
#points = map(point_from_pose, [print_traj.tool_path[0], print_traj.tool_path[-1]])
#midpoint = np.average(list(points), axis=0)
#draw_point(midpoint)
#element_to_base = 0.05*get_unit_vector(get_point(robot) - midpoint)
#retreat_pose = Pose(Point(element_to_base))
# TODO: perpendicular to element
# TODO: solve_ik
start_conf = print_traj.path[0]
set_joint_positions(robot, joints, start_conf)
initial_pose = multiply(print_traj.tool_path[0], approach_pose)
#draw_pose(initial_pose)
#wait_if_gui()
initial_conf = inverse_kinematics(robot, tool_link, initial_pose)
if initial_conf is None:
return None
initial_path = [initial_conf] + list(extend_fn(initial_conf, start_conf))
if any(map(collision_fn, initial_path)):
return None
initial_traj = MotionTrajectory(robot, joints, initial_path)
end_conf = print_traj.path[-1]
set_joint_positions(robot, joints, end_conf)
final_pose = multiply(print_traj.tool_path[-1], approach_pose)
final_conf = inverse_kinematics(robot, tool_link, final_pose)
if final_conf is None:
return None
final_path = [end_conf] + list(extend_fn(
end_conf, final_conf)) # TODO: version that includes the endpoints
if any(map(collision_fn, final_path)):
return None
final_traj = MotionTrajectory(robot, joints, final_path)
return Command([initial_traj, print_traj, final_traj])
def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_user()
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_user()
def fn(body, pose, grasp):
obstacles = [body] + fixed
gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose)
approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose)
draw_pose(gripper_pose, length=0.04)
draw_pose(approach_pose, length=0.04)
for _ in range(num_attempts):
if USE_IKFAST:
q_approach = sample_tool_ik(robot, approach_pose)
if q_approach is not None:
set_joint_positions(robot, movable_joints, q_approach)
else:
set_joint_positions(robot, movable_joints, sample_fn()) # Random seed
q_approach = inverse_kinematics(robot, grasp.link, approach_pose)
if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles):
continue
conf = BodyConf(robot, joints=movable_joints)
if USE_IKFAST:
q_grasp = sample_tool_ik(robot, gripper_pose, closest_only=True)
if q_grasp is not None:
set_joint_positions(robot, movable_joints, q_grasp)
else:
q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose)
if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles):
continue
if teleport:
path = [q_approach, q_grasp]
else:
conf.assign()
#direction, _ = grasp.approach_pose
#path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction,
# quat_from_pose(approach_pose))
path = plan_direct_joint_motion(robot, conf.joints, q_grasp, obstacles=obstacles, \
self_collisions=self_collisions)
if path is None:
if DEBUG_FAILURE: user_input('Approach motion failed')
continue
command = Command([BodyPath(robot, path, joints=movable_joints),
Attach(body, robot, grasp.link),
BodyPath(robot, path[::-1], joints=movable_joints, attachments=[grasp])])
return (conf, command)
# TODO: holding collisions
return None
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_user()
def solve_ik(end_effector, target_pose, nearby=True):
robot = end_effector.robot
movable_joints = get_movable_joints(robot)
if USE_IKFAST:
# TODO: sample from the set of solutions
conf = sample_tool_ik(robot,
target_pose,
closest_only=nearby,
get_all=False)
else:
# TODO: repeat several times
# TODO: condition on current config
if not nearby:
# randomly sample and set joint conf for the pybullet ik fn
sample_fn = get_sample_fn(robot, movable_joints)
set_joint_positions(robot, movable_joints, sample_fn())
# note that the conf get assigned inside this ik fn right away!
conf = inverse_kinematics(robot, end_effector.tool_link, target_pose)
return conf
def create_inverse_reachability(robot, body, table, arm, grasp_type, num_samples=500):
link = get_gripper_link(robot, arm)
movable_joints = get_movable_joints(robot)
default_conf = get_joint_positions(robot, movable_joints)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
while len(gripper_from_base_list) < num_samples:
box_pose = sample_placement(body, table)
set_pose(body, box_pose)
grasp_pose = random.choice(grasps)
gripper_pose = multiply(box_pose, invert(grasp_pose))
set_joint_positions(robot, movable_joints, default_conf)
base_conf = next(uniform_pose_generator(robot, gripper_pose))
set_base_values(robot, base_conf)
if pairwise_collision(robot, table):
continue
conf = inverse_kinematics(robot, link, gripper_pose)
if (conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(invert(get_link_pose(robot, link)), get_pose(robot))
gripper_from_base_list.append(gripper_from_base)
print('{} / {}'.format(len(gripper_from_base_list), num_samples))
filename = IR_FILENAME.format(grasp_type, arm)
path = get_database_file(filename)
data = {
'filename': filename,
'robot': get_body_name(robot),
'grasp_type': grasp_type,
'arg': arm,
'carry_conf': get_carry_conf(arm, grasp_type),
'gripper_link': link,
'gripper_from_base': gripper_from_base_list,
}
write_pickle(path, data)
return path
def fn(body, pose, grasp):
obstacles = [body] + fixed
gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose)
approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose)
draw_pose(get_tool_pose(robot, ARM), length=0.04)
draw_pose(approach_pose, length=0.04)
draw_pose(gripper_pose, length=0.04)
# print(movable_joints)
# print(torso_arm)
# wait_for_interrupt()
# TODO: gantry_x_joint
# TODO: proper inverse reachability
base_link = link_from_name(robot, IK_BASE_FRAMES[ARM])
base_pose = get_link_pose(robot, base_link)
body_point_in_base = point_from_pose(multiply(invert(base_pose), pose.pose))
y_joint = joint_from_name(robot, '{}_gantry_y_joint'.format(prefix_from_arm(ARM)))
initial_y = get_joint_position(robot, y_joint)
ik_y = initial_y + SIGN_FROM_ARM[ARM]*body_point_in_base[1]
set_joint_position(robot, y_joint, ik_y)
for _ in range(num_attempts):
if USE_IKFAST:
q_approach = sample_tool_ik(robot, ARM, approach_pose)
if q_approach is not None:
set_joint_positions(robot, torso_arm, q_approach)
else:
set_joint_positions(robot, torso_arm, sample_fn()) # Random seed
q_approach = inverse_kinematics(robot, grasp.link, approach_pose)
if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles):
print('- ik for approaching fails!')
continue
conf = BodyConf(robot, joints=arm_joints)
if USE_IKFAST:
q_grasp = sample_tool_ik(robot, ARM, gripper_pose, closest_only=True)
if q_grasp is not None:
set_joint_positions(robot, torso_arm, q_grasp)
else:
conf.assign()
q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose)
if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles):
print('- ik for grasp fails!')
continue
if teleport:
path = [q_approach, q_grasp]
else:
conf.assign()
#direction, _ = grasp.approach_pose
#path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction,
# quat_from_pose(approach_pose))
path = plan_direct_joint_motion(robot, torso_arm, q_grasp, obstacles=obstacles,
self_collisions=self_collisions)
if path is None:
if DEBUG_FAILURE:
print('Approach motion failed')
continue
command = Command([BodyPath(robot, path, joints=torso_arm),
Attach(body, robot, grasp.link),
BodyPath(robot, path[::-1], joints=torso_arm, attachments=[grasp])])
return (conf, command)
# TODO: holding collisions
return None
