Python parseIntersectionConePlane示例

示例#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]

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

示例#2

文件： ant_orientation_test.py 项目： pFernbach/animals_description

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

示例#3

文件： scene_jump_harder.py 项目： mylene-campana/monopod_description

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

示例#4

文件： scene_jump_harder.py 项目： pFernbach/animals_description

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