q_goal[rank] = -0.5 rank = robot.rankInConfiguration['r_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['r_elbow_flex_joint'] q_goal[rank] = -0.5 r(q_goal) ps.loadObstacleFromUrdf("iai_maps", "kitchen_area", "") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.selectPathPlanner("PRM") #ps.solve () ps.prepareSolveStepByStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep() ps.executeOneStep()