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