示例#1
0
def get_ompl_rrtconnect_traj(env, robot, active_dof, init_dof, end_dof):
    # assert body in env.GetRobot()
    dof_inds = robot.GetActiveDOFIndices()
    robot.SetActiveDOFs(active_dof)
    robot.SetActiveDOFValues(init_dof)

    params = Planner.PlannerParameters()
    params.SetRobotActiveJoints(robot)
    params.SetGoalConfig(end_dof)  # set goal to all ones
    # forces parabolic planning with 40 iterations
    planner = RaveCreatePlanner(env, 'OMPL_RRTConnect')
    planner.InitPlan(robot, params)
    traj = RaveCreateTrajectory(env, '')
    planner.PlanPath(traj)

    traj_list = []
    for i in range(traj.GetNumWaypoints()):
        # get the waypoint values, this holds velocites, time stamps, etc
        data = traj.GetWaypoint(i)
        # extract the robot joint values only
        dofvalues = traj.GetConfigurationSpecification().ExtractJointValues(
            data, robot, robot.GetActiveDOFIndices())
        # raveLogInfo('waypint %d is %s'%(i,np.round(dofvalues, 3)))
        traj_list.append(np.round(dofvalues, 3))
    robot.SetActiveDOFs(dof_inds)
    return traj_list
示例#2
0
    def CollisionCheckPath(self, traj):
        from openravepy import Interval, Planner

        # NOTE: This assumes that the trajectory only contains joint_values.
        OpenStart = Interval.OpenStart

        params = Planner.PlannerParameters()
        params.SetRobotActiveJoints(self.robot)

        # Check the first waypoint for collision. We do this outside of the
        # loop so we can run all collision checks with OpenEnd. This prevents
        # us from collision checking the intermediate waypoints twice.
        prev_waypoint = traj.GetWaypoint(0)
        check = params.CheckPathAllConstraints(prev_waypoint, prev_waypoint,
                                               [], [], 0., OpenStart)
        if check != 0:
            return True

        # Check the remainder of the path.
        for iwaypoint in xrange(1, traj.GetNumWaypoints() - 1):
            curr_waypoint = traj.GetWaypoint(iwaypoint)

            check = params.CheckPathAllConstraints(prev_waypoint,
                                                   curr_waypoint, [], [], 0.,
                                                   OpenStart)
            if check != 0:
                return True

            prev_waypoint = curr_waypoint

        return False
示例#3
0
def get_rrt_traj(env, robot, active_dof, init_dof, end_dof):
    # assert body in env.GetRobot()
    active_dofs = robot.GetActiveDOFIndices()
    robot.SetActiveDOFs(active_dof)
    robot.SetActiveDOFValues(init_dof)

    params = Planner.PlannerParameters()
    params.SetRobotActiveJoints(robot)
    params.SetGoalConfig(end_dof)  # set goal to all ones
    # # forces parabolic planning with 40 iterations
    # import ipdb; ipdb.set_trace()
    params.SetExtraParameters("""<_postprocessing planner="parabolicsmoother">
        <_nmaxiterations>20</_nmaxiterations>
    </_postprocessing>""")

    planner = RaveCreatePlanner(env, 'birrt')
    planner.InitPlan(robot, params)

    traj = RaveCreateTrajectory(env, '')
    result = planner.PlanPath(traj)
    if result == False:
        robot.SetActiveDOFs(active_dofs)
        return None
    traj_list = []
    for i in range(traj.GetNumWaypoints()):
        # get the waypoint values, this holds velocites, time stamps, etc
        data = traj.GetWaypoint(i)
        # extract the robot joint values only
        dofvalues = traj.GetConfigurationSpecification().ExtractJointValues(
            data, robot, robot.GetActiveDOFIndices())
        # raveLogInfo('waypint %d is %s'%(i,np.round(dofvalues, 3)))
        traj_list.append(np.round(dofvalues, 3))
    robot.SetActiveDOFs(active_dofs)
    return np.array(traj_list)
    def PlanPath(self, planner_name):

        params = Planner.PlannerParameters()
        params.SetRobotActiveJoints(self.robot_)

        init = self.env_.RobotGetInitialPosition()
        goal = self.env_.RobotGetGoalPosition()

        params.SetInitialConfig(init)
        params.SetGoalConfig(goal)
        planner = RaveCreatePlanner(self.env_.env, planner_name)

        if planner is None:
            print "###############################################################"
            print "PLANNER", planner_name, "not implemented"
            print "###############################################################"
            sys.exit(0)

        #######################################################################
        planner.InitPlan(self.robot_, params)

        rave_traj = RaveCreateTrajectory(self.env_.env, '')

        t1 = time.time()
        result = planner.PlanPath(rave_traj)
        t2 = time.time()
        if result != PlannerStatus.HasSolution:
            print "Could not find geometrical path"
            print "Planner:", planner_name
            print "Status :", result
            return None
        print "Planner", planner_name, " success | time:", t2 - t1
        return rave_traj
