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