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