r(ps.getWaypoints (0)[0]) # ref
r(ps.getWaypoints (1)[0]) # should be good...
index = cl.robot.getConfigSize () - 4
q = q1[::]
plotStraightLine ([q[index], q[index+1], q[index+2]], q, r, "normale")
plotCone (q1, cl, r, 0.5, 0.2, "cone1")
plotCone (q2, cl, r, 0.5, 0.2, "cone2")
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 ()
r.client.gui.addToGroup (coneName, r.sceneName)
r.client.gui.applyConfiguration (coneName, qRobot[0:7])
r.client.gui.refresh ()
#r.client.gui.removeFromGroup (coneName, r.sceneName)
# cone orientation seems not to be good....
r.client.gui.applyConfiguration (coneName, [6, -1.6, 0.5, 0, 1, 0, 0])
r.client.gui.refresh ()
## 3D Plot tools ##
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 = qa[::]
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):
r.client.gui.addToGroup(coneName, r.sceneName)
r.client.gui.applyConfiguration(coneName, qRobot[0:7])
r.client.gui.refresh()
#r.client.gui.removeFromGroup (coneName, r.sceneName)
# cone orientation seems not to be good....
r.client.gui.applyConfiguration(coneName, [6, -1.6, 0.5, 0, 1, 0, 0])
r.client.gui.refresh()
## 3D Plot tools ##
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 = qa[::]
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):
