philipp.kramer
/
urcontrol
1
0
Fork 0
Code Issues 4 Pull Requests Releases Wiki Activity

add inverse kinematics with joint constraints

This commit is contained in:
Philipp Kramer 2022-06-09 15:15:56 +02:00
parent 1e0c7349e2
commit 716b6699f3
2 changed files with 241 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
tests/pantilt.py
View File

@ -1,4 +1,3 @@
import rtde_control
import time
import math
@ -29,7 +28,8 @@ def toRad(d):
actual_q = rtde_r.getActualQ() #current joint rotations in radians.
print("Actual Q:"+str(actual_q))
fw_actual_q = rtde_c.getForwardKinematics(actual_q)
#fw_actual_q = rtde_c.getForwardKinematics(actual_q)
fw_actual_q = rtde_c.getForwardKinematics()
print("forward Actual Q:"+str(fw_actual_q))
joint_q=rtde_c.getInverseKinematics(fw_actual_q)
print("Final Q:"+str(joint_q))
@ -41,6 +41,7 @@ tilt=0 #-=down, +=up, degrees
joint_q=[0,toRad(-90),toRad(90),toRad(-180),toRad(-90),toRad(0)] #start position
#rtde_c.moveJ([0,toRad(-90),toRad(90-tilt/2),toRad(-180-tilt/2),toRad(-90-pan),toRad(0)],0.1,0.1)
rtde_c.moveJ(joint_q,0.1,0.1) #move to initial position
@ -62,8 +63,11 @@ speed=1/frequency
def constrain(v,_min,_max):
return min(_max,max(_min,v))
print("Press s to start!")
print("Press s to start")
print("Press q to stop")
while not keyboard.is_pressed("s"):
if keyboard.is_pressed("q"):
exit()
time.sleep(0.1)
print("Starting Servo")
@ -107,15 +111,21 @@ while not keyboard.is_pressed("q"):
# Robot Control stuff
joint_q_aim = [0,toRad(-90),toRad(90),toRad(-180-tilt),toRad(-90-pan),toRad(0)]
#joint_q_aim = rtde_c.getInverseKinematics([-0.895,-0.173,0.911,2.4,2.4,2.4])
# inverse kinematics: rx+ =look up
# ry+ = look right
# rz+ = rotate CW
reachable=rtde_c.isPoseWithinSafetyLimits(rtde_c.getForwardKinematics(joint_q_aim))
if not reachable:
print("Reachable:"+str(reachable))
for i in range(6):
joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -speed,speed)
if reachable:
for i in range(6):
joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -speed,speed)
#print(joint_q)

226
tests/pantilt_inv.py Normal file
View File

