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)