37 lines
756 B
Python
37 lines
756 B
Python
|
#! /usr/bin/python3
|
||
|
|
import time
|
||
|
|
import socket
|
||
|
|
import math
|
||
|
|
import rtde_control
|
||
|
|
|
||
|
|
import rtde_receive
|
||
|
|
rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.101")
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
actual_q = rtde_r.getActualQ()
|
||
|
|
print(actual_q)
|
||
|
|
|
||
|
|
print("faehrt")
|
||
|
|
|
||
|
|
|
||
|
|
ycenter=0.2
|
||
|
|
zcenter=0.5
|
||
|
|
c=0
|
||
|
|
|
||
|
|
scale=0.2
|
||
|
|
|
||
|
|
rtde_c = rtde_control.RTDEControlInterface("192.168.1.101")
|
||
|
|
#rtde_c.moveJ([-0.10, -0.5, 0.20, -0.001, 3.12, 0.04], 0.1, 0.1)
|
||
|
|
#rtde_c.moveJ([-1.54, -1.83, -2.28, -0.59, 1.60, 0.023], 0.1, 0.1)
|
||
|
|
rtde_c.moveJ([2.74, -1.45, -1.63, -1.63, 1.57, 4.31],0.1,0.1)
|
||
|
|
rtde_c.moveJ(rtde_c.getInverseKinematics([-0.5,ycenter+math.cos(c)*scale,zcenter+math.sin(c)*scale,0.0,3.14159,0.0]), 0.1,0.1)
|
||
|
|
|
||
|
|
print("gefahren")
|
||
|
|
|
||
|
|
rtde_c.servoStop()
|
||
|
|
rtde_c.stopScript()
|
||
|
|
|
||
|
|
actual_q = rtde_r.getActualQ()
|
||
|
|
print(actual_q)
|