plotFrame (r, "_", [0,0,4], 0.5) plotPath (cl, 0, r, "pathy", 0.1) plotThetaPlane (q1, q2, r, "ThetaPlane2") plotCone (q1, cl, r, 0.5, 0.4, "c1") plotCone (q2, cl, r, 0.5, 0.4, "c2") index = cl.robot.getConfigSize () - cl.robot.getExtraConfigSize () q = qa[::] q = [-4.77862,-1.56995,2.87339,-0.416537,-0.469186,-0.619709,0.471511,-0.197677,-0.0998335,0.97517,0.619095] qprojCorba=[-4.778619492059025, -1.5699444231861588, 2.873387956706481, 0.9470998051218645, 0.017748399125124163, -0.10999926666084152, 0.3009769340010935, -0.19767685053184691, -0.0998334947491579, 0.9751703113251448, 0.619095] plotStraightLine ([q [index],q [index+1],q [index+2]], q, r, "normale2") plotConeWaypoints (cl, 0, r, 0.5, 0.4, "wpcones") wp = cl.problem.getWaypoints (0) for i in np.arange(0, len(wp), 1): plotCone (wp[i], cl, r, 0.5, 0.4, "wpcones"+str(i)) tanTheta = (q2 [1] - q1 [1]) / (q2 [0] - q1 [0]) num_log = 22393 configs, xPlus_vector, xMinus_vector, zPlus_vector, zMinus_vector = parseIntersectionConePlane (num_log, '479: q: ', '480: x_plus: ', '481: x_minus: ', '482: z_x_plus: ', '483: z_x_minus: ') i = 0 plotStraightLine ([xPlus_vector[i], xPlus_vector[i]*tanTheta, zPlus_vector[i]], q1, r, "inter1") plotStraightLine ([xMinus_vector[i], xMinus_vector[i]*tanTheta, zMinus_vector[i]], q1, r, "inter2") i = 1
q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0]; r(q0) plotFrame (r, "frame", [0,0,0], 0.5) plotPath (cl, 0, r, "pathy", 0.1) plotThetaPlane (q1, q2, r, "ThetaPlane") r.client.gui.removeFromGroup ("ThetaPlane", r.sceneName) r.client.gui.removeFromGroup ("ThetaPlanebis", r.sceneName) #plotCone (q1, cl, r, 0.5, 0.4, "c1"); plotCone (q2, cl, r, 0.5, 0.4, "c2") index = cl.robot.getConfigSize () - cl.robot.getExtraConfigSize () q = q2[::] plotStraightLine ([q [index],q [index+1],q [index+2]], q, r, "normale2") ## Plot all cone waypoints: #plotConeWaypoints (cl, 0, r, 0.5, 0.4, "wpcones") wp = cl.problem.getWaypoints (0) for i in np.arange(0, len(wp), 1): qCone = cl.robot.setOrientation (wp[i]) coneName = "wp_cone_"+str(i) r.loadObstacleModel ("animals_description","friction_cone2",coneName) r.client.gui.applyConfiguration (coneName, qCone[0:7]) r.client.gui.refresh () ## Plot ONE cone-planeTheta intersection: num_log = 3533
## 3D Plot tools ## q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0]; r(q0) plotFrame (r, "frame", [0,0,4], 0.5) plotThetaPlane (q1, q2, r, "ThetaPlane") plotCone (q2, cl, r, "yep", "friction_cone2") plotConeWaypoints (cl, 0, r, "wp", "friction_cone") plotConeWaypoints (cl, 1, r, "wp2", "friction_cone") index = cl.robot.getConfigSize () - cl.robot.getExtraConfigSize () q = q2[::] plotStraightLine ([q[index], q[index+1], q[index+2]], q, r, "normale") # --------------------------------------------------------------------# ## Video capture ## import time pp.dt = 0.01; pp.speed=0.5 r(q1) r.startCapture ("capture","png") r(q1); time.sleep(0.2); r(q1) pp(0) #pp(ps.numberPaths()-1) r(q2); time.sleep(1); r.stopCapture () ## ffmpeg commands ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4
r(q0) plotFrame (r, "frame", [0,0,0], 0.5) plotCone (q, cl, r, "yep", "friction_cone2") plotConeWaypoints (cl, 0, r, "wp", "friction_cone2") plotThetaPlane (q1, q2, r, "ThetaPlane") r.client.gui.removeFromGroup ("ThetaPlane", r.sceneName) r.client.gui.removeFromGroup ("ThetaPlanebis", r.sceneName) #plotCone (q1, cl, r, 0.5, 0.4, "c1"); plotCone (q2, cl, r, 0.5, 0.4, "c2") index = cl.robot.getConfigSize () - cl.robot.getExtraConfigSize () q = q2[::] plotStraightLine ([q [index],q [index+1],q [index+2]], q, r, "normale2") ## Plot all cone waypoints: #plotConeWaypoints (cl, 0, r, 0.5, 0.4, "wpcones") wp = cl.problem.getWaypoints (0) for i in np.arange(0, len(wp), 1): qCone = cl.robot.setOrientation (wp[i]) coneName = "wp_cone_"+str(i) r.loadObstacleModel ("animals_description","friction_cone2",coneName) r.client.gui.applyConfiguration (coneName, qCone[0:7]) r.client.gui.refresh () ## Plot ONE cone-planeTheta intersection: num_log = 3533
r(q0) plotFrame (r, "_", [0,0,4], 0.5) plotPath (cl, 0, r, "pathy", 0.1) plotThetaPlane (q1, q2, r, "ThetaPlane2") plotSphere (q2, cl, r, "sphere1", [0,1,0,1], 0.1) index = cl.robot.getConfigSize () - cl.robot.getExtraConfigSize () q = qa[::] q = [-4.77862,-1.56995,2.87339,-0.416537,-0.469186,-0.619709,0.471511,-0.197677,-0.0998335,0.97517,0.619095] qq =[2.65346,-0.823813,7.48169,-0.236625,0.381534,0.845472,0.289165,-0.329554,0.536459,-0.444451,0.339103] plotStraightLine ([-0.329554, 0.536459, -0.444451], qq, r, "normale") theta = -0.638354; tanTheta = math.tan(theta) xPlus = -0.5; xMinus = -0.5; zPlus = -0.0756764; zMinus = -1.08921 plotStraightLine ([xPlus, xPlus*tanTheta, zPlus], qq, r, "inter12") plotStraightLine ([xMinus, xMinus*tanTheta, zMinus], qq, r, "inter22") qprojCorba=[-4.778619492059025, -1.5699444231861588, 2.873387956706481, 0.9470998051218645, 0.017748399125124163, -0.10999926666084152, 0.3009769340010935, -0.19767685053184691, -0.0998334947491579, 0.9751703113251448, 0.619095] plotStraightLine ([q [index],q [index+1],q [index+2]], q, r, "normale2") plotConeWaypoints (cl, 0, r, 0.5, 0.4, "wpcones") wp = cl.problem.getWaypoints (0) for i in np.arange(0, len(wp), 1): plotCone (wp[i], cl, r, 0.5, 0.4, "wpcones"+str(i))