def sample_push_contact(world, feature, parameter, under=False):
robot = world.robot
arm = feature['arm_name']
body = world.get_body(feature['block_name'])
push_yaw = feature['push_yaw']
center, (width, _, height) = approximate_as_prism(body, body_pose=Pose(euler=Euler(yaw=push_yaw)))
max_backoff = width + 0.1 # TODO: add gripper bounding box
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
tool_pose = get_link_pose(robot, tool_link)
gripper_link = link_from_name(robot, GRIPPER_LINKS[arm])
collision_links = get_link_subtree(robot, gripper_link)
urdf_from_center = Pose(point=center)
reverse_z = Pose(euler=Euler(pitch=math.pi))
rotate_theta = Pose(euler=Euler(yaw=push_yaw))
#translate_z = Pose(point=Point(z=-feature['block_height']/2. + parameter['gripper_z'])) # Relative to base
translate_z = Pose(point=Point(z=parameter['gripper_z'])) # Relative to center
tilt_gripper = Pose(euler=Euler(pitch=parameter['gripper_tilt']))
grasps = []
for i in range(1 + under):
flip_gripper = Pose(euler=Euler(yaw=i * math.pi))
for x in np.arange(0, max_backoff, step=0.01):
translate_x = Pose(point=Point(x=-x))
grasp_pose = multiply(flip_gripper, tilt_gripper, translate_x, translate_z, rotate_theta,
reverse_z, invert(urdf_from_center))
set_pose(body, multiply(tool_pose, TOOL_POSE, grasp_pose))
if not link_pairs_collision(robot, collision_links, body, collision_buffer=0.):
grasps.append(grasp_pose)
break
return grasps
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 sample_tool_ik(robot, arm, world_from_target, max_attempts=10, **kwargs):
world_from_tool = get_link_pose(
robot, link_from_name(robot, PR2_TOOL_FRAMES[arm]))
world_from_ik = get_link_pose(robot, link_from_name(robot, IK_LINK[arm]))
tool_from_ik = multiply(invert(world_from_tool), world_from_ik)
ik_pose = multiply(world_from_target, tool_from_ik)
generator = get_ik_generator(robot, arm, ik_pose, **kwargs)
for _ in range(max_attempts):
try:
solutions = next(generator)
if solutions:
return random.choice(solutions)
except StopIteration:
break
return None
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 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_aabb(robot):
base_link = link_from_name(robot, BASE_LINK_NAME)
region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3))
draw_aabb(region_aabb)
# bodies = get_bodies_in_region(region_aabb)
# print(len(bodies), bodies)
# for body in get_bodies():
# set_pose(body, Pose())
#step_simulation() # Need to call before get_bodies_in_region
#update_scene()
for i in range(3):
with timer(message='{:f}'):
bodies = get_bodies_in_region(
region_aabb) # This does cache some info
print(i, len(bodies), bodies)
# https://github.com/bulletphysics/bullet3/search?q=broadphase
# https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93
# https://andysomogyi.github.io/mechanica/bullet.html
# http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual
aabb = get_aabb(robot)
# aabb = get_subtree_aabb(robot, base_link)
print(aabb)
draw_aabb(aabb)
for link in [BASE_LINK, base_link]:
print(link, get_collision_data(robot, link),
pairwise_link_collision(robot, link, robot, link))
def fn(o, j, a):
link = link_from_name(world.kitchen, o) # link not surface
p = RelPose(
world.kitchen, # link,
confs=[a],
init=a.init)
return (p, )
def get_disabled_collisions(robot):
return {
tuple(
link_from_name(robot, link) for link in pair
if has_link(robot, link))
for pair in DISABLED_COLLISIONS
}
def get_ik_generator(robot, tool_pose):
from .ikfast_kuka_kr6_r900 import get_ik
world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME))
#world_from_base == get_pose(robot)
base_from_tool = multiply(invert(world_from_base), tool_pose)
base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot))
yield compute_inverse_kinematics(get_ik, base_from_ik)
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 test_grasps(robot, node_points, elements):
element = elements[-1]
[element_body] = create_elements(node_points, [element])
phi = 0
link = link_from_name(robot, TOOL_NAME)
link_pose = get_link_pose(robot, link)
draw_pose(link_pose) #, parent=robot, parent_link=link)
wait_for_user()
n1, n2 = element
p1 = node_points[n1]
p2 = node_points[n2]
length = np.linalg.norm(p2 - p1)
# Bottom of cylinder is p1, top is p2
print(element, p1, p2)
for phi in np.linspace(0, np.pi, 10, endpoint=True):
theta = np.pi / 4
for t in np.linspace(-length / 2, length / 2, 10):
translation = Pose(
point=Point(z=-t)
) # Want to be moving backwards because +z is out end effector
direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi))
angle = Pose(euler=Euler(yaw=1.2))
grasp_pose = multiply(angle, direction, translation)
element_pose = multiply(link_pose, grasp_pose)
set_pose(element_body, element_pose)
wait_for_user()
def gen(knob_name, base_conf):
knob_link = link_from_name(world.kitchen, knob_name)
pose = get_link_pose(world.kitchen, knob_link)
presses = cycle(get_grasp_presses(world, knob_name))
max_failures = FIXED_FAILURES if world.task.movable_base else INF
failures = 0
while failures <= max_failures:
for i in range(max_attempts):
grasp = next(presses)
randomize = (random.random() < P_RANDOMIZE_IK)
ik_outputs = next(
plan_press(world,
knob_name,
pose,
grasp,
base_conf,
world.static_obstacles,
randomize=randomize,
**kwargs), None)
if ik_outputs is not None:
print('Fixed press succeeded after {} attempts'.format(i))
yield ik_outputs
break # return
else:
if PRINT_FAILURES:
print('Fixed pull failure after {} attempts'.format(
max_attempts))
yield None
max_failures += 1
def plan_workspace_motion(robot,
arm,
tool_path,
collision_buffer=COLLISION_BUFFER,
**kwargs):
_, resolutions = get_weights_resolutions(robot, arm)
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
arm_joints = get_arm_joints(robot, arm)
arm_waypoints = plan_cartesian_motion(robot,
arm_joints[0],
tool_link,
tool_path,
pos_tolerance=5e-3)
if arm_waypoints is None:
return None
arm_waypoints = [[conf[j] for j in movable_from_joints(robot, arm_joints)]
for conf in arm_waypoints]
set_joint_positions(robot, arm_joints, arm_waypoints[0])
return plan_waypoints_joint_motion(
robot,
arm_joints,
arm_waypoints[1:],
resolutions=resolutions,
max_distance=collision_buffer,
custom_limits=get_pr2_safety_limits(robot),
**kwargs)
def get_link_obstacles(world, link_name):
if link_name in world.movable:
return flatten_links(world.get_body(link_name))
elif has_link(world.kitchen, link_name):
link = link_from_name(world.kitchen, link_name)
return flatten_links(world.kitchen, get_link_subtree(world.kitchen, link)) # subtree?
assert link_name in SURFACE_FROM_NAME
return set()
def create_gripper(robot, arm):
# gripper = load_pybullet(os.path.join(get_data_path(), 'pr2_gripper.urdf'))
# gripper = load_pybullet(os.path.join(get_models_path(), 'pr2_description/pr2_l_gripper.urdf'), fixed_base=False)
links = get_link_subtree(robot, link_from_name(robot, GRIPPER_LINKS[arm]))
#print([get_link_name(robot, link) for link in links])
gripper = clone_body(robot, links=links, visual=True,
collision=True) # TODO: joint limits
return gripper
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 main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table1, Point(y=+0.5))
table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table2, Point(y=-0.5))
tables = [table1, table2]
#set_euler(table1, Euler(yaw=np.pi/2))
with HideOutput():
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
block_z = stable_z(block1, table1)
set_point(block1, Point(y=-0.5, z=block_z))
block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))
block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))
blocks = [block1, block2, block3]
set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z))
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)),
top_offset=0.02, grasp_length=0.03, under=False)[1:2]
for grasp in grasps:
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user()
wait_for_user('Finish?')
disconnect()
def get_tool_pose(robot):
from .ikfast_kuka_kr6_r900 import get_fk
ik_joints = get_movable_joints(robot)
conf = get_joint_positions(robot, ik_joints)
# TODO: this should be linked to ikfast's get numOfJoint function
assert len(conf) == 6
base_from_tool = compute_forward_kinematics(get_fk, conf)
world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME))
return multiply(world_from_base, base_from_tool)
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 get_link_path(self, link_name=TOOL_LINK):
link = link_from_name(self.robot, link_name)
if link not in self.path_from_link:
with BodySaver(self.robot):
self.path_from_link[link] = []
for conf in self.path:
set_joint_positions(self.robot, self.joints, conf)
self.path_from_link[link].append(
get_link_pose(self.robot, link))
return self.path_from_link[link]
def create_surface_attachment(world, obj_name, surface_name):
body = world.get_body(obj_name)
if surface_name in ENV_SURFACES:
surface_body = world.environment_bodies[surface_name]
surface_link = BASE_LINK
else:
surface = surface_from_name(surface_name)
surface_body = world.kitchen
surface_link = link_from_name(surface_body, surface.link)
return create_attachment(surface_body, surface_link, body)
def get_tool_pose(robot, arm):
from .ikfast_eth_rfl import get_fk
ik_joints = get_torso_arm_joints(robot, arm)
conf = get_joint_positions(robot, ik_joints)
# TODO: this should be linked to ikfast's get numOfJoint junction
base_from_ik = compute_forward_kinematics(get_fk, conf)
base_from_tool = multiply(base_from_ik,
invert(get_tool_from_ik(robot, arm)))
world_from_base = get_link_pose(robot,
link_from_name(robot, IK_BASE_FRAMES[arm]))
return multiply(world_from_base, base_from_tool)
def compute_grasp_width(robot, arm, body, grasp_pose, **kwargs):
gripper_joints = get_gripper_joints(robot, arm)
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
tool_pose = get_link_pose(robot, tool_link)
body_pose = multiply(tool_pose, grasp_pose)
set_pose(body, body_pose)
return close_until_collision(robot,
gripper_joints,
bodies=[body],
max_distance=0.0,
**kwargs)
def _create_robot(self):
with ClientSaver(self.client):
with HideOutput():
pr2_path = os.path.join(get_models_path(), PR2_URDF)
self.pr2 = load_pybullet(pr2_path, fixed_base=True)
# Base link is the origin
base_pose = get_link_pose(self.robot,
link_from_name(self.robot, BASE_FRAME))
set_pose(self.robot, invert(base_pose))
return self.pr2
def get_grasp_presses(world, knob, pre_distance=APPROACH_DISTANCE):
knob_link = link_from_name(world.kitchen, knob)
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
post_direction = unit_point()
for i, grasp_pose in enumerate(
get_top_presses(world.kitchen,
link=knob_link,
tool_pose=TOOL_POSE,
top_offset=FINGER_EXTENT[0] / 2 + 5e-3)):
pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose,
Pose(point=post_direction))
grasp = Grasp(world, knob, TOP_GRASP, i, grasp_pose, pregrasp_pose)
yield grasp
def create_gripper(robot, visual=False):
gripper_link = link_from_name(robot, get_gripper_link(robot))
links = get_link_descendants(robot, gripper_link) # get_link_descendants | get_link_subtree
with LockRenderer():
# Actually uses the parent of the first link
gripper = clone_body(robot, links=links, visual=False, collision=True) # TODO: joint limits
if not visual:
for link in get_all_links(gripper):
set_color(gripper, np.zeros(4), link)
#dump_body(robot)
#dump_body(gripper)
#user_input()
return gripper
def test_shelves(visualize):
arms = [LEFT_ARM]
# TODO: smaller (2) shelves
# TODO: sample with respect to the link it will supported by
# shelves.off | shelves_points.off | tableShelves.off
# import os
# data_directory = '/Users/caelan/Programs/LIS/git/lis-data/meshes/'
# mesh = read_mesh_off(os.path.join(data_directory, 'tableShelves.off'))
# draw_mesh(mesh)
# wait_for_interrupt()
start_link = 'shelf2' # shelf1 | shelf2 | shelf3 | shelf4
end_link = 'shelf1'
shelf_name = 'two_shelves'
#shelf_name = 'three_shelves'
cup_name = create_name('bluecup', 1)
world, table_body = create_world([shelf_name, cup_name], visualize=visualize)
cup_x = 0.65
#shelf_x = 0.8
shelf_x = 0.75
initial_poses = {
shelf_name: ([shelf_x, 0.3, 0.0], quat_from_euler(Euler(yaw=-np.pi/2))),
#cup_name: ([cup_x, 0.0, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
shelf_body = world.perception.sim_bodies[shelf_name]
#print([str(get_link_name(shelf_body, link)) for link in get_links(shelf_body)])
#print([str(get_link_name(shelf_body, link)) for link in get_links(world.perception.sim_bodies[cup_name])])
#shelf_link = None
shelf_link = link_from_name(shelf_body, start_link)
cup_z = stable_z(world.perception.sim_bodies[cup_name], shelf_body, surface_link=shelf_link) + Z_EPSILON
world.perception.set_pose(cup_name, [cup_x, 0.1, cup_z], unit_quat())
update_world(world, table_body, camera_point=np.array([-0.5, -1, 1.5]))
init = [
('Stackable', cup_name, shelf_name, end_link),
]
goal = [
('On', cup_name, shelf_name, end_link),
#('On', cup_name, TABLE_NAME, TOP),
#('Holding', cup_name),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def attach(self, arm, obj, **kwargs):
self.holding[arm] = obj
assert obj not in self.attachments
body = self.world.get_body(obj)
with ClientSaver(self.client):
if arm == 'l':
frame = "left_gripper"
elif arm == 'r':
frame = "right_gripper"
else:
raise ValueError("Arm should be l or r but was {}".format(arm))
robot_link = link_from_name(self.robot, PR2_TOOL_FRAMES[frame])
self.attachments[obj] = add_fixed_constraint(
body, self.robot, robot_link, max_force=self.attachment_force)
def get_ik_generator(robot, arm, tool_pose, **kwargs):
from .ikfast_eth_rfl import get_ik
world_from_base = get_link_pose(robot,
link_from_name(robot, IK_BASE_FRAMES[arm]))
base_from_tool = multiply(invert(world_from_base), tool_pose)
base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot, arm))
sampled_limits = get_ik_limits(robot, get_torso_joint(robot, arm),
**kwargs)
while True:
sampled_values = [random.uniform(*sampled_limits)]
ik_joints = get_torso_arm_joints(robot, arm)
confs = compute_inverse_kinematics(get_ik, base_from_ik,
sampled_values)
yield [q for q in confs if not violates_limits(robot, ik_joints, q)]
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])
