diff --git a/tests/servoj_circle.py b/tests/servoj_circle.py index 80d6b62..c8227ac 100644 --- a/tests/servoj_circle.py +++ b/tests/servoj_circle.py @@ -14,25 +14,38 @@ gain = 300 joint_q = [-1.54, -1.83, -2.28, -0.59, 1.60, 0.023] # Move to initial joint position with a regular moveJ -rtde_c.moveJ([2.74, -1.45, -1.63, -1.63, 1.57, 4.31],0.1,0.1) -duration=10 -ycenter=0.2 +duration=20 + +xcenter=-0.5 +ycenter=0.1 zcenter=0.5 c=0 -scale=0.2 +cx=0 + +scale=0.1 + +rtde_c.moveJ([3.091137462206234, -1.2753459082058711, -1.7658837933568385, -1.6701929639036406, 1.5738425257277626, 4.658345567448585],0.1,0.1) + +joint_q=rtde_c.getInverseKinematics([xcenter+math.sin(cx)*0.2,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]) + +print("Start q") +print(joint_q) print("Starting Servo") # Execute 500Hz control loop for 2 seconds, each cycle is 2ms for i in range(frequency*duration): start = time.time() - c=i/(frequency*duration) * 2 *3.1415 * 4 + c=i/(frequency*duration) * 2 *3.1415 * 8 c%= (2*3.1415) + + cx=i/(frequency*duration) * 2 *3.1415 * 1 + cx%= (2*3.1415) - joint_q=rtde_c.getInverseKinematics([-0.5,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]) + joint_q=rtde_c.getInverseKinematics([xcenter+math.sin(cx)*0.2,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]) #print(joint_q) rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain) @@ -45,4 +58,4 @@ for i in range(frequency*duration): print("Finished") rtde_c.servoStop() -rtde_c.stopScript() \ No newline at end of file +rtde_c.stopScript()