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, )