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)