def main():
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
with HideOutput():
with LockRenderer():
robot = load_model(FRANKA_URDF, fixed_base=True)
dump_body(robot)
print('Start?')
wait_for_user()
tool_link = link_from_name(robot, 'panda_hand')
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(10):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
test_retraction(robot,
PANDA_INFO,
tool_link,
max_distance=0.01,
max_time=0.05)
disconnect()
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 main():
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
#with HideOutput():
with LockRenderer():
robot = load_model(MOVO_URDF, fixed_base=True)
dump_body(robot)
print('Start?')
wait_for_user()
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
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(10):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
disconnect()
def main():
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
with LockRenderer():
with HideOutput():
robot = load_model(MOVO_URDF, fixed_base=True)
for link in get_links(robot):
set_color(robot,
color=apply_alpha(0.25 * np.ones(3), 1),
link=link)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
dump_body(robot)
wait_for_user('Start?')
#for arm in ARMS:
# test_close_gripper(robot, arm)
arm = 'right'
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
print(conf)
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_for_user()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_for_user()
test_retraction(robot,
MOVO_INFOS[arm],
tool_link,
fixed_joints=fixed_joints,
max_time=0.1)
disconnect()
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_user()
def get_ik_fn(robot, fixed=[], teleport=False, num_attempts=10, self_collisions=True):
movable_joints = get_track_arm_joints(robot) if USE_IKFAST else get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
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
return fn
def test_door(door):
door_joints = get_movable_joints(door)
sample_fn = get_sample_fn(door, door_joints)
set_joint_positions(door, door_joints, sample_fn())
while True:
positions = sample_fn()
set_joint_positions(door, door_joints, positions)
wait_if_gui()
lower, upper = get_joint_intervals(door, door_joints)
control_joints(door, door_joints, positions=lower)
velocity_control_joints(door, door_joints,
velocities=[PI / 4]) # Able to exceed limits
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 plan_water_motion(body,
joints,
end_conf,
attachment,
obstacles=None,
attachments=[],
self_collisions=True,
disabled_collisions=set(),
max_distance=MAX_DISTANCE,
weights=None,
resolutions=None,
reference_pose=unit_pose(),
custom_limits={},
**kwargs):
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
distance_fn = get_distance_fn(body, joints, weights=weights)
extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
collision_fn = get_collision_fn(body,
joints,
obstacles, {attachment} | set(attachments),
self_collisions,
disabled_collisions,
max_distance=max_distance,
custom_limits=custom_limits)
def water_test(q):
if attachment is None:
return False
set_joint_positions(body, joints, q)
attachment.assign()
attachment_pose = get_pose(attachment.child)
pose = multiply(attachment_pose,
reference_pose) # TODO: confirm not inverted
roll, pitch, _ = euler_from_quat(quat_from_pose(pose))
violation = (MAX_ROTATION < abs(roll)) or (MAX_ROTATION < abs(pitch))
#if violation: # TODO: check whether different confs can be waypoints for each object
# print(q, violation)
# wait_for_user()
return violation
invalid_fn = lambda q, **kwargs: water_test(q) or collision_fn(q, **kwargs)
start_conf = get_joint_positions(body, joints)
if not check_initial_end(start_conf, end_conf, invalid_fn):
return None
return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
invalid_fn, **kwargs)
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 main():
connect(use_gui=True)
add_data_path()
draw_pose(Pose(), length=1.)
set_camera_pose(camera_point=[1, -1, 1])
plane = p.loadURDF("plane.urdf")
with LockRenderer():
with HideOutput(True):
robot = load_pybullet(FRANKA_URDF, fixed_base=True)
assign_link_colors(robot, max_colors=3, s=0.5, v=1.)
#set_all_color(robot, GREEN)
obstacles = [plane] # TODO: collisions with the ground
dump_body(robot)
print('Start?')
wait_for_user()
info = PANDA_INFO
tool_link = link_from_name(robot, 'panda_hand')
draw_pose(Pose(), parent=robot, parent_link=tool_link)
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
check_ik_solver(info)
sample_fn = get_sample_fn(robot, joints)
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
#test_ik(robot, info, tool_link, get_link_pose(robot, tool_link))
test_retraction(robot,
info,
tool_link,
use_pybullet=False,
max_distance=0.1,
max_time=0.05,
max_candidates=100)
disconnect()
def plan_workspace(world,
tool_path,
obstacles,
randomize=True,
teleport=False):
# Assuming that pairs of fixed things aren't in collision at this point
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
distance_fn = get_distance_fn(world.robot, world.arm_joints)
if randomize:
sample_fn = get_sample_fn(world.robot, world.arm_joints)
set_joint_positions(world.robot, world.arm_joints, sample_fn())
else:
world.carry_conf.assign()
arm_path = []
for i, tool_pose in enumerate(tool_path):
#set_joint_positions(world.kitchen, [door_joint], door_path[i])
tolerance = INF if i == 0 else NEARBY_PULL
full_arm_conf = world.solve_inverse_kinematics(
tool_pose, nearby_tolerance=tolerance)
if full_arm_conf is None:
# TODO: this fails when teleport=True
if PRINT_FAILURES: print('Workspace kinematic failure')
return None
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
if PRINT_FAILURES: print('Workspace collision failure')
return None
arm_conf = get_joint_positions(world.robot, world.arm_joints)
if arm_path and not teleport:
distance = distance_fn(arm_path[-1], arm_conf)
if MAX_CONF_DISTANCE < distance:
if PRINT_FAILURES:
print(
'Workspace proximity failure (distance={:.5f})'.format(
distance))
return None
arm_path.append(arm_conf)
# wait_for_user()
return arm_path
def get_ik_gen_fn(robot,
brick_from_index,
obstacle_from_name,
max_attempts=25):
movable_joints = get_movable_joints(robot)
tool_link = link_from_name(robot, TOOL_NAME)
disabled_collisions = get_disabled_collisions(robot)
sample_fn = get_sample_fn(robot, movable_joints)
approach_distance = 0.1
#approach_distance = 0.0
approach_vector = approach_distance * np.array([0, 0, -1])
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
return gen_fn
def main(num_iterations=10):
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
side = 0.05
block = create_box(w=side, l=side, h=side, color=RED)
start_time = time.time()
with LockRenderer():
with HideOutput():
# TODO: MOVO must be loaded last
robot = load_model(MOVO_URDF, fixed_base=True)
#set_all_color(robot, color=MOVO_COLOR)
assign_link_colors(robot)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
print('Load time: {:.3f}'.format(elapsed_time(start_time)))
dump_body(robot)
#print(get_colliding(robot))
#for arm in ARMS:
# test_close_gripper(robot, arm)
#test_grasps(robot, block)
arm = RIGHT
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_info = MOVO_INFOS[arm]
print_ik_warning(ik_info)
ik_joints = get_ik_joints(robot, ik_info, tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
wait_if_gui('Start?')
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(num_iterations):
conf = sample_fn()
print('Iteration: {}/{} | Conf: {}'.format(i + 1, num_iterations,
np.array(conf)))
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_if_gui()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_if_gui()
test_retraction(robot,
ik_info,
tool_link,
fixed_joints=fixed_joints,
max_time=0.05,
max_candidates=100)
disconnect()
def plan_pick(world,
obj_name,
pose,
grasp,
base_conf,
obstacles,
randomize=True,
**kwargs):
# TODO: check if within database convex hull
# TODO: flag to check if initially in collision
obj_body = world.get_body(obj_name)
pose.assign()
base_conf.assign()
world.open_gripper()
robot_saver = BodySaver(world.robot)
obj_saver = BodySaver(obj_body)
if randomize:
sample_fn = get_sample_fn(world.robot, world.arm_joints)
set_joint_positions(world.robot, world.arm_joints, sample_fn())
else:
world.carry_conf.assign()
world_from_body = pose.get_world_from_body()
gripper_pose = multiply(world_from_body, invert(
grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1
full_grasp_conf = world.solve_inverse_kinematics(gripper_pose)
if full_grasp_conf is None:
if PRINT_FAILURES: print('Grasp kinematic failure')
return
moving_links = get_moving_links(world.robot, world.arm_joints)
robot_obstacle = (world.robot, frozenset(moving_links))
#robot_obstacle = get_descendant_obstacles(world.robot, child_link_from_joint(world.arm_joints[0]))
#robot_obstacle = world.robot
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
if PRINT_FAILURES: print('Grasp collision failure')
#set_renderer(enable=True)
#wait_for_user()
#set_renderer(enable=False)
return
approach_pose = multiply(world_from_body, invert(grasp.pregrasp_pose))
approach_path = plan_approach(
world,
approach_pose, # attachments=[grasp.get_attachment()],
obstacles=obstacles,
**kwargs)
if approach_path is None:
if PRINT_FAILURES: print('Approach plan failure')
return
if MOVE_ARM:
aq = FConf(world.robot, world.arm_joints, approach_path[0])
else:
aq = world.carry_conf
gripper_motion_fn = get_gripper_motion_gen(world, **kwargs)
finger_cmd, = gripper_motion_fn(world.open_gq, grasp.get_gripper_conf())
attachment = create_surface_attachment(world, obj_name, pose.support)
objects = [obj_name]
cmd = Sequence(State(world,
savers=[robot_saver, obj_saver],
attachments=[attachment]),
commands=[
ApproachTrajectory(objects, world, world.robot,
world.arm_joints, approach_path),
finger_cmd.commands[0],
Detach(world, attachment.parent, attachment.parent_link,
attachment.child),
AttachGripper(world, obj_body, grasp=grasp),
ApproachTrajectory(objects, world, world.robot,
world.arm_joints,
reversed(approach_path)),
],
name='pick')
yield (
aq,
cmd,
)
def compute_motion(robot, fixed_obstacles, element_bodies,
printed_elements, start_conf, end_conf,
collisions=True, max_time=INF, buffer=0.1, smooth=100):
# TODO: can also just plan to initial conf and then shortcut
joints = get_movable_joints(robot)
assert len(joints) == len(end_conf)
weights = JOINT_WEIGHTS
resolutions = np.divide(RESOLUTION * np.ones(weights.shape), weights)
disabled_collisions = get_disabled_collisions(robot)
custom_limits = {}
#element_from_body = {b: e for e, b in element_bodies.items()}
hulls, obstacles = {}, []
if collisions:
element_obstacles = {element_bodies[e] for e in printed_elements}
obstacles = set(fixed_obstacles) | element_obstacles
#hulls, obstacles = decompose_structure(fixed_obstacles, element_bodies, printed_elements)
#print(hulls)
#print(obstacles)
#wait_for_user()
#printed_elements = set(element_bodies)
bounding = None
if printed_elements:
# TODO: pass in node_points
bounding = create_bounding_mesh(printed_elements, element_bodies=element_bodies, node_points=None,
buffer=0.01) # buffer=buffer)
#wait_for_user()
sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits)
distance_fn = get_distance_fn(robot, joints, weights=weights)
extend_fn = get_extend_fn(robot, joints, resolutions=resolutions)
#collision_fn = get_collision_fn(robot, joints, obstacles, attachments={}, self_collisions=SELF_COLLISIONS,
# disabled_collisions=disabled_collisions, custom_limits=custom_limits, max_distance=0.)
collision_fn = get_element_collision_fn(robot, obstacles)
fine_extend_fn = get_extend_fn(robot, joints, resolutions=1e-1*resolutions) #, norm=INF)
def test_bounding(q):
set_joint_positions(robot, joints, q)
collision = (bounding is not None) and pairwise_collision(robot, bounding, max_distance=buffer) # body_collision
return q, collision
def dynamic_extend_fn(q_start, q_end):
# TODO: retime trajectories to be move more slowly around the structure
for (q1, c1), (q2, c2) in get_pairs(map(test_bounding, extend_fn(q_start, q_end))):
# print(c1, c2, len(list(fine_extend_fn(q1, q2))))
# set_joint_positions(robot, joints, q2)
# wait_for_user()
if c1 and c2:
for q in fine_extend_fn(q1, q2):
# set_joint_positions(robot, joints, q)
# wait_for_user()
yield q
else:
yield q2
def element_collision_fn(q):
if collision_fn(q):
return True
#for body in get_bodies_in_region(get_aabb(robot)): # Perform per link?
# if (element_from_body.get(body, None) in printed_elements) and pairwise_collision(robot, body):
# return True
for hull, bodies in hulls.items():
if pairwise_collision(robot, hull) and any(pairwise_collision(robot, body) for body in bodies):
return True
return False
path = None
if check_initial_end(start_conf, end_conf, collision_fn):
path = birrt(start_conf, end_conf, distance_fn, sample_fn, dynamic_extend_fn, element_collision_fn,
restarts=50, iterations=100, smooth=smooth, max_time=max_time)
# path = plan_joint_motion(robot, joints, end_conf, obstacles=obstacles,
# self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions,
# weights=weights, resolutions=resolutions,
# restarts=50, iterations=100, smooth=100)
# if (bounding is not None) and (path is not None):
# for i, q in enumerate(path):
# print('{}/{}'.format(i, len(path)))
# set_joint_positions(robot, joints, q)
# wait_for_user()
if bounding is not None:
remove_body(bounding)
for hull in hulls:
remove_body(hull)
if path is None:
print('Failed to find a motion plan!')
return None
return MotionTrajectory(robot, joints, path)
def get_ik_fn(robot, fixed=[], teleport=False, num_attempts=10, self_collisions=True):
# movable_joints = get_movable_joints(robot)
torso_arm = get_torso_arm_joints(robot, ARM)
arm_joints = joints_from_names(robot, get_arm_joint_names(ARM))
sample_fn = get_sample_fn(robot, torso_arm)
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
return fn
def plan_press(world,
knob_name,
pose,
grasp,
base_conf,
obstacles,
randomize=True,
**kwargs):
base_conf.assign()
world.close_gripper()
robot_saver = BodySaver(world.robot)
if randomize:
sample_fn = get_sample_fn(world.robot, world.arm_joints)
set_joint_positions(world.robot, world.arm_joints, sample_fn())
else:
world.carry_conf.assign()
gripper_pose = multiply(pose, invert(
grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1
#set_joint_positions(world.gripper, get_movable_joints(world.gripper), world.closed_gq.values)
#set_tool_pose(world, gripper_pose)
full_grasp_conf = world.solve_inverse_kinematics(gripper_pose)
#wait_for_user()
if full_grasp_conf is None:
# if PRINT_FAILURES: print('Grasp kinematic failure')
return
robot_obstacle = (world.robot,
frozenset(get_moving_links(world.robot,
world.arm_joints)))
if any(pairwise_collision(robot_obstacle, b) for b in obstacles):
#if PRINT_FAILURES: print('Grasp collision failure')
return
approach_pose = multiply(pose, invert(grasp.pregrasp_pose))
approach_path = plan_approach(world,
approach_pose,
obstacles=obstacles,
**kwargs)
if approach_path is None:
return
aq = FConf(world.robot, world.arm_joints,
approach_path[0]) if MOVE_ARM else world.carry_conf
#gripper_motion_fn = get_gripper_motion_gen(world, **kwargs)
#finger_cmd, = gripper_motion_fn(world.open_gq, world.closed_gq)
objects = []
cmd = Sequence(
State(world, savers=[robot_saver]),
commands=[
#finger_cmd.commands[0],
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
approach_path),
ApproachTrajectory(objects, world, world.robot, world.arm_joints,
reversed(approach_path)),
#finger_cmd.commands[0].reverse(),
Wait(world, duration=1.0),
],
name='press')
yield (
aq,
cmd,
)
