# Plot trajectories
from viewer_display_library_OPTIM import plotPointBodyCurvPath
dt = 0.02
jointName = 'wrist_3_joint'
plotPointBodyCurvPath (r, cl, robot, dt, 0, jointName, [0.28,0,0], 'pathPoint_'+jointName, [1,0,0,1])
plotPointBodyCurvPath (r, cl, robot, dt, 16, jointName, [0.28,0,0], 'pathPointRS_'+jointName, [0,0,1,1])
plotPointBodyCurvPath (r, cl, robot, dt, 15, jointName, [0.28,0,0], 'pathPointGB_'+jointName, [0,1,0,1])
r.client.gui.removeFromGroup ('pathPoint_'+jointName, r.sceneName)
r.client.gui.removeFromGroup ('pathPointRS_'+jointName, r.sceneName)
r.client.gui.removeFromGroup ('pathPointGB_'+jointName, r.sceneName)
# test function in cl.robot
jointPosition = robot.getJointPosition ('wrist_3_joint')
pointInJoint = [0.3,0,0]
posAtester = cl.robot.computeGlobalPosition (jointPosition, pointInJoint)
r(q1)
robot.setCurrentConfig (q1)
sphereName = "machin"
r.client.gui.addSphere (sphereName,0.03,[0.1,0.1,0.1,1]) # black
configSphere = posAtester [::]
configSphere.extend ([1,0,0,0])
r.client.gui.applyConfiguration (sphereName,configSphere)
r.client.gui.addToGroup (sphereName, r.sceneName)
r.client.gui.refresh ()
r.client.gui.removeFromGroup ('pathPointRS_'+str(1)+jointName, r.sceneName)
