def __init__(self, dev='/dev/ttyUSB0'):
self.dxl_type= 'MX-64AR'
self.dev= dev
self.baudrate= 2e6
self.op_mode= 'CURRPOS'
self.dxl= TDynamixel1(self.dxl_type,dev=self.dev)
#Thread locker:
self.port_locker= threading.RLock()
self.state_locker= threading.RLock()
self.moveth_locker= threading.RLock()
self.state= {'stamp':0.0, 'position':None, 'velocity':None, 'effort':None}
self.moveth_cmd= {'pos':None,'max_effort':None}
self.hz_state_obs= 40 #State observation rate (Hz).
self.hz_moveth_ctrl= 60 #MoveTh control rate (Hz).
self.threads= { #ThreadName:[IsActive,ThreadObject]
'StateObserver':[False,None],
'MoveThController':[False,None],}
self.CmdMax= 2104 #Gripper opened widely.
self.CmdMin= 50 #Gripper closed strongly.
self.CmdOpen= 809 #Gripper opened.
self.CmdClose= 233 #Gripper closed.
#Gripper command-position conversions.
self.ezg_range= [0.0,0.150]
self.ezg_cmd2pos= lambda cmd: max(self.ezg_range[0], self.ezg_range[0] + (cmd-self.CmdClose)*(self.ezg_range[1]-self.ezg_range[0])/(self.CmdOpen-self.CmdClose))
self.ezg_pos2cmd= lambda pos: self.CmdClose + (pos-self.ezg_range[0])*(self.CmdOpen-self.CmdClose)/(self.ezg_range[1]-self.ezg_range[0])
self.ezg_cmd2vel= lambda cmd: (self.ezg_range[1]-self.ezg_range[0])/(self.dxl.ConvPos(self.CmdOpen)-self.dxl.ConvPos(self.CmdClose))*self.dxl.ConvVel(cmd)
self.ezg_vel2cmd= lambda vel: self.dxl.InvConvVel(vel*(self.dxl.ConvPos(self.CmdOpen)-self.dxl.ConvPos(self.CmdClose))/(self.ezg_range[1]-self.ezg_range[0]))
def __init__(self, dev='/dev/ttyUSB0'):
self.dxl_type= 'XM430-W350'
self.dev= dev
self.baudrate= 2e6
self.dxl= TDynamixel1(self.dxl_type,dev=self.dev)
#Thread locker:
self.port_locker= threading.RLock()
self.state_locker= threading.RLock()
self.moveth_locker= threading.RLock()
self.state= {'stamp':0.0, 'position':None, 'velocity':None, 'effort':None}
self.moveth_cmd= {'pos':None,'max_effort':None}
self.hz_state_obs= 40 #State observation rate (Hz).
self.hz_moveth_ctrl= 60 #MoveTh control rate (Hz).
self.threads= { #ThreadName:[IsActive,ThreadObject]
'StateObserver':[False,None],
'MoveThController':[False,None],}
#Holding controller:
self.holding= None
self.holding_max_pwm_rate= 0.9
#self.CmdMax= 2382 #Gripper closed.
self.CmdMax= 2200 #Gripper closed (with FingerVision).
self.CmdMin= 1200 #Gripper opened widely.
self.CmdOpen= 1900 #Gripper opened moderately.
#Gripper command-position conversions.
self.dxlg_range= [0.0,0.095]
self.dxlg_cmd2pos= lambda cmd: self.dxlg_range[1] + (cmd-self.CmdOpen)*(self.dxlg_range[0]-self.dxlg_range[1])/(self.CmdMax-self.CmdOpen)
self.dxlg_pos2cmd= lambda pos: self.CmdOpen + (pos-self.dxlg_range[1])*(self.CmdMax-self.CmdOpen)/(self.dxlg_range[0]-self.dxlg_range[1])
self.dxlg_cmd2vel= lambda cmd: (self.dxlg_range[0]-self.dxlg_range[1])/(self.dxl.ConvPos(self.CmdMax)-self.dxl.ConvPos(self.CmdOpen))*self.dxl.ConvVel(cmd)
self.dxlg_vel2cmd= lambda vel: self.dxl.InvConvVel(vel*(self.dxl.ConvPos(self.CmdMax)-self.dxl.ConvPos(self.CmdOpen))/(self.dxlg_range[0]-self.dxlg_range[1]))
def __init__(self, dev='/dev/ttyUSB0'):
self.dxl_type= ['XH540-W270']*2
self.dxl_ids= [1,2]
self.dev= dev
self.baudrate= 2e6
self.dxl= [TDynamixel1(dxl_type,dev=self.dev) for dxl_type in self.dxl_type]
#Thread locker:
self.port_locker= threading.RLock()
self.state_locker= threading.RLock()
self.moveth_locker= threading.RLock()
self.state= {'stamp':0.0, 'position':[], 'velocity':[], 'effort':[]}
self.moveth_cmd= {'pos':[],'max_effort':[]}
self.hz_state_obs= 40 #State observation rate (Hz).
self.hz_moveth_ctrl= 60 #MoveTh control rate (Hz).
self.threads= { #ThreadName:[IsActive,ThreadObject]
'StateObserver':[False,None],
'MoveThController':[False,None],}
#CmdMinMax= [[CmdMin[0],CmdMin[1]],[CmdMax[0],CmdMax[1]]]
self.CmdMinMax= [
[2048-370,2048-1110], #id1-closed,id2-opened
[2048+1110,2048+370], #id1-opened,id2-closed
]
self.CmdOrigin=[2048,2048] #closed,closed
self.pos_open= [0.6136]*2 #cmd=+400
self.pos_close= [0.1657]*2 #cmd=+108; With FV+
#Gripper command-position conversions.
self.gripper_range= [[-0.5676]*2,[1.7027]*2] #Position range in radian (lower, upper).
self.gripper_cmd2pos= lambda cmd: [self.dxl[0].ConvPos(cmd[0]),-self.dxl[1].ConvPos(cmd[1])]
self.gripper_pos2cmd= lambda pos: [self.dxl[0].InvConvPos(pos[0]),self.dxl[1].InvConvPos(-pos[1])]
self.gripper_cmd2vel= lambda cmd: [self.dxl[0].ConvVel(cmd[0]),-self.dxl[1].ConvVel(cmd[1])]
self.gripper_vel2cmd= lambda vel: [self.dxl[0].InvConvVel(vel[0]),self.dxl[1].InvConvVel(-vel[1])]
def __init__(self, dev='/dev/ttyUSB0'):
self.dxl_type = 'RH-P12-RN'
self.dev = dev
self.baudrate = 2e6
self.dxl = TDynamixel1(self.dxl_type, dev=self.dev)
#Thread locker:
self.port_locker = threading.RLock()
self.state_locker = threading.RLock()
self.moveth_locker = threading.RLock()
self.state = {
'stamp': 0.0,
'position': None,
'velocity': None,
'effort': None
}
self.moveth_cmd = {'pos': None, 'max_effort': None}
self.hz_state_obs = 40 #State observation rate (Hz).
self.hz_moveth_ctrl = 60 #MoveTh control rate (Hz).
self.threads= { #ThreadName:[IsActive,ThreadObject]
'StateObserver':[False,None],
'MoveThController':[False,None],}
self.CmdMax = 776 #Gripper closed.
self.CmdMin = 26 #Gripper opened.
#Gripper command-position conversions.
self.thg_range = [0.0, 0.109]
self.thg_cmd2pos = lambda cmd: self.thg_range[1] + (
cmd - self.CmdMin) * (self.thg_range[0] - self.thg_range[1]) / (
self.CmdMax - self.CmdMin)
self.thg_pos2cmd = lambda pos: self.CmdMin + (pos - self.thg_range[
1]) * (self.CmdMax - self.CmdMin) / (self.thg_range[0] - self.
thg_range[1])
self.thg_cmd2vel = lambda cmd: (self.thg_range[0] - self.thg_range[
1]) / (self.dxl.ConvPos(self.CmdMax) - self.dxl.ConvPos(
self.CmdMin)) * self.dxl.ConvVel(cmd)
self.thg_vel2cmd = lambda vel: self.dxl.InvConvVel(
vel *
(self.dxl.ConvPos(self.CmdMax) - self.dxl.ConvPos(self.CmdMin)) /
(self.thg_range[0] - self.thg_range[1]))
def __init__(self, dev='/dev/ttyUSB0', finger_type=None):
self.dxl_type= 'PH54-200-S500'
self.dev= dev
self.baudrate= 2e6
self.dxl= TDynamixel1(self.dxl_type,dev=self.dev)
#Thread locker:
self.port_locker= threading.RLock()
self.state_locker= threading.RLock()
self.moveth_locker= threading.RLock()
self.state= {'stamp':0.0, 'position':None, 'velocity':None, 'effort':None}
self.moveth_cmd= {'pos':None,'max_effort':None}
self.hz_state_obs= 40 #State observation rate (Hz).
self.hz_moveth_ctrl= 60 #MoveTh control rate (Hz).
self.threads= { #ThreadName:[IsActive,ThreadObject]
'StateObserver':[False,None],
'MoveThController':[False,None],}
if finger_type is None:
raise Exception('TDxlpO2: finger_type is not specified. Set finger type such as Straight1.')
elif finger_type=='Straight1':
self.CmdMax= 0 #Gripper opened widely.
self.CmdMin= -250962 #Gripper closed.
self.CmdOpen= -150000 #Gripper opened.
self.CmdClose= -250962 #Gripper closed.
#Gripper range in meter:
self.GrpClose= 0.0
self.GrpOpen= 0.300
self.gripper_range= [0.0,0.300] #FIXME: 0.3 is inaccurate.
elif finger_type=='SRound1': #Small, round finger (yellow)
self.CmdMax= 0 #Gripper opened widely.
self.CmdMin= -250962 #Gripper closed.
self.CmdOpen= -150000 #Gripper opened.
self.CmdClose= -250962 #Gripper closed.
#Gripper range in meter:
self.GrpClose= 0.0
self.GrpOpen= 0.1950
self.gripper_range= [0.0,0.1950]
#elif finger_type=='Fork1':
#self.CmdMax= 0 #Gripper opened widely.
#self.CmdMin= -200962 #Gripper closed.
#self.CmdOpen= -150000 #Gripper opened.
#self.CmdClose= -200962 #Gripper closed.
##Gripper range in meter:
#self.GrpClose= 0.03
#self.GrpOpen= 0.23
#self.gripper_range= [0.03,0.230]
#elif finger_type=='Fork1': #TODO:FIXME:Change this name to Fork2
#self.CmdMax= 0 #Gripper opened widely. #DxlPo2f1
##self.CmdMin= -234000 #Gripper closed.
#self.CmdMin= -243160 #Gripper closed.
##self.CmdOpen= -124100 #Gripper opened.
#self.CmdOpen= -134500 #Gripper opened.
##self.CmdClose= -234000 #Gripper closed.
#self.CmdClose= -243160 #Gripper closed.
##Gripper range in meter:
#self.GrpClose= 0.0
#self.GrpOpen= 0.23
#self.gripper_range= [0.0,0.23]
elif finger_type=='Fork1': #TODO:FIXME:Change this name to Fork2cross
self.CmdMax= 150000 #Gripper opened widely. #DxlPo2f1
self.CmdMin= 10000 #Gripper over-closed (crossed).
self.CmdOpen= 111181 #Gripper opened.
self.CmdClose= 18732 #Gripper closed.
#Gripper range in meter:
self.GrpClose= 0.0
self.GrpOpen= 0.20
self.gripper_range= [-0.0189,0.20] #-0.008 is obtained by gripper_cmd2pos(CmdMin) w/o gripper_range.
#FIXME:Overwirting for the tong attachment:
self.CmdMin= -9500 #Gripper over-closed (crossed).
self.CmdClose= -2805 #Gripper closed.
self.CmdOpen= 75678 #Gripper opened.
self.gripper_range= [-0.017,0.20] #min is obtained by gripper_cmd2pos(CmdMin) w/o gripper_range.
elif finger_type=='???':
self.CmdMax= 0 #Gripper opened widely.
self.CmdMin= -160000 #Gripper closed.
self.CmdOpen= -150000 #Gripper opened.
self.CmdClose= -160000 #Gripper closed.
#Gripper range in meter:
self.GrpClose= 0.0
self.GrpOpen= 0.2200
self.gripper_range= [0.0,0.2200]
else:
raise Exception('TDxlpO2: Unknown finger_type: {finger_type}'.format(finger_type=finger_type))
#Gripper command-position conversions.
self.gripper_cmd2pos= lambda cmd: min(self.gripper_range[1],max(self.gripper_range[0], self.GrpClose + (cmd-self.CmdClose)*(self.GrpOpen-self.GrpClose)/(self.CmdOpen-self.CmdClose) ))
self.gripper_pos2cmd= lambda pos: min(self.CmdMax,max(self.CmdMin, self.CmdClose + (pos-self.GrpClose)*(self.CmdOpen-self.CmdClose)/(self.GrpOpen-self.GrpClose) ))
#WARNING: The following functions are not considered well (coped from other gripper).
self.gripper_cmd2vel= lambda cmd: (self.GrpOpen-self.GrpClose)/(self.dxl.ConvPos(self.CmdOpen)-self.dxl.ConvPos(self.CmdClose))*self.dxl.ConvVel(cmd)
self.gripper_vel2cmd= lambda vel: self.dxl.InvConvVel(vel*(self.dxl.ConvPos(self.CmdOpen)-self.dxl.ConvPos(self.CmdClose))/(self.GrpOpen-self.GrpClose))
def __init__(self, dev='/dev/ttyUSB0', finger_type=None):
self.dxl_type= 'PH54-200-S500'
self.dev= dev
self.baudrate= 2e6
self.dxl= TDynamixel1(self.dxl_type,dev=self.dev)
#Thread locker:
self.port_locker= threading.RLock()
self.state_locker= threading.RLock()
self.moveth_locker= threading.RLock()
self.state= {'stamp':0.0, 'position':None, 'velocity':None, 'effort':None}
self.moveth_cmd= {'pos':None,'max_effort':None}
self.hz_state_obs= 40 #State observation rate (Hz).
self.hz_moveth_ctrl= 60 #MoveTh control rate (Hz).
self.threads= { #ThreadName:[IsActive,ThreadObject]
'StateObserver':[False,None],
'MoveThController':[False,None],}
if finger_type is None:
raise Exception('TDxlpO2: finger_type is not specified. Set finger type such as Straight1.')
elif finger_type=='Straight1':
self.CmdMax= 0 #Gripper opened widely.
self.CmdMin= -250962 #Gripper closed.
self.CmdOpen= -150000 #Gripper opened.
self.CmdClose= -250962 #Gripper closed.
#Gripper range in meter:
self.gripper_range= [0.0,0.300] #FIXME: 0.3 is inaccurate.
elif finger_type=='SRound1': #Small, round finger (yellow)
self.CmdMax= 0 #Gripper opened widely.
self.CmdMin= -250962 #Gripper closed.
self.CmdOpen= -150000 #Gripper opened.
self.CmdClose= -250962 #Gripper closed.
#Gripper range in meter:
self.gripper_range= [0.0,0.1950]
elif finger_type=='Fork1':
self.CmdMax= 0 #Gripper opened widely.
self.CmdMin= -200962 #Gripper closed.
self.CmdOpen= -150000 #Gripper opened.
self.CmdClose= -200962 #Gripper closed.
#Gripper range in meter:
self.gripper_range= [0.03,0.230]
elif finger_type=='???':
self.CmdMax= 0 #Gripper opened widely.
self.CmdMin= -160000 #Gripper closed.
self.CmdOpen= -150000 #Gripper opened.
self.CmdClose= -160000 #Gripper closed.
#Gripper range in meter:
self.gripper_range= [0.0,0.2200]
else:
raise Exception('TDxlpO2: Unknown finger_type: {finger_type}'.format(finger_type=finger_type))
#Gripper command-position conversions.
self.gripper_cmd2pos= lambda cmd: max(self.gripper_range[0], self.gripper_range[0] + (cmd-self.CmdClose)*(self.gripper_range[1]-self.gripper_range[0])/(self.CmdOpen-self.CmdClose))
self.gripper_pos2cmd= lambda pos: self.CmdClose + (pos-self.gripper_range[0])*(self.CmdOpen-self.CmdClose)/(self.gripper_range[1]-self.gripper_range[0])
#WARNING: The following functions are not considered well (coped from other gripper).
self.gripper_cmd2vel= lambda cmd: (self.gripper_range[1]-self.gripper_range[0])/(self.dxl.ConvPos(self.CmdOpen)-self.dxl.ConvPos(self.CmdClose))*self.dxl.ConvVel(cmd)
self.gripper_vel2cmd= lambda vel: self.dxl.InvConvVel(vel*(self.dxl.ConvPos(self.CmdOpen)-self.dxl.ConvPos(self.CmdClose))/(self.gripper_range[1]-self.gripper_range[0]))