示例#5
0
文件: retimer.py 项目: beiju/prpy
    def RetimeTrajectory(self, robot, path, options=None, **kw_args):
        from openravepy import CollisionOptions, CollisionOptionsStateSaver
        from copy import deepcopy

        # Validate the input path.
        cspec = path.GetConfigurationSpecification()
        joint_values_group = cspec.GetGroupFromName('joint_values')

        if joint_values_group is None:
            raise ValueError('Trajectory is missing the "joint_values" group.')
        elif HasAffineDOFs(cspec):
            raise UnsupportedPlanningError(
                'OpenRAVERetimer does not support affine DOFs.')
        elif joint_values_group.interpolation != 'linear':
            logger.warning(
                'Path has interpolation of type "%s"; only "linear"'
                ' interpolation is supported.',
                joint_values_group.interpolation)

        # Set parameters.
        all_options = deepcopy(self.default_options)
        if options is not None:
            all_options.update(options)

        params = Planner.PlannerParameters()
        params.SetConfigurationSpecification(
            self.env, cspec.GetTimeDerivativeSpecification(0))

        params_str = CreatePlannerParametersString(all_options, params)

        # Copy the input trajectory into the planning environment. This is
        # necessary for two reasons: (1) the input trajectory may be in another
        # environment and/or (2) the retimer modifies the trajectory in-place.
        output_traj = CopyTrajectory(path, env=self.env)

        # Remove co-linear waypoints. Some of the default OpenRAVE retimers do
        # not perform this check internally (e.g. ParabolicTrajectoryRetimer).
        if not IsTimedTrajectory(output_traj):
            output_traj = SimplifyTrajectory(output_traj, robot)

        # Only collision check the active DOFs.
        dof_indices, _ = cspec.ExtractUsedIndices(robot)
        robot.SetActiveDOFs(dof_indices)

        # Compute the timing. This happens in-place.
        self.planner.InitPlan(None, params_str)

        with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
                                        CollisionOptions.ActiveDOFs):
            status = self.planner.PlanPath(output_traj, releasegil=True)

        if status not in [
                PlannerStatus.HasSolution,
                PlannerStatus.InterruptedWithSolution
        ]:
            raise PlanningError('Retimer returned with status {:s}.'.format(
                str(status)))

        return output_traj
示例#6
0
    def test_rrt_planner(self):
        # Adopting examples from openrave
        domain, problem, params = load_environment(
            '../domains/baxter_domain/baxter.domain',
            '../domains/baxter_domain/baxter_probs/grasp_1234_1.prob')

        env = Environment()  # create openrave environment
        objLst = [i[1] for i in params.items() if not i[1].is_symbol()]
        view = OpenRAVEViewer.create_viewer(env)
        view.draw(objLst, 0, 0.7)
        can_body = view.name_to_rave_body["can0"]
        baxter_body = view.name_to_rave_body["baxter"]
        can = can_body.env_body
        robot = baxter_body.env_body
        dof = robot.GetActiveDOFValues()

        inds = baxter_body._geom.dof_map['rArmPose']
        r_init = params['robot_init_pose']
        r_end = params['robot_end_pose']

        dof[inds] = r_init.rArmPose.flatten()
        robot.SetActiveDOFValues(dof)

        robot.SetActiveDOFs(inds)  # set joints the first 4 dofs

        plan_params = Planner.PlannerParameters()
        plan_params.SetRobotActiveJoints(robot)
        plan_params.SetGoalConfig(
            [0.7, -0.204, 0.862, 1.217, 2.731, 0.665,
             2.598])  # set goal to all ones
        # forces parabolic planning with 40 iterations

        traj = RaveCreateTrajectory(env, '')
        # Using openrave built in planner
        trajectory = {}
        plan_params.SetExtraParameters(
            """  <_postprocessing planner="parabolicsmoother">
                                                <_nmaxiterations>17</_nmaxiterations>
                                            </_postprocessing>""")
        trajectory["BiRRT"] = planing(env, robot, plan_params, traj,
                                      'BiRRT')  # 3.5s
        # trajectory["BasicRRT"] = planing(env, robot, plan_params, traj, 'BasicRRT') # 0.05s can't run it by its own
        # trajectory["ExplorationRRT"] = planing(env, robot, plan_params, traj, 'ExplorationRRT') # 0.03s
        # plan_params.SetExtraParameters('<range>0.2</range>')
        # Using OMPL planner
        # trajectory["OMPL_RRTConnect"] = planing(env, robot, plan_params, traj, 'OMPL_RRTConnect') # 1.5s
        # trajectory["OMPL_RRT"] = planing(env, robot, plan_params, traj, 'OMPL_RRT') # 10s
        # trajectory["OMPL_RRTstar"] = planing(env, robot, plan_params, traj, 'OMPL_RRTstar') # 10s
        # trajectory["OMPL_TRRT"] = planing(env, robot, plan_params, traj, 'OMPL_TRRT')  # 10s
        # trajectory["OMPL_pRRT"] = planing(env, robot, plan_params, traj, 'OMPL_pRRT') # Having issue, freeze
        # trajectory["OMPL_LazyRRT"] = planing(env, robot, plan_params, traj, 'OMPL_LazyRRT') # 1.5s - 10s unsatble
        # ompl_traj = trajectory["OMPL_RRTConnect"]
        or_traj = trajectory["BiRRT"]
        result = process_traj(or_traj, 20)
        self.assertTrue(result.shape[1] == 20)
