Пример #1
0
def workspace_traj(oracle, vector, manip_name=None):  # TODO - use IK to check if even possibly valid
    distance, direction = length(vector), normalize(vector)
    steps = 10  # NOTE - relates into the number of analytical IK calls. 10 seems to be a magic number.
    step_length = distance / steps
    with oracle.robot:
        if manip_name is not None:
            oracle.robot.SetActiveManipulator(manip_name)
        oracle.robot.SetActiveDOFs(get_arm_indices(oracle))
        with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs):
            try:  # TODO - Bug in specifying minsteps. Need to specify at least 2 times the desired otherwise it stops early
                return TrajTrajectory(
                    CSpace.robot_arm(oracle.base_manip.robot.GetActiveManipulator()),
                    oracle.base_manip.MoveHandStraight(
                        direction,
                        minsteps=10 * steps,
                        maxsteps=steps,
                        steplength=step_length,
                        ignorefirstcollision=None,
                        starteematrix=None,
                        greedysearch=True,
                        execute=False,
                        outputtraj=None,
                        maxdeviationangle=None,
                        planner=None,
                        outputtrajobj=True,
                    ),
                )
            except planning_error:
                return None
def inverse_kinematics(oracle, manip_trans): # 0.005 sec
  with oracle.robot:
    oracle.robot.SetActiveDOFs(get_arm_indices(oracle)) # TODO - is this slow and/or necessary?
    with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs):
      config = oracle.robot.GetActiveManipulator().FindIKSolution(manip_trans, 0)
      if config is None: return None
      set_config(oracle.robot, config, oracle.robot.GetActiveManipulator().GetArmIndices())
      if robot_collision(oracle): return None
      return config
Пример #3
0
def vector_traj(oracle, vector, manip_name=None):
    with oracle.robot:
        if manip_name is not None:
            oracle.robot.SetActiveManipulator(manip_name)
        oracle.robot.SetActiveDOFs(get_arm_indices(oracle))
        with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs):
            end_config = inverse_kinematics(oracle, vector_trans(get_manip_trans(oracle), vector))
            if end_config is None:
                return None
            return linear_arm_traj(oracle, end_config, manip_name=manip_name)
Пример #4
0
def linear_arm_traj(oracle, end_config, manip_name=None):
    with oracle.robot:
        if manip_name is not None:
            oracle.robot.SetActiveManipulator(manip_name)
        oracle.robot.SetActiveDOFs(get_arm_indices(oracle))
        with collision_saver(oracle.env, openravepy_int.CollisionOptions.ActiveDOFs):
            start_config = get_active_config(oracle.robot)
            extend = extend_fn(oracle.robot)
            collision = collision_fn(oracle.env, oracle.robot)
            path = [start_config] + list(extend(start_config, end_config))
            if any(collision(q) for q in path):
                return None
            return PathTrajectory(CSpace.robot_arm(oracle.robot.GetActiveManipulator()), path)