# 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)
plotPointsAndLines (r, lineNamePrefix, sphereNamePrefix, x1_J1Viewer, x2_J1Viewer, sphereSize) lineNamePrefix = "lineJ2_"; sphereNamePrefix = "sphereJ2_" plotPointsAndLines (r, lineNamePrefix, sphereNamePrefix, x1_J2Viewer, x2_J2Viewer, sphereSize) # Plot trajectories from viewer_display_library_OPTIM import plotPointBodyCurvPath dt = 0.06 jointName = 'wrist_3_joint' plotPointBodyCurvPath (r, cl, robot, dt, 0, jointName, [0.28,0,0], 'pathPoint_'+jointName, [0.9,0.1,0.1,1]) plotPointBodyCurvPath (r, cl, robot, dt, 1, jointName, [0.28,0,0], 'pathPointRS_'+jointName, [0.1,0.1,0.9,1]) plotPointBodyCurvPath (r, cl, robot, dt, ps.numberPaths ()-1, jointName, [0.28,0,0], 'pathPointGB_'+jointName, [0.2,0.9,0.2,1]) # test function in cl.robot jointPosition = robot.getJointPosition ('wrist_3_joint') pointInJoint = [0.28,0,0] posAtester = cl.robot.computeGlobalPosition (jointPosition, pointInJoint) r(q1) robot.setCurrentConfig (q2) 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 ()