示例#7
0
    def RetimeTrajectory(self, robot, path):
        # Copy the input trajectory into the planning environment. This is
        # necessary for two reasons: (1) the input trajectory may be in another
        # environment and/or (2) the retimer modifies the trajectory in-place.
        output_traj = CopyTrajectory(path, env=self.env)

        # Remove co-linear waypoints.
        if not IsTimedTrajectory(output_traj):
            output_traj = SimplifyTrajectory(output_traj, robot)

        # Blend the piecewise-linear input trajectory. The blender outputs a
        # collision-free path, consisting of piecewise-linear segments and
        # quintic blends through waypoints.
        try:
            params = Planner.PlannerParameters()
            self.blender.InitPlan(robot, params)
            status = self.blender.PlanPath(output_traj)
            if status not in [
                    PlannerStatus.HasSolution,
                    PlannerStatus.InterruptedWithSolution
            ]:
                raise PlanningError('Blending trajectory failed.')
        except openrave_exception as e:
            raise PlanningError('Blending trajectory failed: ' + str(e))

        # Find the time-optimal trajectory that follows the blended path
        # subject to joint velocity and acceleration limits.
        try:
            params = Planner.PlannerParameters()
            self.retimer.InitPlan(robot, params)
            status = self.retimer.PlanPath(output_traj)
            if status not in [
                    PlannerStatus.HasSolution,
                    PlannerStatus.InterruptedWithSolution
            ]:
                raise PlanningError('Timing trajectory failed.')
        except openrave_exception as e:
            raise PlanningError('Timing trajectory failed: ' + str(e))

        SetTrajectoryTags(output_traj, {Tags.SMOOTH: True}, append=True)
        return output_traj
示例#8
0
    def test_RetimeTrajectory(self):
        import numpy
        from numpy.testing import assert_allclose
        from openravepy import planningutils, Planner

        # Setup/Test
        traj = self.planner.RetimeTrajectory(self.robot, self.feasible_path)

        # Assert
        position_cspec = self.feasible_path.GetConfigurationSpecification()
        velocity_cspec = position_cspec.ConvertToDerivativeSpecification(1)
        zero_dof_values = numpy.zeros(position_cspec.GetDOF())

        # Verify that the trajectory passes through the original waypoints.
        waypoints = [self.waypoint1, self.waypoint2, self.waypoint3]
        waypoint_indices = [None] * len(waypoints)

        for iwaypoint in xrange(traj.GetNumWaypoints()):
            joint_values = traj.GetWaypoint(iwaypoint, position_cspec)

            # Compare the waypoint against every input waypoint.
            for icandidate, candidate_waypoint in enumerate(waypoints):
                if numpy.allclose(joint_values, candidate_waypoint):
                    self.assertIsNone(
                        waypoint_indices[icandidate],
                        'Input waypoint {} appears twice in the output'
                        ' trajectory (indices: {} and {})'.format(
                            icandidate, waypoint_indices[icandidate],
                            iwaypoint))

                    waypoint_indices[icandidate] = iwaypoint

        self.assertEquals(waypoint_indices[0], 0)
        self.assertEquals(waypoint_indices[-1], traj.GetNumWaypoints() - 1)

        for iwaypoint in waypoint_indices:
            self.assertIsNotNone(iwaypoint)

            # Verify that the velocity at the waypoint is zero.
            joint_velocities = traj.GetWaypoint(iwaypoint, velocity_cspec)
            assert_allclose(joint_velocities, zero_dof_values)

        # Verify the trajectory between waypoints.
        for t in numpy.arange(self.dt, traj.GetDuration(), self.dt):
            iafter = traj.GetFirstWaypointIndexAfterTime(t)
            ibefore = iafter - 1

            joint_values = traj.Sample(t, position_cspec)
            joint_values_before = traj.GetWaypoint(ibefore, position_cspec)
            joint_values_after = traj.GetWaypoint(iafter, position_cspec)

            distance_full = numpy.linalg.norm(joint_values_after -
                                              joint_values_before)
            distance_before = numpy.linalg.norm(joint_values -
                                                joint_values_before)
            distance_after = numpy.linalg.norm(joint_values -
                                               joint_values_after)
            deviation = distance_before + distance_after - distance_full
            self.assertLess(deviation, self.tolerance * distance_full)

        # Check joint limits and dynamic feasibility.
        params = Planner.PlannerParameters()
        params.SetRobotActiveJoints(self.robot)

        planningutils.VerifyTrajectory(params, traj, self.dt)