def fn(a, o, p, g, bq):
gripper_pose = multiply(p.value,
invert(g.value)) # w_f_g = w_f_o * (g_f_o)^-1
approach_pose = multiply(g.approach, gripper_pose)
link = get_gripper_link(robot, a)
arm_joints = get_arm_joints(robot, a)
def ik_fn(target_pose):
try:
# TODO: switch to is_ik_compiled
from .pr2_ik.ik import sample_tool_ik, get_torso_arm_joints
joints = get_torso_arm_joints(robot, a)
torso_arm_conf = sample_tool_ik(robot,
a,
target_pose,
torso_limits=None)
if torso_arm_conf is None:
return None
set_joint_positions(robot, joints, torso_arm_conf)
except ImportError:
movable_conf = sub_inverse_kinematics(robot, arm_joints[0],
link, target_pose)
if (movable_conf is None) or any(
pairwise_collision(robot, b) for b in fixed):
return None
return get_joint_positions(robot, arm_joints)
default_conf = arm_conf(a, g.carry)
p.assign()
bq.assign()
# TODO: randomly sample initial position to make sampler?
# TODO: perturb this randomly
set_joint_positions(robot, arm_joints, default_conf)
approach_conf = ik_fn(approach_pose)
if approach_conf is None:
return None
grasp_conf = ik_fn(gripper_pose)
if grasp_conf is None:
return None
if teleport:
path = [default_conf, approach_conf, grasp_conf]
else:
#set_joint_positions(robot, arm_joints, approach_conf)
control_path = plan_joint_motion(robot,
arm_joints,
approach_conf,
obstacles=fixed,
self_collisions=False,
direct=True)
set_joint_positions(robot, arm_joints, approach_conf)
retreat_path = plan_joint_motion(robot,
arm_joints,
default_conf,
obstacles=fixed,
self_collisions=False)
path = retreat_path[::-1] + control_path[::-1]
mt = create_trajectory(robot, arm_joints, path)
return (mt, )
def assign_fluent_state(fluents):
obstacles = []
for fluent in fluents:
name, args = fluent[0], fluent[1:]
if name == 'atpose':
o, p = args
obstacles.append(o)
p.assign()
else:
raise ValueError(name)
return obstacles
def gen(body, surface):
# TODO: surface poses are being sampled in pr2_belief
if surface is None:
surfaces = problem.surfaces
else:
surfaces = [surface]
while True:
surface = random.choice(surfaces)
body_pose = sample_placement(body, surface)
if body_pose is None:
break
p = Pose(body, body_pose, surface)
p.assign()
if not any(pairwise_collision(body, obst) for obst in obstacles if obst not in {body, surface}):
yield (p,)
def sampler(a, o, p, g):
gripper_pose = multiply(p.value,
invert(g.value)) # w_f_g = w_f_o * (g_f_o)^-1
default_conf = arm_conf(a, g.carry)
arm_joints = get_arm_joints(robot, a)
base_joints = get_group_joints(robot, 'base')
if learned: # TODO: cache the learned distribution
base_generator = learned_pose_generator(robot,
gripper_pose,
arm=a,
grasp_type=g.grasp_type)
else:
base_generator = uniform_pose_generator(robot, gripper_pose)
base_conf = next(base_generator, None)
if base_conf is None:
return None
bq = Conf(robot, base_joints, base_conf)
p.assign()
bq.assign()
set_joint_positions(robot, arm_joints, default_conf)
if any(pairwise_collision(robot, b) for b in fixed):
return None
return (bq, )