class ShortcutPathTest(object): def setUp(self): from openravepy import RaveCreateTrajectory self.input_path = RaveCreateTrajectory(self.env, '') self.input_path.Init( self.robot.GetActiveConfigurationSpecification('linear')) self.input_path.Insert(0, self.waypoint1) self.input_path.Insert(1, self.waypoint2) self.input_path.Insert(2, self.waypoint3) def test_ShortcutPath_ShortcutExists_ReducesLength(self): from numpy.testing import assert_allclose # Setup/Test smoothed_path = self.planner.ShortcutPath(self.robot, self.input_path) # Assert self.assertEquals(smoothed_path.GetConfigurationSpecification(), self.input_path.GetConfigurationSpecification()) self.assertGreaterEqual(smoothed_path.GetNumWaypoints(), 2) n = smoothed_path.GetNumWaypoints() assert_allclose(smoothed_path.GetWaypoint(0), self.waypoint1) assert_allclose(smoothed_path.GetWaypoint(n - 1), self.waypoint3) self.assertLess(self.ComputeArcLength(smoothed_path), 0.5 * self.ComputeArcLength(self.input_path))
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 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
class RetimeTrajectoryTest(object): def setUp(self): from openravepy import planningutils, Planner, RaveCreateTrajectory cspec = self.robot.GetActiveConfigurationSpecification('linear') self.feasible_path = RaveCreateTrajectory(self.env, '') self.feasible_path.Init(cspec) self.feasible_path.Insert(0, self.waypoint1) self.feasible_path.Insert(1, self.waypoint2) self.feasible_path.Insert(2, self.waypoint3) self.dt = 0.01 self.tolerance = 0.1 # 10% error 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)