@ -0,0 +1,226 @@
import rtde_control
import time
import math
import socket
import keyboard # pip install keyboard
import numpy as np
# Socket stuff
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
host =""
port =30002
s.bind((host,port))
s.listen(1) # Number of connections
s.setblocking(False)
client = None
# Robot Control Stuff
rtde_c = rtde_control.RTDEControlInterface("192.168.1.101")
import rtde_receive
rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.101")
def toRad(d):
return d/360*2*math.pi
def toDeg(r):
return r*360/2/math.pi
def RPYtoVec(roll,pitch,yaw):
yawMatrix = np.matrix([
[math.cos(yaw), -math.sin(yaw), 0],
[math.sin(yaw), math.cos(yaw), 0],
[0, 0, 1]
])
pitchMatrix = np.matrix([
[math.cos(pitch), 0, math.sin(pitch)],
[0, 1, 0],
[-math.sin(pitch), 0, math.cos(pitch)]
])
rollMatrix = np.matrix([
[1, 0, 0],
[0, math.cos(roll), -math.sin(roll)],
[0, math.sin(roll), math.cos(roll)]
])
R = yawMatrix * pitchMatrix * rollMatrix
theta = math.acos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1) / 2)
multi = 1 / (2 * math.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta
ry = multi * (R[0, 2] - R[2, 0]) * theta
rz = multi * (R[1, 0] - R[0, 1]) * theta
return rx, ry, rz
paused=False
actual_q = rtde_r.getActualQ() #current joint rotations in radians.
print("Actual Q:"+str(actual_q))
#fw_actual_q = rtde_c.getForwardKinematics(actual_q)
fw_actual_q = rtde_c.getForwardKinematics()
print("forward Actual Q:"+str(fw_actual_q))
joint_q=rtde_c.getInverseKinematics(fw_actual_q)
print("Final Q:"+str(joint_q))
centerpos=[-0.895,-0.177,0.91]
centerrotRPY=[-1.572,0,1.572] #roll, pitch, yaw
pan=0 #-=left, +=right, degrees
tilt=0 #-=down, +=up, degrees
joint_q_init=[0,toRad(-90),toRad(90),toRad(-180),toRad(-90),toRad(0)] #start position
joint_q=joint_q_init
'''
_rot=RPYtoVec(centerrotRPY[0],centerrotRPY[1],centerrotRPY[2])
pos_q=[centerpos[0],centerpos[1],centerpos[2],_rot[0],_rot[1],_rot[2]]
joint_q2=rtde_c.getInverseKinematics(pos_q,joint_q_init)
print("Ninal Q:"+str(joint_q2))
'''
#rtde_c.moveJ([0,toRad(-90),toRad(90-tilt/2),toRad(-180-tilt/2),toRad(-90-pan),toRad(0)],0.1,0.1)
rtde_c.moveJ(joint_q,0.2,0.1) #move to initial position
# Parameters
velocity = 0.2 #0.5
acceleration = 0.1 #0.5
frequency= 100
dt = 1.0/frequency # 2ms
lookahead_time = 0.1
gain = 300
speed=[1,1,1,3,3,3]
def constrain(v,_min,_max):
return min(_max,max(_min,v))
print("Press s to start")
print("Press q to stop")
print("Press p to pause")
while not keyboard.is_pressed("s"):
if keyboard.is_pressed("q"):
exit()
time.sleep(0.1)
print("Starting Servo")
while not keyboard.is_pressed("q"):
start = time.time()
# Receive Socket stuff
try:
client, address = s.accept()
print("Connected to", address)
except socket.error:
pass
if client is not None:
try:
data = client.recv( 1024 ).decode( 'utf-8' )
if data:
lastdata=data.split('\n')
#print("Received :", repr(data))
if (len(lastdata)>1):
splitdata=lastdata[-2].split(",")
try:
_pan=float(splitdata[0])
_tilt=float(splitdata[1])
winkellimit=45
if _pan>=-winkellimit and _pan <=winkellimit and _tilt>=-winkellimit and _tilt <=winkellimit:
pan=_pan
tilt=_tilt
print("Pan="+str(pan)+" Tilt="+str(tilt))
except ValueError:
print("Not a float")
except socket.error:
pass
# Robot Control stuff
_rot=RPYtoVec(centerrotRPY[0]+toRad(tilt),centerrotRPY[1],centerrotRPY[2]+toRad(-pan))
pos_q=[centerpos[0],centerpos[1],centerpos[2],_rot[0],_rot[1],_rot[2]]
joint_q_aim=rtde_c.getInverseKinematics(pos_q,joint_q_init)
#joint_q_aim = rtde_c.getInverseKinematics([-0.895,-0.173,0.911,2.4,2.4,2.4])
# inverse kinematics: rx+ =look up
# ry+ = look right
# rz+ = rotate CW
reachable=rtde_c.isPoseWithinSafetyLimits(pos_q)
if not reachable:
print("Reachable:"+str(reachable))
insideJointConstraints=True
if joint_q_aim[0]<toRad(-20) or joint_q_aim[0]>toRad(20):
print("Joint 0 Outside constraint with "+str(joint_q_aim[0])+" ("+str(toDeg(joint_q_aim[0]))+")")
insideJointConstraints=False
if joint_q_aim[1]<toRad(-90-40) or joint_q_aim[1]>toRad(-90+40):
print("Joint 1 Outside constraint with "+str(joint_q_aim[1])+" ("+str(toDeg(joint_q_aim[1]))+")")
insideJointConstraints=False
if joint_q_aim[2]<toRad(90-90) or joint_q_aim[2]>toRad(90+40):
print("Joint 2 Outside constraint with "+str(joint_q_aim[2])+" ("+str(toDeg(joint_q_aim[2]))+")")
insideJointConstraints=False
if joint_q_aim[3]<toRad(-180-80) or joint_q_aim[3]>toRad(-180+110):
print("Joint 3 Outside constraint with "+str(joint_q_aim[3])+" ("+str(toDeg(joint_q_aim[3]))+")")
insideJointConstraints=False
if joint_q_aim[4]<toRad(-90-70) or joint_q_aim[4]>toRad(-90+70):
print("Joint 4 Outside constraint with "+str(joint_q_aim[4])+" ("+str(toDeg(joint_q_aim[4]))+")")
insideJointConstraints=False
if joint_q_aim[5]<toRad(0-60) or joint_q_aim[5]>toRad(0+60):
print("Joint 5 Outside constraint with "+str(joint_q_aim[5])+" ("+str(toDeg(joint_q_aim[5]))+")")
insideJointConstraints=False
if reachable and insideJointConstraints and not paused:
for i in range(6):
joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -speed[i]/frequency,speed[i]/frequency) #constrain joint speeds
if keyboard.is_pressed("p"):
print("paused")
paused=True
if keyboard.is_pressed("s"):
print("resumed")
paused=False
#print(joint_q)
rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain)
end = time.time()
stepduration = end - start
if stepduration < dt:
time.sleep(dt - stepduration)
print("Finished")
rtde_c.servoStop()
rtde_c.stopScript()
s.close()