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
plotStraightLine ([xPlus_vector[i], xPlus_vector[i]*tanTheta, zPlus_vector[i]], q2, r, "inter33")
plotStraightLine ([xMinus_vector[i], xMinus_vector[i]*tanTheta, zMinus_vector[i]], q2, r, "inter44")
r.client.gui.removeFromGroup ("inter2"+"straight", r.sceneName)
## 2D Plot tools ##
import matplotlib.pyplot as plt
theta = math.atan2((q2 [1] - q1 [1]) , (q2 [0] - q1 [0]))
ps.setInitialConfig (qt1); ps.addGoalConfig (qt2); ps.solve () texcoord="UVMap" <instance_material symbol="stone14-material" target="#stone14-material"> <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/> </instance_material> tanTheta = (q2 [1] - q1 [1]) / (q2 [0] - q1 [0]) num_log = 7795 line = 517 # "q: ..." line configs, xPlus_vector, xMinus_vector, zPlus_vector, zMinus_vector = parseIntersectionConePlane (num_log, str(line)+': q: ', str(line+1)+': x_plus: ', str(line+2)+': x_minus: ', str(line+3)+': z_x_plus: ', str(line+4)+': z_x_minus: ') i = 0 plotStraightLine ([xPlus_vector[i], xPlus_vector[i]*tanTheta, zPlus_vector[i]], qt1, r, "inter1") plotStraightLine ([xMinus_vector[i], xMinus_vector[i]*tanTheta, zMinus_vector[i]], qt1, r, "inter2") i = 1 plotStraightLine ([xPlus_vector[i], xPlus_vector[i]*tanTheta, zPlus_vector[i]], qt2, r, "inter3") plotStraightLine ([xMinus_vector[i], xMinus_vector[i]*tanTheta, zMinus_vector[i]], qt2, r, "inter4") ## 2D Plot tools ## import matplotlib.pyplot as plt theta = math.atan2((qt2 [1] - qt1 [1]) , (qt2 [0] - qt1 [0])) index = cl.robot.getConfigSize () - 3 NconeOne = [qt1 [index]*math.cos(theta) + qt1 [index+1]*math.sin(theta), qt1 [index+2]]
## 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
configs, theta_vector, xPlus_vector, xMinus_vector, zPlus_vector, zMinus_vector = parseIntersectionConePlane (num_log,'220: theta: ', '544: q: ', '545: x_plus: ', '546: x_minus: ', '547: z_x_plus: ', '548: z_x_minus: ', 11)
i = 0
plotStraightLine ([xPlus_vector[i], xPlus_vector[i]*math.tan(theta_vector[i]), zPlus_vector[i]], q11, r, "inter1")
plotStraightLine ([xMinus_vector[i], xMinus_vector[i]*math.tan(theta_vector[i]), zMinus_vector[i]], q11, r, "inter2")
i = 1
plotStraightLine ([xPlus_vector[i], xPlus_vector[i]*math.tan(theta_vector[i]), zPlus_vector[i]], q22, r, "inter33")
plotStraightLine ([xMinus_vector[i], xMinus_vector[i]*math.tan(theta_vector[i]), zMinus_vector[i]], q22, r, "inter44")
#r.client.gui.removeFromGroup ("inter2"+"straight", r.sceneName)
## Plot all cone-planeTheta intersections:
num_log = 7308
configs, theta_vector, xPlus_vector, xMinus_vector, zPlus_vector, zMinus_vector = parseIntersectionConePlane (num_log,'222: theta: ', '549: q: ', '550: x_plus: ', '551: x_minus: ', '552: z_x_plus: ', '553: z_x_minus: ', 11)
len(xPlus_vector)
for i in range(0,len(xPlus_vector)/4):
## 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
configs, theta_vector, xPlus_vector, xMinus_vector, zPlus_vector, zMinus_vector = parseIntersectionConePlane (num_log,'220: theta: ', '544: q: ', '545: x_plus: ', '546: x_minus: ', '547: z_x_plus: ', '548: z_x_minus: ', 11)
i = 0
plotStraightLine ([xPlus_vector[i], xPlus_vector[i]*math.tan(theta_vector[i]), zPlus_vector[i]], q11, r, "inter1")
plotStraightLine ([xMinus_vector[i], xMinus_vector[i]*math.tan(theta_vector[i]), zMinus_vector[i]], q11, r, "inter2")
i = 1
plotStraightLine ([xPlus_vector[i], xPlus_vector[i]*math.tan(theta_vector[i]), zPlus_vector[i]], q22, r, "inter33")
plotStraightLine ([xMinus_vector[i], xMinus_vector[i]*math.tan(theta_vector[i]), zMinus_vector[i]], q22, r, "inter44")
#r.client.gui.removeFromGroup ("inter2"+"straight", r.sceneName)
## Plot all cone-planeTheta intersections:
num_log = 7308
configs, theta_vector, xPlus_vector, xMinus_vector, zPlus_vector, zMinus_vector = parseIntersectionConePlane (num_log,'222: theta: ', '549: q: ', '550: x_plus: ', '551: x_minus: ', '552: z_x_plus: ', '553: z_x_minus: ', 11)
len(xPlus_vector)
for i in range(0,len(xPlus_vector)/4):
