2.3
|
e code Python suivant montre comment composer des limites à plages multiples pour plusieurs liaisons. Ajoutez ces lignes au fichier Python de l'étape 2.1.
# Import libraries
from centypes import *
from cenpylib import DynamicLimits as DL
def CheckJointsDynamicLimits(joints):
j1 = joints[0]
j2 = joints[1]
j3 = joints[2]
j5 = joints[4]
j6 = joints[5]
# Check J1 dependent on J2
j1min = InRange(j2, -65, -47)*DL.Ramp(j2, -65, -125, -47, -154) + InRange(j2, -47, 180)*-170
j1max = InRange(j2, -65, -41)*DL.Ramp(j2, -65, 130, -41, 151) + InRange(j2, -41, 180)*170
if not (j1 >= j1min and j1 <= j1max):
return UNREACHABLE
# Check J3 dependent on J2
j3min = InRange(j2, -65, 35)*DL.Ramp(j2, -65, -80, -35, -15) + InRange(j2, -35, 180)*-80
j3max = InRange(j2, -65, 180)*100
if not (j3 >= j3min and j3 <= j3max):
return UNREACHABLE
# Check J6 dependent on J5
j6min = InRange(j5,-140,-70)*DL.Ramp(j5,-140,-110,-70,-165) + InRange(j5,-70,-35)*DL.Ramp(j5,-70,-165,-35,-200) + InRange(j5,-35,-20)*DL.Ramp(j5,-35,-200,-20,-300)
j6max = InRange(j5,-140,-70)*DL.Ramp(j5,-140,110,-70,165) + InRange(j5,-70,-35)*DL.Ramp(j5,-70,165,-35,200) + InRange(j5,-35,-20)*DL.Ramp(j5,-35,200,-20,300)
if not (j6 >= j6min and j6 <= j6max):
return UNREACHABLE
return REACHABLE
def InRange(joint, x1, x2):
"""Dynamic limits utility functions.
Returns:
integer: 0 or 1.
"""
if joint < x1 or joint > x2:
return 0
else:
return 1
|
