add basedeviation
This commit is contained in:
parent
4327d8f162
commit
d67c6bad1c
1 changed files with 18 additions and 10 deletions
|
@ -117,11 +117,12 @@ if use_rtde:
|
|||
|
||||
winkellimit=45
|
||||
|
||||
maxbasedeviation=60 # in degrees, maximum rotation deviation from 0°
|
||||
basespeed=1.5
|
||||
maxbasedeviation=60 # in degrees, maximum rotation deviation from 0° #60
|
||||
basespeed=3 #1.5
|
||||
distfromcenter=0.9
|
||||
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]
|
||||
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:
|
||||
pan=_pan
|
||||
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
|
||||
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
|
||||
centerpos=[math.cos(baserotation)*distfromcenter,math.sin(baserotation)*distfromcenter,0.91]
|
||||
#baserotation=toRad(180-basedeviation) #positive is ccw
|
||||
_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[0]+= -toRad(basedeviation)
|
||||
|
||||
#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
|
||||
|
@ -292,7 +296,11 @@ while running:
|
|||
insideJointConstraints=True
|
||||
for i in range(6):
|
||||
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
|
||||
|
||||
|
||||
|
|
Loading…
Reference in a new issue