def plan_head_traj(robot, head_conf):
# head_conf = get_joint_positions(robot, head_joints)
# head_path = [head_conf, hq.values]
head_joints = get_group_joints(robot, 'head')
head_path = plan_direct_joint_motion(robot,
head_joints,
head_conf,
obstacles=None,
self_collisions=False)
assert (head_path is not None)
return create_trajectory(robot, head_joints, head_path)
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 IK_FAST:
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 IK_FAST:
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_head_traj(task, head_conf):
robot = task.robot
obstacles = get_fixed_bodies(task) # TODO: movable objects
# head_conf = get_joint_positions(robot, head_joints)
# head_path = [head_conf, hq.values]
head_joints = get_group_joints(robot, 'head')
head_path = plan_direct_joint_motion(robot,
head_joints,
head_conf,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS)
assert (head_path is not None)
return create_trajectory(robot, head_joints, head_path)
def test(hq1, hq2):
if teleport:
ht = Trajectory([hq1, hq2])
else:
hq1.assign()
path = plan_direct_joint_motion(rover,
hq1.joints,
hq2.values,
self_collisions=False)
if path is None:
return None
ht = create_trajectory(rover, hq1.joints, path)
return (ht, )