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