def __init_traj(self, joint_target, algorithm):
"""
Creates an initial trajectory plan between start and end goal poses
"""
# Issue with the init and goal configuratons -> Planner thinks that the arms are in collision
algo = "OMPL_" + algorithm
planner = op.RaveCreatePlanner(self.env, algo) # Initializes a planner with algorithm
simplifier = op.RaveCreatePlanner(self.env, 'OMPL_Simplifier')
params = planner.PlannerParameters() # Creates an empty param to be filled
params.SetRobotActiveJoints(self.robot)
params.SetGoalConfig(joint_target)
extraParams = ('<_nmaxiterations>{:d}</_nmaxiterations>'.format(20000))
params.SetExtraParameters(extraParams)
with self.env:
with self.get_robot():
print "Starting intial plan using {:s} algorithm".format(algorithm)
traj = op.RaveCreateTrajectory(self.env, '')
planner.InitPlan(self.get_robot(), params)
result = planner.PlanPath(traj)
IPython.embed()
assert result == op.PlannerStatus.HasSolution
# print 'Calling the OMPL_Simplifier for shortcutting.'
# simplifier.InitPlan(self.get_robot(), op.Planner.PlannerParameters())
# result = simplifier.PlanPath(traj)
# assert result == op.PlannerStatus.HasSolution
trajectory = [traj.GetWaypoint(i).tolist() for i in range(traj.GetNumWaypoints())]
return trajectory
def __init_traj(self, manip, joint_target, algorithm):
"""
Creates an initial trajectory plan between start and end goal poses
"""
# Issue with the init and goal configuratons -> Planner thinks that the arms are in collision
Algo = "OMPL_" + algorithm
planner = op.RaveCreatePlanner(
self.env, Algo) # Initializes a planner with algorithm
simplifier = op.RaveCreatePlanner(self.env, 'OMPL_Simplifier')
# self.env.GetCollisionChecker().SetCollisionOptions(op.CollisionOptions.Contacts)
with self.env:
arm_indx = self.get_manip(name=manip).GetArmIndices()
self.get_robot().SetActiveDOFs(
arm_indx) # Plan for only the arm specified in manip
self.get_robot().SetActiveDOFValues(self.right_arm_DOF)
self.get_robot().SetActiveManipulator(self.get_manip(name=manip))
params = planner.PlannerParameters(
) # Creates an empty param to be filled
params.SetRobotActiveJoints(self.robot)
params.SetGoalConfig(joint_target[0])
extraParams = ('<_nmaxiterations>{:d}</_nmaxiterations>'.format(10000))
params.SetExtraParameters(extraParams)
with self.env:
with self.get_robot():
print "Starting intial plan using {:s} algorithm".format(
algorithm)
traj = op.RaveCreateTrajectory(self.env, '')
planner.InitPlan(self.get_robot(), params)
result = planner.PlanPath(traj)
assert result == op.PlannerStatus.HasSolution
print 'Calling the OMPL_Simplifier for shortcutting.'
simplifier.InitPlan(self.get_robot(),
op.Planner.PlannerParameters())
result = simplifier.PlanPath(traj)
assert result == op.PlannerStatus.HasSolution
trajectory = [
traj.GetWaypoint(i).tolist()
for i in range(traj.GetNumWaypoints())
]
return trajectory
def PlanToConfigurations(self, robot, goal_configs, **kw_args):
params = self.defaults._replace(**kw_args)
# create family
with type(self).AddedFamilyModule(self.env, robot) as family:
setcache_path = self.get_setcache_path(family, params.roadmap, read_only=True)
planner = openravepy.RaveCreatePlanner(self.env, 'FamilyPlanner')
if planner is None:
raise prpy.planning.base.UnsupportedPlanningError('Unable to create FamilyPlanner planner.')
# serialize parameters
orparams = openravepy.Planner.PlannerParameters()
orparams.SetRobotActiveJoints(robot)
goal_configs_stacked = []
for goal_config in goal_configs:
goal_configs_stacked.extend(goal_config)
orparams.SetGoalConfig(goal_configs_stacked)
xml = self.xml_from_params(params)
xml.append('<family_module>{}</family_module>'.format(family.SendCommand('GetInstanceId')))
if setcache_path != '':
xml.append('<family_setcaches><setcache><filename>{}</filename></setcache></family_setcaches>'.format(setcache_path))
orparams.SetExtraParameters('\n'.join(xml))
planner.InitPlan(robot, orparams)
# plan path
traj = openravepy.RaveCreateTrajectory(self.env, '')
status = planner.PlanPath(traj)
if status == openravepy.PlannerStatus.HasSolution:
return traj
else:
raise prpy.planning.base.PlanningError('LEMUR status: {}'.format(status))
def plan(self, qgoal, max_iters=40, max_ppiters=40):
traj = orpy.RaveCreateTrajectory(self.env, '')
params = orpy.Planner.PlannerParameters()
# Configuration specification
robot_name = self.robot.GetName()
torso_idx = self.torso_joint.GetDOFIndex()
spec = orpy.ConfigurationSpecification()
spec.AddGroup('joint_values {0} {1}'.format(robot_name, torso_idx), 1,
'')
left_spec = self.left_manip.GetArmConfigurationSpecification()
right_spec = self.right_manip.GetArmConfigurationSpecification()
spec += left_spec + right_spec
params.SetConfigurationSpecification(self.env, spec)
params.SetGoalConfig(qgoal)
params.SetMaxIterations(max_iters)
params.SetPostProcessing(
'ParabolicSmoother',
'<_nmaxiterations>{}</_nmaxiterations>'.format(max_ppiters))
planner = orpy.RaveCreatePlanner(self.env, 'BiRRT')
success = planner.InitPlan(None, params)
status = orpy.PlannerStatus.Failed
if success:
status = planner.PlanPath(traj)
if status != orpy.PlannerStatus.HasSolution:
traj = None
return traj
def retime_trajectory(robot, traj, method):
"""
Retime an OpenRAVE trajectory using the specified method.
Parameters
----------
robot: orpy.Robot
The OpenRAVE robot
traj: orpy.Trajectory
The traj to be retimed. The time paremetrization will be *overwritten*.
method: str
Retiming method. Available options are: `LinearTrajectoryRetimer`,
`ParabolicTrajectoryRetimer`, `CubicTrajectoryRetimer`
Returns
-------
status: orpy.PlannerStatus
Flag indicating the status of the trajectory retiming. It can be: `Failed`,
`HasSolution`, `Interrupted` or `InterruptedWithSolution`.
"""
env = robot.GetEnv()
# Populate planner parameters
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetMaxIterations(20)
params.SetPostProcessing('', '')
# Generate the trajectory
planner = orpy.RaveCreatePlanner(env, method)
status = orpy.PlannerStatus.Failed
success = planner.InitPlan(robot, params)
if success:
status = planner.PlanPath(traj)
return status
def __init__(self,env,youbots,parallelplanning=False):
self.env = env
self.planner = orpy.RaveCreatePlanner(self.env,'birrt')
self.youbots = youbots
self.parallelplanning = parallelplanning
#self.youbots = {} # TODO remove after debug
#self.youbots['drc1'] = youbots['drc1'] # TODO remove
#self.youbots['drc2'] = youbots['drc2'] # TODO remove
if self.parallelplanning:
self.awaypose = np.eye(4)
self.awaypose[:2,3] = np.array([100.0, 100.0])
self.youbots_base_joints = {}
for yname in self.youbots:
print yname
youbot_base_joints = env.ReadRobotURI('robots/kuka-youbot-baseasjoints.robot.xml')
youbot_base_joints.SetName(yname+'_baseasjoints')
self.env.Add(youbot_base_joints,True)
youbot_base_joints.SetTransform(self.awaypose)
youbot_base_joints.GetLink('base0').Enable(False)
youbot_base_joints.GetLink('base1').Enable(False)
youbot_base_joints.GetLink('base2').Enable(False)
self.youbots_base_joints[yname] = youbot_base_joints
else:
self.planners = {}
for y in youbots:
self.planners[y] = orpy.interfaces.BaseManipulation(youbots[y],plannername='BiRRT')
def visualize_motions(self):
planner = orpy.RaveCreatePlanner(self.env, 'birrt')
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(self.robot)
candidate_traj = []
for i in range(len(self.solutions)):
print "Visualizing the ", i, " solution"
self.get_current_joint_states()
time.sleep(1)
#raw_input("Press enter to bring robot home")
self.robot.SetActiveDOFValues(self.initConfig)
params.SetInitialConfig(self.initConfig)
params.SetGoalConfig(self.solutions[i-1])
params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>50</_nmaxiterations>')
planner.InitPlan(self.robot, params)
traj = orpy.RaveCreateTrajectory(self.env, '')
planner.PlanPath(traj)
ros_traj = ros_trajectory_from_openrave(self.robot.GetName(), traj)
print "duration of the path is: ", ros_traj.points[-1].time_from_start
candidate_traj.append(ros_traj)
controller = self.robot.GetController()
raw_input("Press enter to visualize the plan in openrave")
controller.SetPath(traj)
# sort all the feasible paths according to their durations
candidate_traj.sort(key=sort_time)
self.optimalPath = candidate_traj[0]
def run_planner(self, planner_name):
with self.env:
# Setup
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(self.robot)
params.SetGoalConfig(self.GOAL_CONFIG)
params.SetExtraParameters('<time_limit>30</time_limit>')
cspec = self.robot.GetActiveConfigurationSpecification()
traj = openravepy.RaveCreateTrajectory(self.env, '')
for i in xrange(self.NUM_ATTEMPTS):
# Act
planner = openravepy.RaveCreatePlanner(self.env,
'OMPL_' + planner_name)
planner.InitPlan(self.robot, params)
result = planner.PlanPath(traj)
if result == openravepy.PlannerStatus.HasSolution:
break
# Assert
self.assertEqual(result, openravepy.PlannerStatus.HasSolution)
self.assertGreaterEqual(traj.GetNumWaypoints(), 1)
numpy.testing.assert_array_almost_equal(traj.GetWaypoint(0, cspec),
self.START_CONFIG)
numpy.testing.assert_array_almost_equal(
traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec),
self.GOAL_CONFIG)
def __init__(self, robot_name, _env, end_effector_name='gripper'):
self.env = _env
# initialize robot
self.robot = self.env.GetRobot(robot_name)
self.manipulator = self.robot.SetActiveManipulator(end_effector_name)
self.robot.SetActiveDOFs(self.manipulator.GetArmIndices())
# initialize IK model
self.ikmodel = orpy.databases.inversekinematics.InverseKinematicsModel(
self.robot, iktype=orpy.IkParameterization.Type.Transform6D)
if not self.ikmodel.load():
print "[INFO] generating IK model"
self.ikmodel.autogenerate()
# create path planner
self.planner = orpy.RaveCreatePlanner(
self.env, 'birrt') # Using bidirectional RRT
self.params = orpy.Planner.PlannerParameters()
self.params.SetRobotActiveJoints(self.robot)
# # get robot controller
# self.controller = self.robot.GetController()
# create interface for manipulation task
self.taskmanip = orpy.interfaces.TaskManipulation(self.robot)
def __init__(self):
super(SBPLPlanner, self).__init__()
try:
self.planner = openravepy.RaveCreatePlanner(self.env, 'SBPL')
except openravepy.openrave_exception:
raise UnsupportedPlanningError('Unable to create SBPL module')
def calculate_all_paths(self, goal_config_list):
result = []
path_list = []
ros_traj_list = []
sum_plan_time = 0
for goal_config in goal_config_list:
planner = orpy.RaveCreatePlanner(self.env, 'birrt')
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(self.robot)
params.SetInitialConfig(self.init_config)
params.SetGoalConfig(goal_config)
#params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>40</_nmaxiterations>')
#params.SetPostProcessing('shortcut_linear', '<_nmaxiterations>40</_nmaxiterations>')
planner.InitPlan(self.robot, params)
traj = orpy.RaveCreateTrajectory(self.env, '')
curr_start_time = time.time()
planner.PlanPath(traj)
curr_end_time = time.time()
sum_plan_time = sum_plan_time + curr_end_time - curr_start_time
ros_traj = test_irb1200_model.ros_trajectory_from_openrave(
self.robot.GetName(), traj)
print "duration of the path is: ", ros_traj.points[
-1].time_from_start
path_list.append(traj)
ros_traj_list.append(ros_traj)
average_plan_time = sum_plan_time / len(goal_config_list)
print "average motion planning time is: ", average_plan_time
result.append(path_list)
result.append(ros_traj_list)
return result
def _Plan(self,
robot,
goals,
maxiter=500,
continue_planner=False,
or_args=None,
**kw_args):
env = robot.GetEnv()
planner = openravepy.RaveCreatePlanner(env, self.algorithm)
if planner is None:
raise UnsupportedPlanningError(
'Unable to create {:s} module.'.format(str(self)))
# Get rid of default postprocessing
extraParams = ('<_postprocessing planner="">'
'<_nmaxiterations>0</_nmaxiterations>'
'</_postprocessing>')
# Maximum planner iterations
extraParams += (
'<_nmaxiterations>{:d}</_nmaxiterations>'.format(maxiter))
if or_args is not None:
for key, value in or_args.iteritems():
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(k=str(key),
v=str(value))
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(goals)
params.SetExtraParameters(extraParams)
traj = openravepy.RaveCreateTrajectory(env, 'GenericTrajectory')
try:
env.Lock()
with robot.CreateRobotStateSaver(
openravepy.Robot.SaveParameters.LinkTransformation):
# Plan.
if (not continue_planner) or not self.setup:
planner.InitPlan(robot, params)
self.setup = True
status = planner.PlanPath(traj, releasegil=True)
from openravepy import PlannerStatus
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError(
'Planner returned with status {:s}.'.format(
str(status)))
except Exception as e:
raise PlanningError('Planning failed with error: {:s}'.format(e))
finally:
env.Unlock()
return traj
def wam7_test(self):
env = openravepy.Environment()
try:
robot = env.ReadRobotXMLFile('robots/barrettwam.robot.xml')
env.Add(robot)
robot.SetActiveDOFs(robot.GetManipulator('arm').GetArmIndices())
robot.SetActiveDOFValues(q_start)
planner = openravepy.RaveCreatePlanner(env, 'LEMUR')
self.assertIsNotNone(planner)
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(q_goal)
params.SetExtraParameters(
'<roadmap_type>Halton</roadmap_type>' +
'<roadmap_param>num=1000</roadmap_param>' +
'<roadmap_param>radius=2.0</roadmap_param>')
planner.InitPlan(robot, params)
traj = openravepy.RaveCreateTrajectory(env, '')
result = planner.PlanPath(traj)
self.assertEqual(result, openravepy.PlannerStatus.HasSolution)
self.assertEqual(6, traj.GetNumWaypoints())
for iwp in range(6):
wp = traj.GetWaypoint(iwp)
self.assertEqual(7, len(wp))
for a, b in zip(wp, traj_expected[iwp]):
self.assertAlmostEqual(a, b, delta=1.0e-5)
finally:
env.Destroy()
def __init__(self):
super(OMPLSimplifier, self).__init__()
planner_name = 'OMPL_Simplifier'
self.planner = openravepy.RaveCreatePlanner(self.env, planner_name)
if self.planner is None:
raise UnsupportedPlanningError(
'Unable to create OMPL_Simplifier planner.')
def __init__(self, algorithm='birrt'):
super(OpenRAVEPlanner, self).__init__()
self.setup = False
self.algorithm = algorithm
try:
self.planner = openravepy.RaveCreatePlanner(self.env, algorithm)
except openravepy.openrave_exception:
raise UnsupportedPlanningError(
'Unable to create {:s} module.'.format(str(self)))
def __init__(self, **kw_args):
super(LEMURPlanner, self).__init__()
self.planner = openravepy.RaveCreatePlanner(self.env, 'LEMUR')
if self.planner is None:
raise prpy.planning.base.UnsupportedPlanningError(
'Unable to create LEMUR planner.')
Params = type(self).Params
self.defaults = Params(*[None
for f in Params._fields])._replace(**kw_args)
def plan_to_joint_values(qgoal):
# Create the planner
planner = orpy.RaveCreatePlanner(env,'birrt') # Using bidirectional RRT
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(qgoal)
params.SetExtraParameters('<_postprocessing planner="ParabolicSmoother"><_nmaxiterations>40</_nmaxiterations></_postprocessing>')
planner.InitPlan(robot, params)
# Plan a trajectory
traj = orpy.RaveCreateTrajectory(env, '')
planner.PlanPath(traj)
return traj
def __init__(self, algorithm='RRTConnect'):
super(OMPLPlanner, self).__init__()
self.setup = False
self.algorithm = algorithm
planner_name = 'OMPL_{:s}'.format(algorithm)
self.planner = openravepy.RaveCreatePlanner(self.env, planner_name)
if self.planner is None:
raise UnsupportedPlanningError(
'Unable to create "{:s}" planner. Is or_ompl installed?'.
format(planner_name))
def initialize(self, robot):
# construct a new module/planner if necessary
if self.module is None:
self.module = openravepy.RaveCreateModule(self.env, 'Family')
if self.module is None:
raise prpy.planning.base.UnsupportedPlanningError(
'Unable to create Family module.')
if self.planner is None:
self.planner = openravepy.RaveCreatePlanner(
self.env, 'FamilyPlanner')
if self.planner is None:
raise prpy.planning.base.UnsupportedPlanningError(
'Unable to create FamilyPlanner planner.')
def path_planning(self):
planner = orpy.RaveCreatePlanner(self.env,
'birrt') # Using bidirectional RRT
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(self.robot)
params.SetGoalConfig(self.qgrasp)
print "goal is: ", self.qgrasp
params.SetPostProcessing('ParabolicSmoother',
'<_nmaxiterations>40</_nmaxiterations>')
planner.InitPlan(self.robot, params)
# Plan a trajectory
traj = orpy.RaveCreateTrajectory(self.env, '')
planner.PlanPath(traj)
# Execute the trajectory
controller = self.robot.GetController()
controller.SetPath(traj)
def motion_plan(self, target_config = []):
planner = orpy.RaveCreatePlanner(self.env, 'birrt')
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(self.robot)
self.get_current_joint_states()
params.SetInitialConfig(self.initConfig)
self.robot.SetActiveDOFValues(self.initConfig)
self.initConfig = []
if len(target_config)>0:
print "going home"
print target_config
params.SetGoalConfig(target_config)
else:
choose = raw_input("Enter 1 to execute the shortest path, and 2 to customize path selection")
if choose == "1":
return self.optimalPath
elif choose == "2":
sol_num = raw_input("Please enter the solution number that you want to execute")
sol_num = int(sol_num)-1
print "planning to goal"
print self.solutions[sol_num]
params.SetGoalConfig(self.solutions[sol_num])
else:
print "Invalid input, exiting ..."
return
params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>40</_nmaxiterations>')
planner.InitPlan(self.robot, params)
# Plan a trajectory
traj = orpy.RaveCreateTrajectory(self.env, '')
planner.PlanPath(traj)
controller = self.robot.GetController()
raw_input("Press enter to visualize the plan in openrave")
controller.SetPath(traj)
# print the sequence of joint angles
times = np.arange(0, traj.GetDuration(), 0.01)
qvect = np.zeros((len(times), self.robot.GetActiveDOF()))
spec = traj.GetConfigurationSpecification()
for i in range(len(times)):
trajdata = traj.Sample(times[i])
qvect[i,:] = spec.ExtractJointValues(trajdata, self.robot, self.manipulator.GetArmIndices(), 0)
print qvect
ros_traj = ros_trajectory_from_openrave(self.robot.GetName(), traj)
print "duration of the path is: ", ros_traj.points[-1].time_from_start
return ros_traj
def ShortcutPath(self, robot, path, timeout=1., **kwargs):
# The planner operates in-place, so we need to copy the input path. We
# also need to copy the trajectory into the planning environment.
planner_name = 'OMPL_Simplifier'
env = robot.GetEnv()
planner = openravepy.RaveCreatePlanner(env, planner_name)
if planner is None:
raise UnsupportedPlanningError(
'Unable to create {} planner.'.format(planner_name))
output_path = CopyTrajectory(path, env=env)
extraParams = '<time_limit>{:f}</time_limit>'.format(timeout)
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetExtraParameters(extraParams)
# TODO: It would be nice to call planningutils.SmoothTrajectory here,
# but we can't because it passes a NULL robot to InitPlan. This is an
# issue that needs to be fixed in or_ompl.
with robot.CreateRobotStateSaver(
Robot.SaveParameters.LinkTransformation):
planner.InitPlan(robot, params)
status = planner.PlanPath(output_path, releasegil=True)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError(
'Simplifier returned with status {0:s}.'.format(
str(status)))
# Tag the trajectory as non-deterministic since the OMPL path
# simplifier samples random candidate shortcuts. It does not, however,
# change the endpoint, so we leave DETERMINISTIC_ENDPOINT unchanged.
SetTrajectoryTags(output_path, {
Tags.SMOOTH: True,
Tags.DETERMINISTIC_TRAJECTORY: False,
},
append=True)
return output_path
def plan(self, method, args, kwargs):
# Can't handle deferred planning yet.
assert not kwargs['defer']
# According to prpy spec, the first positional argument is 'robot'.
robot = args[0]
# Call the wrapped planner to get the seed trajectory.
planner_method = getattr(self._planners[0], method)
traj = planner_method(*args, **kwargs)
# Simplify redundant waypoints in the trajectory.
from prpy.util import SimplifyTrajectory
traj = SimplifyTrajectory(traj, robot)
# Run path shortcutting on the RRT path.
import openravepy
params = openravepy.Planner.PlannerParameters()
params.SetExtraParameters('<time_limit>0.25</time_limit>')
simplifier = openravepy.RaveCreatePlanner(robot.GetEnv(),
'OMPL_Simplifier')
simplifier.InitPlan(robot, params)
with robot:
result = simplifier.PlanPath(traj)
assert result == openravepy.PlannerStatus.HasSolution
# Call Trajopt to optimize the seed trajectory.
# Try different distance penalties until out of collision.
penalties = numpy.logspace(numpy.log(0.05), numpy.log(0.5), num=4)
for distance_penalty in penalties:
try:
return self._trajopt.OptimizeTrajectory(robot, traj, **kwargs)
except PlanningError:
logger.warn(
"Failed to optimize trajectory "
"with distance penalty: {:f}".format(distance_penalty))
# If all of the optimizations ended in collision,
# just return the original.
logger.warn("Failed to optimize trajectory, "
"returning unoptimized solution.")
return traj
def planRrt(self, configStart, configEnd, mover):
'''TODO'''
print("Planning with RRT* ...")
planner = openravepy.RaveCreatePlanner(mover.env, 'OMPL_RRTstar')
# Setup the planning instance.
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(mover.robot)
params.SetInitialConfig(configStart)
params.SetGoalConfig(configEnd)
# Set the timeout and planner-specific parameters. You can view a list of
# supported parameters by calling: planner.SendCommand('GetParameters')
params.SetExtraParameters('<range>' + str(self.maxCSpaceJump) +
'</range>')
params.SetExtraParameters('<time_limit>' + str(self.timeout) +
'</time_limit>')
isGoalValid = planner.InitPlan(mover.robot, params)
if not isGoalValid:
print("planRrt: End configuration is invalid.")
return None
# invoke the planner
traj = openravepy.RaveCreateTrajectory(mover.env, '')
result = planner.PlanPath(traj)
if not result == openravepy.PlannerStatus.HasSolution:
print("planRrt: No solution found.")
return None
traj_waypoints = []
for i in xrange(traj.GetNumWaypoints()):
traj_waypoints.append(traj.GetWaypoint(i)[:7])
subsampled_traj = traj_waypoints
subsampled_traj = self.subsampleTraj(traj_waypoints)
return subsampled_traj
def plan_to_joint_configuration(robot,
qgoal,
planner='birrt',
max_planner_iterations=20,
max_postprocessing_iterations=40):
env = robot.GetEnv()
rave_planner = orpy.RaveCreatePlanner(env, planner)
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(qgoal)
params.SetMaxIterations(max_planner_iterations)
params.SetPostProcessing(
'ParabolicSmoother', '<_nmaxiterations>{0}</_nmaxiterations>'.format(
max_postprocessing_iterations))
success = rave_planner.InitPlan(robot, params)
if not success:
return None
# Plan a trajectory
traj = orpy.RaveCreateTrajectory(env, '')
status = rave_planner.PlanPath(traj)
if status != orpy.PlannerStatus.HasSolution:
return None
return traj
def test_Simplifier(self):
with self.env:
# Setup
simplifier = openravepy.RaveCreatePlanner(self.env,
'OMPL_Simplifier')
params = openravepy.Planner.PlannerParameters()
cspec = self.robot.GetActiveConfigurationSpecification()
traj = openravepy.RaveCreateTrajectory(self.env, '')
traj.deserialize(self.TRAJECTORY_XML)
# Act
simplifier.InitPlan(self.robot, params)
result = simplifier.PlanPath(traj)
# Assert
self.assertEqual(result, openravepy.PlannerStatus.HasSolution)
self.assertGreaterEqual(traj.GetNumWaypoints(), 1)
numpy.testing.assert_array_almost_equal(traj.GetWaypoint(0, cspec),
self.START_CONFIG)
numpy.testing.assert_array_almost_equal(
traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec),
self.GOAL_CONFIG)
def plan_transit_motion(robot,
q_start,
q_goal,
pregrasp_start=True,
pregrasp_goal=True):
"""
Plan a transit motion (non-grasping motion) for robot moving from q_start to q_goal.
@type robot: openravepy.Robot
@param robot: The robot to plan this motion for
@type q_start: numpy.ndarray
@param q_start: Start configuration of the robot
@type q_goal: numpy.ndarray
@param q_goal: Goal configuration of the robot
@type pregrasp_start: bool
@param pregrasp_start: Indicates if pregrasp motion at the beginning of the trajectory
should be planned
@type pregrasp_goal: bool
@param pregrasp_goal: Indicates if pregrasp motion at the end of the trajectory should be planned
@rtype: oepnravepy.Trajectory
@return: An OpenRAVE trajectory for the robot moving from q_start to q_goal
"""
# MoveHandStraight parameters
minsteps = 1
maxsteps = 10
steplength = 0.005
execute = False
env = robot.GetEnv()
manip = robot.GetActiveManipulator()
basemanip = orpy.interfaces.BaseManipulation(robot)
q_original = robot.GetActiveDOFValues()
traj_pregrasp_start = None
traj_pregrasp_goal = None
with robot:
# Plan pregrasp motion at the start configuration
if pregrasp_start:
robot.SetActiveDOFValues(q_start)
eematrix = manip.GetTransform()
direction = -eematrix[0:3, 2]
try:
traj_pregrasp_start = basemanip.MoveHandStraight(
direction,
minsteps=minsteps,
maxsteps=maxsteps,
steplength=steplength,
starteematrix=eematrix,
execute=execute,
outputtrajobj=True)
except:
_log.info(
"Caught an exception in MoveHandStraight (pregrasp_start)."
)
return None
if traj_pregrasp_start is None:
_log.info("MoveHandStraight failed (pregrasp_start).")
new_q_start = q_start
pregrasp_start = False
else:
new_q_start = traj_pregrasp_start.GetWaypoint(
traj_pregrasp_start.GetNumWaypoints() - 1,
traj_pregrasp_start.GetConfigurationSpecification().
GetGroupFromName("joint_values"))
robot.SetActiveDOFValues(new_q_start)
if robot.CheckSelfCollision():
new_q_start = q_start
pregrasp_start = False
else:
orpy.planningutils.RetimeActiveDOFTrajectory(
traj_pregrasp_start,
robot,
hastimestamps=False,
maxvelmult=0.9,
maxaccelmult=0.81,
plannername="parabolictrajectoryretimer")
else:
new_q_start = q_start
# Plan pregrasp motion at the goal configuration
if pregrasp_goal:
robot.SetActiveDOFValues(q_goal)
eematrix = manip.GetTransform()
direction = -eematrix[0:3, 2]
try:
traj_pregrasp_goal = basemanip.MoveHandStraight(
direction,
minsteps=minsteps,
maxsteps=maxsteps,
steplength=steplength,
starteematrix=eematrix,
execute=execute,
outputtrajobj=True)
except:
_log.info(
"Caught an exception in MoveHandStraight (pregrasp_goal).")
return None
if traj_pregrasp_goal is None:
_log.info("MoveHandStraight failed (pregrasp_goal).")
new_q_goal = q_goal
pregrasp_goal = False
else:
new_q_goal = traj_pregrasp_goal.GetWaypoint(
traj_pregrasp_goal.GetNumWaypoints() - 1,
traj_pregrasp_goal.GetConfigurationSpecification().
GetGroupFromName("joint_values"))
robot.SetActiveDOFValues(new_q_goal)
if robot.CheckSelfCollision():
new_q_goal = q_goal
pregrasp_goal = False
else:
traj_pregrasp_goal_rev = orpy.planningutils.ReverseTrajectory(
traj_pregrasp_goal)
orpy.planningutils.RetimeActiveDOFTrajectory(
traj_pregrasp_goal_rev,
robot,
hastimestamps=False,
maxvelmult=0.9,
maxaccelmult=0.81,
plannername="parabolictrajectoryretimer")
traj_pregrasp_goal = traj_pregrasp_goal_rev
else:
new_q_goal = q_goal
_log.info("new_q_start = {0}".format(new_q_start.__repr__()))
_log.info("new_q_goal = {0}".format(new_q_goal.__repr__()))
# Plan a motion connecting new_q_start and new_q_goal
try:
# Using MoveActiveJoints sometimes has problems with jittering. As a fix, we plan using this birrt + parabolicsmoother
planner = orpy.RaveCreatePlanner(env, 'birrt')
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(
robot
) # robot's active DOFs must be correctly set before calling this function
params.SetInitialConfig(new_q_start)
params.SetGoalConfig(new_q_goal)
params.SetMaxIterations(10000)
extraparams = '<_postprocessing planner="parabolicsmoother2"><_nmaxiterations>100</_nmaxiterations></_postprocessing>'
params.SetExtraParameters(extraparams)
planner.InitPlan(robot, params)
traj_connect = orpy.RaveCreateTrajectory(env, '')
res = planner.PlanPath(traj_connect)
if not (res == orpy.PlannerStatus.HasSolution):
_log.info("Planner failed.")
return None
except Exception as e:
_log.info(
"Caught an exception ({0}) in MoveActiveJoints.".format(e))
return None
if traj_connect is None:
_log.info("MoveActiveJoints failed.")
return None
trajs_list = [
traj for traj in
[traj_pregrasp_start, traj_connect, traj_pregrasp_goal]
if traj is not None
]
final_traj = combine_openrave_trajectories(trajs_list)
return final_traj
def plan_to_joint_configuration(robot, qgoal, pname='BiRRT', max_iters=20,
max_ppiters=40, try_swap=False):
"""
Plan a trajectory to the given `qgoal` configuration.
Parameters
----------
robot: orpy.Robot
The OpenRAVE robot
qgoal: array_like
The goal configuration
pname: str
Name of the planning algorithm. Available options are: `BasicRRT`, `BiRRT`
max_iters: float
Maximum iterations for the planning stage
max_ppiters: float
Maximum iterations for the post-processing stage. It will use a parabolic
smoother wich short-cuts the trajectory and then smooths it
try_swap: bool
If set, will compute the direct and reversed trajectory. The minimum
duration trajectory is used.
Returns
-------
traj: orpy.Trajectory
Planned trajectory. If plan fails, this function returns `None`.
"""
qstart = robot.GetActiveDOFValues()
env = robot.GetEnv()
planner = orpy.RaveCreatePlanner(env, pname)
params = orpy.Planner.PlannerParameters()
params.SetMaxIterations(max_iters)
if max_ppiters > 0:
params.SetPostProcessing('ParabolicSmoother',
'<_nmaxiterations>{0}</_nmaxiterations>'.format(max_ppiters))
else:
params.SetPostProcessing('', '')
# Plan trajectory
best_traj = None
min_duration = float('inf')
reversed_is_better = False
count = 0
for qa, qb in itertools.permutations([qstart, qgoal], 2):
count += 1
with robot:
robot.SetActiveDOFValues(qa)
params.SetGoalConfig(qb)
params.SetRobotActiveJoints(robot)
initsuccess = planner.InitPlan(robot, params)
if initsuccess:
traj = orpy.RaveCreateTrajectory(env, '')
# Plan the trajectory
status = planner.PlanPath(traj)
if status == orpy.PlannerStatus.HasSolution:
duration = traj.GetDuration()
if duration < min_duration:
min_duration = duration
best_traj = orpy.RaveCreateTrajectory(
env, traj.GetXMLId())
best_traj.Clone(traj, 0)
if count == 2:
reversed_is_better = True
if not try_swap:
break
# Check if we need to reverse the trajectory
if reversed_is_better:
best_traj = orpy.planningutils.ReverseTrajectory(best_traj)
return best_traj
def run(self, offset=0.01):
"""Run the demo.
For each object, the robot first move to a pose that is
directly on top of it. Afterward, it moves down. Next, it
grabs the object to be transported. Finally, it moves to
another configuration.
Note: planning is done using rrt, not constrained rrt. The
later is far too inefficient to be of any use. An even more
critical issue with constrained rrt is that its path is too
jerky, failing most smoothing attempts.
Args:
offset: (float, optional) Approach distance.
"""
fail = False
# Start planning/control loop:
# Every pick cycle follows the same procedure:
# 1. APPROACH: visit a configuration that is directly on top of the object to pick
# 2. REACH: move down to make contact w the object
# 3. APPROACH: visit the same configuration as 1.
# 4. TRANSPORT: visit the goal configuration.
for obj_dict in self._scenario['objects']:
t0 = self.get_time()
# Basic setup
manip_name = obj_dict["object_attach_to"]
manip = self.get_robot().SetActiveManipulator(manip_name)
Tstart = np.array(obj_dict['T_start']) # object's transform
Tgoal = np.array(obj_dict['T_goal'])
T_ee_start = np.dot(
Tstart, self.get_object(
obj_dict['name']).get_T_object_link())
self.verify_transform(manip, T_ee_start)
T_ee_top = np.array(T_ee_start)
try:
T_ee_top[:3, 3] -= obj_dict['offset'] * T_ee_top[:3, 2]
except BaseException:
T_ee_top[:3, 3] -= offset * T_ee_top[:3, 2]
q_nominal = np.r_[-0.3, 0.9, 0.9, 0, 0, 0]
t1 = self.get_time()
# 1. APPROACH
logger.info("Plan path to APPROACH")
traj0 = plan_to_manip_transform(
self._robot, T_ee_top, q_nominal, max_ppiters=200, max_iters=100)
t1a = self.get_time()
if self.mode == "PRACTICE":
self.check_continue()
fail = not self.execute_trajectory(traj0)
self.get_robot().WaitForController(0)
# 2. REACH: Move a "short" trajectory to reach the object
logger.info("Plan path to REACH")
t2 = self.get_time()
traj0b = plan_to_manip_transform(
self._robot,
T_ee_start,
q_nominal,
max_ppiters=200,
max_iters=100)
t2a = self.get_time()
if self.mode == "PRACTICE":
self.check_continue()
fail = not self.execute_trajectory(traj0b)
self.get_robot().WaitForController(0)
with self._env:
self._robot.Grab(self.get_env().GetKinBody(obj_dict['name']))
logger.info("Grabbing the object. Continue moving in 1 sec.")
# time.sleep(1)
# 3+4: APPROACH+TRANSPORT: Plan two trajectories, one
# trajectory to reach the REACH position, another
# trajectory to reach the GOAL position. Merge them, then
# execute.
t3 = self.get_time()
logger.info("Plan path to GOAL")
# 1st trajectory
traj0c = plan_to_manip_transform(
self._robot, T_ee_top, q_nominal, max_ppiters=1, max_iters=100)
traj0c_waypoints, traj0c_ss = self.extract_waypoints(traj0c)
T_ee_goal = np.dot(
Tgoal, self.get_object(
obj_dict['name']).get_T_object_link())
self.verify_transform(manip, T_ee_goal)
q_nominal = np.r_[0.3, 0.9, 0.9, 0, 0, 0]
# 2nd trajectory
self._robot.SetActiveDOFValues(traj0c_waypoints[-1])
traj1_transport = plan_to_manip_transform(
self._robot, T_ee_goal, q_nominal, max_ppiters=1, max_iters=100)
self._robot.SetActiveDOFValues(traj0c_waypoints[0])
traj1_transport_waypoints, traj1_transport_ss = self.extract_waypoints(
traj1_transport)
# concatenate the two trajectories
traj2_waypoints = np.vstack(
(traj0c_waypoints, traj1_transport_waypoints[1:]))
# retime
traj2_ss = np.hstack(
(traj0c_ss, traj0c_ss[-1] + traj1_transport_ss[1:]))
traj2_ss[:] = traj2_ss / traj2_ss[-1]
traj2_rave = orpy.RaveCreateTrajectory(self._env, "")
spec = self._robot.GetActiveConfigurationSpecification()
traj2_rave.Init(spec)
for p in traj2_waypoints:
traj2_rave.Insert(traj2_rave.GetNumWaypoints(), p)
t3a = self.get_time()
planner = orpy.RaveCreatePlanner(self._env, "ParabolicSmoother")
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(self._robot)
params.SetMaxIterations(100)
params.SetPostProcessing('', '')
success = planner.InitPlan(self._robot, params)
status = planner.PlanPath(traj2_rave)
if not success or not status:
logger.fatal("[Plan Transport Path] Init status: {1:}, Plan status: {0:}. "
"Use traj2_rave directly.".format(status, success))
t3b = self.get_time()
# Retime the transport trajectory and execute it
logger.debug(
"Original traj nb waypoints: {:d}".format(
traj1_transport.GetNumWaypoints()))
logger.debug("Retime using toppra.")
t4 = self.get_time()
contact = self.get_object(obj_dict['name']).get_contact()
contact_constraint = create_object_transporation_constraint(
contact, self.get_object(obj_dict['name']))
contact_constraint.set_discretization_type(1)
traj2_retimed = toppra.retime_active_joints_kinematics(
traj2_rave,
self._robot,
additional_constraints=[contact_constraint],
solver_wrapper=SOLVER,
vmult=0.999,
amult=0.999)
if traj2_retimed is None:
logger.error(
"Transport trajectory retime fails! Try again without contact constraints.")
traj2_retimed = toppra.retime_active_joints_kinematics(
traj2_rave, self.get_robot(), additional_constraints=[])
t4a = self.get_time()
if self.mode == "PRACTICE":
self.check_continue()
fail = not self.execute_trajectory(traj2_retimed)
self.get_robot().WaitForController(0)
# release the object
logger.info("RELEASE object")
self.check_continue() # release object now
self._robot.Release(self.get_env().GetKinBody(obj_dict['name']))
t4b = self.get_time()
logger.info("Time report"
"\n - setup :{0:f} secs"
"\n - APPROACH plan :{1:f} secs"
"\n - APPROACH duration :{6:f} secs"
"\n - REACH plan :{2:f} secs"
"\n - REACH duration :{7:f} secs"
"\n - MOVE plan :{3:f} secs"
"\n - MOVE shortcut :{4:f} secs"
"\n - MOVE retime :{5:f} secs"
"\n - MOVE duration :{8:f} secs"
"\n - TOTAL duration :{9:f} secs".format(
t1 - t0, t1a - t1, t2a - t2, t3a - t3, t3b - t3a, t4a - t4,
traj0.GetDuration(), traj0b.GetDuration(),
traj2_retimed.GetDuration(), t4b - t0))
# remove objects from environment
T_cur = self.get_env().GetKinBody(obj_dict['name']).GetTransform()
T_cur[2, 3] -= 3e-3 # simulate 5mm drop
self.get_env().GetKinBody(obj_dict['name']).SetTransform(T_cur)
# Move the robot back to HOME
trajHOME = plan_to_joint_configuration(self._robot, self._qHOME)
fail = not self.execute_trajectory(trajHOME)
return not fail
raw_input('enter to quit')
exit()
# save with box
family.SendCommand('Let AllBox = $live')
print('PrintCurrentFamily ...')
s = family.SendCommand('PrintCurrentFamily')
print(s)
self_header = family.SendCommand('GetHeaderFromSet Self')
print('self_header:')
print(self_header)
# create a planner
planner = openravepy.RaveCreatePlanner(env, 'FamilyPlanner')
# turn off the box, and plan
box.Enable(False)
# plan q_start -> q_goal (with no box!)
robot.SetActiveDOFValues(q_start)
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetExtraParameters('''
<roadmap_type>RGGDens</roadmap_type>
<roadmap>
<num_per_batch>10000</num_per_batch>
<radius_first_batch>2.0</radius_first_batch>
<seed>0</seed>
</roadmap>
