r( ps.configAtParam(0,2) ) ps.pathLength(0) ps.getWaypoints (0) ps.getWaypoints (1) plotPath (cl, 0, r, "pathy", 0.1) # time-step should depend on sub-path length ? plotFrame (r, "framy", [0,0,0], 0.5) plotThetaPlane (q1, q2, r, "ThetaPlane") ## DEBUG commands cl.obstacle.getObstaclePosition('decor_base') robot.getJointOuterObjects('base_joint_xyz') robot.isConfigValid(q1) robot.distancesToCollision() r( ps.configAtDistance(0,5) ) ps.optimizePath (0) ps.clearRoadmap () ps.resetGoalConfigs () from numpy import * argmin(robot.distancesToCollision()[0]) robot.getJointNames () robot.getConfigSize () ps.setInitialConfig (qt1); ps.addGoalConfig (qt2); ps.solve ()