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

add basedeviation

Browse source
This commit is contained in:
Philipp Kramer 2022-08-23 12:32:53 +02:00
parent 4327d8f162
commit d67c6bad1c
1 changed files with 18 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

28
pantilt_inv.py
View file

@ -117,11 +117,12 @@ if use_rtde:
winkellimit=45 winkellimit=45
maxbasedeviation=60 # in degrees, maximum rotation deviation from 0° maxbasedeviation=60 # in degrees, maximum rotation deviation from 0° #60
basespeed=1.5 basespeed=3 #1.5
distfromcenter=0.9 distfromcenter=0.9
basedeviation=0 basedeviation=0
centerpos=[math.cos(toRad(180))*distfromcenter,math.sin(toRad(180))*distfromcenter,0.91] #centerpos=[math.cos(toRad(180))*distfromcenter,math.sin(toRad(180))*distfromcenter,0.91]
centerpos=[math.cos(toRad(180))*distfromcenter,math.sin(toRad(180))*distfromcenter,1.0]
#centerpos=[-0.895,-0.177,0.91] #centerpos=[-0.895,-0.177,0.91]
centerrotRPY=[-1.572,0,1.572] #roll, pitch, yaw centerrotRPY=[-1.572,0,1.572] #roll, pitch, yaw
@ -260,24 +261,27 @@ while running:
if _pan>=-winkellimit and _pan <=winkellimit and _tilt>=-winkellimit and _tilt <=winkellimit: if _pan>=-winkellimit and _pan <=winkellimit and _tilt>=-winkellimit and _tilt <=winkellimit:
pan=_pan pan=_pan
tilt=_tilt tilt=_tilt
if not paused:
_basemovement=min(toDeg(speed[0]),max(toDeg(-speed[0]) ,pan*basespeed/frequency)) #limit speed
basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation+_basemovement))
basedeviation*=-winkellimit*basespeed/frequency/maxbasedeviation+1
basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation))
# Robot Control stuff # Robot Control stuff
if use_rtde: if use_rtde:
if not paused:
basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation+pan*basespeed/frequency))
basedeviation*=-winkellimit*basespeed/frequency/maxbasedeviation+1
basedeviation=min(maxbasedeviation,max(-maxbasedeviation,basedeviation))
baserotation=toRad(180-basedeviation) #positive is ccw #baserotation=toRad(180-basedeviation) #positive is ccw
centerpos=[math.cos(baserotation)*distfromcenter,math.sin(baserotation)*distfromcenter,0.91]
_rot=RPYtoVec(centerrotRPY[0]+toRad(tilt),centerrotRPY[1],centerrotRPY[2]+toRad(-pan)) _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]] 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(pos_q,joint_q_init)
joint_q_aim[0]+= -toRad(basedeviation)
#joint_q_aim = rtde_c.getInverseKinematics([-0.895,-0.173,0.911,2.4,2.4,2.4]) #joint_q_aim = rtde_c.getInverseKinematics([-0.895,-0.173,0.911,2.4,2.4,2.4])
# inverse kinematics: rx+ =look up # inverse kinematics: rx+ =look up
# ry+ = look right # ry+ = look right
@ -292,7 +296,11 @@ while running:
insideJointConstraints=True insideJointConstraints=True
for i in range(6): for i in range(6):
if joint_q_aim[i]<joint_min[i] or joint_q_aim[i]>joint_max[i]: if joint_q_aim[i]<joint_min[i] or joint_q_aim[i]>joint_max[i]:
print("Joint "+str(i)+" Outside constraint with "+str(joint_q_aim[i])+" ("+str(toDeg(joint_q_aim[i]))+")") if joint_q_aim[i]<joint_min[i]:
print("Joint "+str(i)+" Outside constraint with "+str(joint_q_aim[i])+" ("+str(toDeg(joint_q_aim[i]))+") min is "+str(joint_min[i])+" ("+str(toDeg(joint_min[i]))+")")
if joint_q_aim[i]>joint_max[i]:
print("Joint "+str(i)+" Outside constraint with "+str(joint_q_aim[i])+" ("+str(toDeg(joint_q_aim[i]))+") min is "+str(joint_max[i])+" ("+str(toDeg(joint_max[i]))+")")
insideJointConstraints=False insideJointConstraints=False