philipp.kramer/urcontrol
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

add spiral movement

Browse source
This commit is contained in:
Philipp Kramer 2022-05-19 17:29:54 +02:00
parent a4c0a0d30e
commit 486b470f6c
1 changed files with 20 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

25
tests/servoj_circle.py
View file

@ -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)
joint_q=rtde_c.getInverseKinematics([-0.5,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0])
cx=i/(frequency*duration) * 2 *3.1415 * 1
cx%= (2*3.1415)
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)