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 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_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 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 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 test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
ik_joints = get_ik_joints(robot, info, tool_link)
start_pose = get_link_pose(robot, tool_link)
end_pose = multiply(start_pose, Pose(Point(z=-distance)))
handles = [
add_line(point_from_pose(start_pose),
point_from_pose(end_pose),
color=BLUE)
]
#handles.extend(draw_pose(start_pose))
#handles.extend(draw_pose(end_pose))
path = []
pose_path = list(
interpolate_poses(start_pose, end_pose, pos_step_size=0.01))
for i, pose in enumerate(pose_path):
print('Waypoint: {}/{}'.format(i + 1, len(pose_path)))
handles.extend(draw_pose(pose))
conf = next(
either_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
None)
if conf is None:
print('Failure!')
path = None
wait_for_user()
break
set_joint_positions(robot, ik_joints, conf)
path.append(conf)
wait_for_user()
# for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1):
# set_joint_positions(robot, joints[:len(conf)], conf)
# wait_for_user()
remove_handles(handles)
return path
def experimental_inverse_kinematics(robot,
link,
pose,
null_space=False,
max_iterations=200,
tolerance=1e-3):
(point, quat) = pose
# https://github.com/bulletphysics/bullet3/blob/389d7aaa798e5564028ce75091a3eac6a5f76ea8/examples/SharedMemory/PhysicsClientC_API.cpp
# https://github.com/bulletphysics/bullet3/blob/c1ba04a5809f7831fa2dee684d6747951a5da602/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
joints = get_joints(
robot) # Need to have all joints (although only movable returned)
movable_joints = get_movable_joints(robot)
current_conf = get_joint_positions(robot, joints)
# TODO: sample other values for the arm joints as the reference conf
min_limits = [get_joint_limits(robot, joint)[0] for joint in joints]
max_limits = [get_joint_limits(robot, joint)[1] for joint in joints]
#min_limits = current_conf
#max_limits = current_conf
#max_velocities = [get_max_velocity(robot, joint) for joint in joints] # Range of Jacobian
max_velocities = [10000] * len(joints)
# TODO: cannot have zero velocities
# TODO: larger definitely better for velocities
#damping = tuple(0.1*np.ones(len(joints)))
#t0 = time.time()
#kinematic_conf = get_joint_positions(robot, movable_joints)
for iterations in range(max_iterations): # 0.000863273143768 / iteration
# TODO: return none if no progress
if null_space:
kinematic_conf = p.calculateInverseKinematics(
robot,
link,
point,
quat,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=max_velocities,
restPoses=current_conf,
#jointDamping=damping,
)
else:
kinematic_conf = p.calculateInverseKinematics(
robot, link, point, quat)
if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)):
return None
set_joint_positions(robot, movable_joints, kinematic_conf)
link_point, link_quat = get_link_pose(robot, link)
if np.allclose(link_point, point, atol=tolerance) and np.allclose(
link_quat, quat, atol=tolerance):
#print iterations
break
else:
return None
if violates_limits(robot, movable_joints, kinematic_conf):
return None
#total_time = (time.time() - t0)
#print total_time
#print (time.time() - t0)/max_iterations
return kinematic_conf
def create_inverse_reachability2(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500):
tool_link = get_gripper_link(robot, arm)
problem = MockProblem(robot, fixed=[table], grasp_types=[grasp_type])
placement_gen_fn = get_stable_gen(problem)
grasp_gen_fn = get_grasp_gen(problem, collisions=True)
ik_ir_fn = get_ik_ir_gen(problem, max_attempts=max_attempts, learned=False, teleport=True)
placement_gen = placement_gen_fn(body, table)
grasps = list(grasp_gen_fn(body))
print('Grasps:', len(grasps))
# TODO: sample the torso height
# TODO: consider IK with respect to the torso frame
start_time = time.time()
gripper_from_base_list = []
while len(gripper_from_base_list) < num_samples:
[(p,)] = next(placement_gen)
(g,) = random.choice(grasps)
output = next(ik_ir_fn(arm, body, p, g), None)
if output is None:
print('Failed to find a solution after {} attempts'.format(max_attempts))
else:
(_, ac) = output
[at,] = ac.commands
at.path[-1].assign()
gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot))
gripper_from_base_list.append(gripper_from_base)
print('{} / {} [{:.3f}]'.format(
len(gripper_from_base_list), num_samples, elapsed_time(start_time)))
wait_if_gui()
return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
def create_inverse_reachability(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500):
tool_link = get_gripper_link(robot, arm)
robot_saver = BodySaver(robot)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
start_time = time.time()
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))
for attempt in range(max_attempts):
robot_saver.restore()
base_conf = next(uniform_pose_generator(robot, gripper_pose)) #, reachable_range=(0., 1.)))
#set_base_values(robot, base_conf)
set_group_conf(robot, 'base', base_conf)
if pairwise_collision(robot, table):
continue
grasp_conf = pr2_inverse_kinematics(robot, arm, gripper_pose) #, nearby_conf=USE_CURRENT)
#conf = inverse_kinematics(robot, link, gripper_pose)
if (grasp_conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot))
#wait_if_gui()
gripper_from_base_list.append(gripper_from_base)
print('{} / {} | {} attempts | [{:.3f}]'.format(
len(gripper_from_base_list), num_samples, attempt, elapsed_time(start_time)))
wait_if_gui()
break
else:
print('Failed to find a kinematic solution after {} attempts'.format(max_attempts))
return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
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 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(group=SE3):
connect(use_gui=True)
set_camera_pose(camera_point=SIZE*np.array([1., -1., 1.]))
# TODO: can also create all links and fix some joints
# TODO: SE(3) motion planner (like my SE(3) one) where some dimensions are fixed
obstacle = create_box(w=SIZE, l=SIZE, h=SIZE, color=RED)
#robot = create_cylinder(radius=0.025, height=0.1, color=BLUE)
obstacles = [obstacle]
collision_id, visual_id = create_shape(get_cylinder_geometry(radius=0.025, height=0.1), color=BLUE)
robot = create_flying_body(group, collision_id, visual_id)
body_link = get_links(robot)[-1]
joints = get_movable_joints(robot)
joint_from_group = dict(zip(group, joints))
print(joint_from_group)
#print(get_aabb(robot, body_link))
dump_body(robot, fixed=False)
custom_limits = {joint_from_group[j]: l for j, l in CUSTOM_LIMITS.items()}
# sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits)
# for i in range(10):
# conf = sample_fn()
# set_joint_positions(robot, joints, conf)
# wait_for_user('Iteration: {}'.format(i))
# return
initial_point = SIZE*np.array([-1., -1., 0])
#initial_point = -1.*np.ones(3)
final_point = -initial_point
initial_euler = np.zeros(3)
add_line(initial_point, final_point, color=GREEN)
initial_conf = np.concatenate([initial_point, initial_euler])
final_conf = np.concatenate([final_point, initial_euler])
set_joint_positions(robot, joints, initial_conf)
#print(initial_point, get_link_pose(robot, body_link))
#set_pose(robot, Pose(point=-1.*np.ones(3)))
path = plan_joint_motion(robot, joints, final_conf, obstacles=obstacles,
self_collisions=False, custom_limits=custom_limits)
if path is None:
disconnect()
return
for i, conf in enumerate(path):
set_joint_positions(robot, joints, conf)
point, quat = get_link_pose(robot, body_link)
#euler = euler_from_quat(quat)
euler = intrinsic_euler_from_quat(quat)
print(conf)
print(point, euler)
wait_for_user('Step: {}/{}'.format(i, len(path)))
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 compute_door_paths(world,
joint_name,
door_conf1,
door_conf2,
obstacles=set(),
teleport=False):
door_paths = []
if door_conf1 == door_conf2:
return door_paths
door_joint = joint_from_name(world.kitchen, joint_name)
door_joints = [door_joint]
# TODO: could unify with grasp path
door_extend_fn = get_extend_fn(world.kitchen,
door_joints,
resolutions=[DOOR_RESOLUTION])
door_path = [door_conf1.values] + list(
door_extend_fn(door_conf1.values, door_conf2.values))
if teleport:
door_path = [door_conf1.values, door_conf2.values]
# TODO: open until collision for the drawers
sign = world.get_door_sign(door_joint)
pull = (sign * door_path[0][0] < sign * door_path[-1][0])
# door_obstacles = get_descendant_obstacles(world.kitchen, door_joint)
for handle_grasp in get_handle_grasps(world, door_joint, pull=pull):
link, grasp, pregrasp = handle_grasp
handle_path = []
for door_conf in door_path:
set_joint_positions(world.kitchen, door_joints, door_conf)
# if any(pairwise_collision(door_obst, obst)
# for door_obst, obst in product(door_obstacles, obstacles)):
# return
handle_path.append(get_link_pose(world.kitchen, link))
# Collide due to adjacency
# TODO: check pregrasp path as well
# TODO: check gripper self-collisions with the robot
set_configuration(world.gripper, world.open_gq.values)
tool_path = [
multiply(handle_pose, invert(grasp)) for handle_pose in handle_path
]
for i, tool_pose in enumerate(tool_path):
set_joint_positions(world.kitchen, door_joints, door_path[i])
set_tool_pose(world, tool_pose)
# handles = draw_pose(handle_path[i], length=0.25)
# handles.extend(draw_aabb(get_aabb(world.kitchen, link=link)))
# wait_for_user()
# for handle in handles:
# remove_debug(handle)
if any(
pairwise_collision(world.gripper, obst)
for obst in obstacles):
break
else:
door_paths.append(
DoorPath(door_path, handle_path, handle_grasp, tool_path))
return door_paths
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 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 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 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 _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 test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
ik_joints = get_ik_joints(robot, info, tool_link)
start_pose = get_link_pose(robot, tool_link)
end_pose = multiply(start_pose, Pose(Point(z=-distance)))
for pose in interpolate_poses(start_pose, end_pose, pos_step_size=0.01):
conf = next(
closest_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
None)
if conf is None:
print('Failure!')
wait_for_user()
break
set_joint_positions(robot, ik_joints, conf)
wait_for_user()
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 solve_trac_ik(self, world_from_tool, nearby_tolerance=INF):
assert self.ik_solver is not None
init_lower, init_upper = self.ik_solver.get_joint_limits()
base_link = link_from_name(self.robot, self.ik_solver.base_link)
world_from_base = get_link_pose(self.robot, base_link)
tip_link = link_from_name(self.robot, self.ik_solver.tip_link)
tool_from_tip = multiply(
invert(get_link_pose(self.robot, self.tool_link)),
get_link_pose(self.robot, tip_link))
world_from_tip = multiply(world_from_tool, tool_from_tip)
base_from_tip = multiply(invert(world_from_base), world_from_tip)
joints = joints_from_names(
self.robot,
self.ik_solver.joint_names) # self.ik_solver.link_names
seed_state = get_joint_positions(self.robot, joints)
# seed_state = [0.0] * self.ik_solver.number_of_joints
lower, upper = init_lower, init_upper
if nearby_tolerance < INF:
tolerance = nearby_tolerance * np.ones(len(joints))
lower = np.maximum(lower, seed_state - tolerance)
upper = np.minimum(upper, seed_state + tolerance)
self.ik_solver.set_joint_limits(lower, upper)
(x, y, z), (rx, ry, rz, rw) = base_from_tip
# TODO: can also adjust tolerances
conf = self.ik_solver.get_ik(seed_state, x, y, z, rx, ry, rz, rw)
self.ik_solver.set_joint_limits(init_lower, init_upper)
if conf is None:
return conf
# if nearby_tolerance < INF:
# print(lower.round(3))
# print(upper.round(3))
# print(conf)
# print(get_difference(seed_state, conf).round(3))
set_joint_positions(self.robot, joints, conf)
return get_configuration(self.robot)
def test(object_name, pose, base_conf):
if object_name in ALL_SURFACES:
surface_name = object_name
if surface_name not in vertices_from_surface:
vertices_from_surface[surface_name] = grow_polygon(
map(point_from_pose,
load_inverse_placements(world, surface_name)),
radius=GROW_INVERSE_BASE)
if not vertices_from_surface[surface_name]:
return False
base_conf.assign()
pose.assign()
surface = surface_from_name(surface_name)
world_from_surface = get_link_pose(
world.kitchen, link_from_name(world.kitchen, surface.link))
world_from_base = get_link_pose(world.robot, world.base_link)
surface_from_base = multiply(invert(world_from_surface),
world_from_base)
#result = is_point_in_polygon(point_from_pose(surface_from_base), vertices_from_surface[surface_name])
#if not result:
# draw_pose(surface_from_base)
# points = [Point(x, y, 0) for x, y, in vertices_from_surface[surface_name]]
# add_segments(points, closed=True)
# wait_for_user()
return is_point_in_polygon(point_from_pose(surface_from_base),
vertices_from_surface[surface_name])
else:
if not base_from_objects:
return False
base_conf.assign()
pose.assign()
world_from_base = get_link_pose(world.robot, world.base_link)
world_from_object = pose.get_world_from_body()
base_from_object = multiply(invert(world_from_base),
world_from_object)
return is_point_in_polygon(point_from_pose(base_from_object),
base_from_objects)
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 gen(knob_name):
obstacles = world.static_obstacles
knob_link = link_from_name(world.kitchen, knob_name)
pose = get_link_pose(world.kitchen, knob_link)
#pose = RelPose(world.kitchen, knob_link, init=True)
presses = cycle(get_grasp_presses(world, knob_name))
grasp = next(presses)
gripper_pose = multiply(pose, invert(
grasp.grasp_pose)) # w_f_g = w_f_o * (g_f_o)^-1
if learned:
base_generator = cycle(load_pull_base_poses(world, knob_name))
else:
base_generator = uniform_pose_generator(world.robot, gripper_pose)
safe_base_generator = inverse_reachability(world,
base_generator,
obstacles=obstacles,
**kwargs)
while True:
for i in range(max_attempts):
try:
base_conf = next(safe_base_generator)
except StopIteration:
return
if base_conf is None:
yield None
continue
grasp = next(presses)
randomize = (random.random() < P_RANDOMIZE_IK)
ik_outputs = next(
plan_press(world,
knob_name,
pose,
grasp,
base_conf,
obstacles,
randomize=randomize,
**kwargs), None)
if ik_outputs is not None:
print('Press succeeded after {} attempts'.format(i))
yield (base_conf, ) + ik_outputs
break
else:
if PRINT_FAILURES:
print(
'Press failure after {} attempts'.format(max_attempts))
#if not pose.init:
# break
yield None
def get_ik_generator(robot, tool_pose, track_limits=False, prev_free_list=[]):
from .ikfast_abb_irb6600_track import get_ik
world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME))
base_from_tool = multiply(invert(world_from_base), tool_pose)
base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot))
sampled_limits = get_ik_limits(robot, joint_from_name(robot, *TRACK_JOINT),
track_limits)
while True:
if not prev_free_list:
sampled_values = [random.uniform(*sampled_limits)]
else:
sampled_values = prev_free_list
ik_joints = get_track_arm_joints(robot)
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 test(joint_name, base_conf):
if not DOOR_PROXIMITY:
return True
if joint_name not in vertices_from_joint:
base_confs = list(load_pull_base_poses(world, joint_name))
vertices_from_joint[joint_name] = grow_polygon(
base_confs, radius=GROW_INVERSE_BASE)
if not vertices_from_joint[joint_name]:
return False
# TODO: can't open hitman_drawer_top_joint any more
# Likely due to conservative carter geometry
base_conf.assign()
base_point = point_from_pose(
get_link_pose(world.robot, world.base_link))
return is_point_in_polygon(base_point[:2],
vertices_from_joint[joint_name])
def is_robot_visible(world, links):
for link in links:
link_point = point_from_pose(get_link_pose(world.robot, link))
visible = False
for camera_body, camera_matrix, camera_depth in world.cameras.values():
camera_pose = get_pose(camera_body)
#camera_point = point_from_pose(camera_pose)
#add_line(link_point, camera_point)
if is_visible_point(camera_matrix, camera_depth, link_point,
camera_pose):
visible = True
break
if not visible:
return False
#wait_for_user()
return True
def test_grasps(robot, block):
for arm in ARMS:
gripper_joints = get_gripper_joints(robot, arm)
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
tool_pose = get_link_pose(robot, tool_link)
#handles = draw_pose(tool_pose)
#grasps = get_top_grasps(block, under=True, tool_pose=unit_pose())
grasps = get_side_grasps(block, under=True, tool_pose=unit_pose())
for i, grasp_pose in enumerate(grasps):
block_pose = multiply(tool_pose, grasp_pose)
set_pose(block, block_pose)
close_until_collision(robot, gripper_joints, bodies=[block], open_conf=get_open_positions(robot, arm),
closed_conf=get_closed_positions(robot, arm))
handles = draw_pose(block_pose)
wait_if_gui('Grasp {}'.format(i))
remove_handles(handles)
