class Arm: def __init__(self, port=USB_PORT): self.port = port self.goal = Point(0, 0, 0, 0) self.opened = False self.pi = None def open(self): self.pi = pigpio.pi() self.dyn_chain = DxlChain(self.port, rate=1000000) self.dyn_chain.open() self.motors = self.dyn_chain.get_motor_list() assert len( self.motors ) == 6, 'Some arm motors are missing. Expected 6 instead got %d' % len( self.motors) self.tyro_manager = TyroManager(self.dyn_chain) self.tyro_manager.start() self.opened = True def close(self): self.opened = False self.dyn_chain.close() def goto2D(self, y, z, r, speed=50): ''' Uses inverse kinematic to go to a position in planar space (only y and z) ''' r = math.radians(r) self.goal = Point(0, y, z, r) joints = ik(*self.goal) joints = map(math.degrees, joints) goals = angles_to_motors(*joints) return self.write_goal_without_base(goals[1], goals[2], goals[3], speed=speed) def goto(self, x, y, z, r, speed=50): ''' Uses inverse kinematic to go to a position in space ''' r = math.radians(r) self.goal = Point(x, y, z, r) joints = ik(*self.goal) joints = map(math.degrees, joints) goals = angles_to_motors(*joints) return self.write_goal(*goals, speed=speed) def write_goal_without_base(self, goal23, goal4, goal5, speed=50): ''' Write goal positions of all servos with given speed ''' goal23 = clamp(goal23, MOTORS[2]['min'], MOTORS[2]['max']) goal4 = clamp(goal4, MOTORS[4]['min'], MOTORS[4]['max']) goal5 = clamp(goal5, MOTORS[5]['min'], MOTORS[5]['max']) if isinstance(speed, list): s23, s4, s5 = speed self.dyn_chain.sync_write_pos_speed( [2, 3, 4, 5], [goal23, 1023 - goal23, goal4, goal5], [s23, s23, s4, s5]) else: self.dyn_chain.sync_write_pos_speed( [2, 3, 4, 5], [goal23, 1023 - goal23, goal4, goal5], [speed] * 5) def write_single_goal(self, motor_id, value, speed=50): if motor_id == 2: self.dyn_chain.sync_write_pos_speed([2, 3], [value, 1023 - value], [speed] * 2) else: self.dyn_chain.sync_write_pos_speed([motor_id], [value], [speed]) def write_goal(self, goal1, goal23, goal4, goal5, speed=50): ''' Write goal positions of all servos with given speed ''' goal1 = clamp(goal1, MOTORS[1]['min'], MOTORS[1]['max']) goal23 = clamp(goal23, MOTORS[2]['min'], MOTORS[2]['max']) goal4 = clamp(goal4, MOTORS[4]['min'], MOTORS[4]['max']) goal5 = clamp(goal5, MOTORS[5]['min'], MOTORS[5]['max']) if isinstance(speed, list): s1, s23, s4, s5 = speed self.dyn_chain.sync_write_pos_speed( [1, 2, 3, 4, 5], [goal1, goal23, 1023 - goal23, goal4, goal5], [s1, s23, s23, s4, s5]) else: self.dyn_chain.sync_write_pos_speed( [1, 2, 3, 4, 5], [goal1, goal23, 1023 - goal23, goal4, goal5], [speed] * 5) def disable_all(self): ''' Disable torque of all motors ''' self.dyn_chain.disable() def move_base(self, direction, speed=30): ''' Moves the base in the given direction at the given speed Direction = 1 / -1 for cw/ccw direction, 0 => stop ''' if direction == 1: self.dyn_chain.goto(1, MOTORS[1]['min'], speed=speed, blocking=False) elif direction == -1: self.dyn_chain.goto(1, MOTORS[1]['max'], speed=speed, blocking=False) else: self.dyn_chain.disable(1) def get_angles(self): ''' Estimates joints angle based on servos positions ''' return motors_to_angles(*self.get_position()) def get_position(self): ''' Servos positions ''' positions = self.dyn_chain.get_position() return (positions[1], positions[2], positions[4], positions[5]) def motors_load(self): ''' Ratio of maximal torque of each joints. A value of 0.5 means 50% of maximmal torque applied in clockwise direction A value of -0.25 means 25% of maximmal torque applied in counter-clockwise Direction Return value is (motor1, motor2, motor4, motor5) ''' loads = [] for i in [1, 2, 4, 5, 8]: present_load = self.dyn_chain.get_reg(i, "present_load") ratio = (present_load & 1023) / 1023.0 direction = ((present_load >> 10) & 1) * 2 - 1 loads.append(ratio * direction) return loads def wait_stopped(self): ''' Sleeps until all the motors reached their goals ''' self.dyn_chain.wait_stopped([1, 2, 3, 4, 5]) def stop_movement(self): ''' Sets the goal to the current position of the motors ''' positions = self.get_position() self.write_goal(*positions) def wait_stopped_sleep(self, sleep=time.sleep): ''' Sleeps using the provided function until all the motors reached their goals ''' ids = self.dyn_chain.get_motors([1, 2, 3, 4, 5]) while True: moving = False for id in ids: if self.dyn_chain.get_reg(id, 'moving') != 0: moving = True break if not moving: break sleep(0.1) def set_tyro_manager_state(self, state): self.tyro_manager.state = state def get_chain_info(self): dyn_infos = {} loads = self.motors_load() positions = self.get_position() for i in [1, 2]: motor = { 'temp': self.dyn_chain.get_reg(i, 'present_temp'), 'load': loads[i - 1], 'present_pos': positions[i - 1] } dyn_infos['motor' + str(i)] = motor for i in [4, 5]: motor = { 'temp': self.dyn_chain.get_reg(i, 'present_temp'), 'load': loads[i - 2], 'present_pos': positions[i - 2] } dyn_infos['motor' + str(i)] = motor dyn_infos['motor3'] = { 'temp': self.dyn_chain.get_reg(3, 'present_temp'), 'load': loads[1], 'present_pos': positions[1] } dyn_infos['motor8'] = { 'temp': self.dyn_chain.get_reg(8, 'present_temp'), 'load': loads[4], 'present_pos': 0 } return dyn_infos def reset_servos_torque(self): servos = [1, 2, 3, 4, 5, 8] self.dyn_chain.disable(servos) for id in servos: self.dyn_chain.set_reg(id, 'torque_limit', 1023)
class HWCoupling: def __init__(self): rospy.init_node('full_hw_controller') self.excitation = False self.lower_limit = False self.upper_limit = False self.calibrated = False self.chain = DxlChain(rospy.get_param('~port'), rate=rospy.get_param('~rate')) self.motors = self.chain.get_motor_list() self.pub_reset_enc = rospy.Publisher( '/scara_setup/linear_encoder/reset', Empty, queue_size=100) rospy.Subscriber("/full_hw_controller/linear/command", Float64, self.callback_linear) rospy.Subscriber("/full_hw_controller/linear/override", Float64, self.callback_linear_override) self.shoulder_pub = rospy.Publisher( "/full_hw_controller/shoulder/state", Float64, queue_size=10) rospy.Subscriber("/full_hw_controller/shoulder/command", Float64, self.callback_shoulder) self.elbow_pub = rospy.Publisher("/full_hw_controller/elbow/state", Float64, queue_size=10) rospy.Subscriber("/full_hw_controller/elbow/command", Float64, self.callback_elbow) self.wrist_pub = rospy.Publisher("/full_hw_controller/wrist/state", Float64, queue_size=10) rospy.Subscriber("/full_hw_controller/wrist/command", Float64, self.callback_wrist) self.fingerjoint_pub = rospy.Publisher( "/full_hw_controller/fingerjoint/state", Float64, queue_size=10) rospy.Subscriber("/full_hw_controller/fingerjoint/command", Float64, self.callback_fingerjoint) rospy.Subscriber("/full_hw_controller/set_excitation", Bool, self.callback_excitation) rospy.Subscriber("/scara_setup/linear_encoder/lower_limit", Bool, self.callback_lower_limit) rospy.Subscriber("/scara_setup/linear_encoder/upper_limit", Bool, self.callback_upper_limit) rospy.Subscriber("/scara_setup/linear_encoder/calibrated", Bool, self.callback_calibrated) rospy.Subscriber("/full_hw_controller/cog_linear", Empty, self.callback_cog_linear) self.rate = rospy.Rate(20) # 10hz self.set_excitation(False) def callback_cog_linear(self, data): self.callback_linear_override(Float64(-0.4)) while self.lower_limit == False: pass self.pub_reset_enc.publish(Bool(True)) rospy.sleep(0.05) self.callback_linear_override(Float64(0.4)) while self.calibrated == False: pass self.callback_linear_override(Float64(0.0)) def set_excitation(self, par): if par: val = 1 #print "Excitation ON" self.excitation = True else: val = 0 self.excitation = False #print "Excitation OFF" self.chain.set_reg(1, "torque_enable", val) self.chain.set_reg(2, "torque_enable", val) self.chain.set_reg(3, "torque_enable", val) self.chain.set_reg(4, "torque_enable", val) self.chain.set_reg(5, "torque_enable", val) def callback_linear(self, data): command = abs(round(data.data * 25600)) #if command > 5: # command = command + 80 if command > 1023: command = 1023 if data.data < 0: command = command + 1024 if self.lower_limit and (data.data < 0.0): return if self.upper_limit and (data.data > 0.0): return if self.excitation: self.chain.set_reg(1, "moving_speed", command) def callback_linear_override(self, data): if data.data == 0.0: self.chain.set_reg(1, "torque_enable", 0) return self.chain.set_reg(1, "torque_enable", 1) command = abs(round(data.data * 25600)) #if command > 5: # command = command + 80 if command > 1023: command = 1023 if data.data < 0: command = command + 1024 if self.lower_limit and (data.data < 0.0): return if self.upper_limit and (data.data > 0.0): return self.chain.set_reg(1, "moving_speed", command) def callback_excitation(self, data): self.set_excitation(data.data) def callback_calibrated(self, data): self.calibrated = data.data def callback_lower_limit(self, data): self.lower_limit = data.data if self.lower_limit and self.chain.get_reg(1, "present_speed") > 1024: self.chain.set_reg(1, "moving_speed", 0) def callback_upper_limit(self, data): self.upper_limit = data.data if self.upper_limit and self.chain.get_reg(1, "present_speed") < 1024: self.chain.set_reg(1, "moving_speed", 0) def callback_shoulder(self, data): command = 2048 + round(data.data / 0.001533203) #chain.goto(2, command, speed=0, blocking=False) if self.excitation: self.chain.set_reg(2, "goal_pos", command) def callback_elbow(self, data): command = 2048 + round(data.data / 0.001533203) #chain.goto(3, command, speed=0, blocking=False) if self.excitation: self.chain.set_reg(3, "goal_pos", command) def callback_wrist(self, data): command = 2048 + round(data.data / 0.001533203) #chain.goto(4, command, speed=0, blocking=False) if self.excitation: self.chain.set_reg(4, "goal_pos", command) def callback_fingerjoint(self, data): command = 512 + round(data.data / 0.005117188) #chain.goto(5, command, speed=0, blocking=False) if self.excitation: self.chain.set_reg(5, "goal_pos", command) def loop(self): while not rospy.is_shutdown(): rawval = (self.chain.get_reg(2, "present_position") - 2048) * 0.001533203 self.shoulder_pub.publish(Float64(rawval)) rawval = (self.chain.get_reg(3, "present_position") - 2048) * 0.001533203 self.elbow_pub.publish(Float64(rawval)) rawval = (self.chain.get_reg(4, "present_position") - 2048) * 0.001533203 self.wrist_pub.publish(Float64(rawval)) rawval = (self.chain.get_reg(5, "present_position") - 512) * 0.005117188 self.fingerjoint_pub.publish(Float64(rawval)) self.rate.sleep() self.set_excitation(False) self.chain.disable()
curses() """ # Move a bit chain.goto(1,500,speed=200) # Motor ID 1 is sent to position 500 with high speed chain.goto(1,400) # Motor ID 1 is sent to position 100 with last speed value chain.goto(1,0, speed=1023) chain.goto(1,750, speed=1023) # move Y axis motor chain.goto(2,150, speed=1023) # straight vertical chain.goto(2,550) # should be lowest available # outofwhacking everything chain.goto(1,random.randint(0,750),speed=1023) chain.goto(2,random.randint(150,550) ,speed=1023) #zero everything back chain.goto(1,512, speed=144) chain.goto(2,512, speed=144) while chain.is_moving(): print chain.get_reg_si(id, "present_position") # Move and print current position of all motors while moving chain.goto(1,400,speed=20,blocking=False) # Motor ID 1 is sent to position 1000 while chain.is_moving(): print chain.get_position() """ # Disable the motors chain.disable()
class HWCoupling: def __init__(self): rospy.init_node('full_hw_controller') self.excitation = False self.lower_limit = False self.upper_limit = False self.calibrated = False self.chain = DxlChain(rospy.get_param('~port'), rate=rospy.get_param('~rate')) self.motors = self.chain.get_motor_list() self.pub_reset_enc = rospy.Publisher('/scara_setup/linear_encoder/reset', Empty, queue_size=100) rospy.Subscriber("/full_hw_controller/linear/command", Float64, self.callback_linear) rospy.Subscriber("/full_hw_controller/linear/override", Float64, self.callback_linear_override) self.shoulder_pub = rospy.Publisher("/full_hw_controller/shoulder/state", Float64, queue_size=10) rospy.Subscriber("/full_hw_controller/shoulder/command", Float64, self.callback_shoulder) self.elbow_pub = rospy.Publisher("/full_hw_controller/elbow/state", Float64, queue_size=10) rospy.Subscriber("/full_hw_controller/elbow/command", Float64, self.callback_elbow) self.wrist_pub = rospy.Publisher("/full_hw_controller/wrist/state", Float64, queue_size=10) rospy.Subscriber("/full_hw_controller/wrist/command", Float64, self.callback_wrist) self.fingerjoint_pub = rospy.Publisher("/full_hw_controller/fingerjoint/state", Float64, queue_size=10) rospy.Subscriber("/full_hw_controller/fingerjoint/command", Float64, self.callback_fingerjoint) rospy.Subscriber("/full_hw_controller/set_excitation", Bool, self.callback_excitation) rospy.Subscriber("/scara_setup/linear_encoder/lower_limit", Bool, self.callback_lower_limit) rospy.Subscriber("/scara_setup/linear_encoder/upper_limit", Bool, self.callback_upper_limit) rospy.Subscriber("/scara_setup/linear_encoder/calibrated", Bool, self.callback_calibrated) rospy.Subscriber("/full_hw_controller/cog_linear", Empty, self.callback_cog_linear) self.rate = rospy.Rate(20) # 10hz self.set_excitation(False) def callback_cog_linear(self, data): self.callback_linear_override(Float64(-0.4)) while self.lower_limit == False: pass self.pub_reset_enc.publish(Bool(True)) rospy.sleep(0.05) self.callback_linear_override(Float64(0.4)) while self.calibrated == False: pass self.callback_linear_override(Float64(0.0)) def set_excitation(self, par): if par: val = 1 #print "Excitation ON" self.excitation = True else : val = 0 self.excitation = False #print "Excitation OFF" self.chain.set_reg(1, "torque_enable", val) self.chain.set_reg(2, "torque_enable", val) self.chain.set_reg(3, "torque_enable", val) self.chain.set_reg(4, "torque_enable", val) self.chain.set_reg(5, "torque_enable", val) def callback_linear(self, data): command = abs(round(data.data * 25600)) #if command > 5: # command = command + 80 if command > 1023: command = 1023 if data.data < 0: command = command + 1024 if self.lower_limit and (data.data < 0.0): return if self.upper_limit and (data.data > 0.0): return if self.excitation: self.chain.set_reg(1, "moving_speed", command) def callback_linear_override(self, data): if data.data == 0.0: self.chain.set_reg(1, "torque_enable", 0) return self.chain.set_reg(1, "torque_enable", 1) command = abs(round(data.data * 25600)) #if command > 5: # command = command + 80 if command > 1023: command = 1023 if data.data < 0: command = command + 1024 if self.lower_limit and (data.data < 0.0): return if self.upper_limit and (data.data > 0.0): return self.chain.set_reg(1, "moving_speed", command) def callback_excitation(self, data): self.set_excitation(data.data) def callback_calibrated(self, data): self.calibrated = data.data def callback_lower_limit(self, data): self.lower_limit = data.data if self.lower_limit and self.chain.get_reg(1, "present_speed") > 1024: self.chain.set_reg(1, "moving_speed", 0) def callback_upper_limit(self, data): self.upper_limit = data.data if self.upper_limit and self.chain.get_reg(1, "present_speed") < 1024: self.chain.set_reg(1, "moving_speed", 0) def callback_shoulder(self, data): command = 2048 + round(data.data / 0.001533203) #chain.goto(2, command, speed=0, blocking=False) if self.excitation: self.chain.set_reg(2, "goal_pos", command) def callback_elbow(self, data): command = 2048 + round(data.data / 0.001533203) #chain.goto(3, command, speed=0, blocking=False) if self.excitation: self.chain.set_reg(3, "goal_pos", command) def callback_wrist(self, data): command = 2048 + round(data.data / 0.001533203) #chain.goto(4, command, speed=0, blocking=False) if self.excitation: self.chain.set_reg(4, "goal_pos", command) def callback_fingerjoint(self, data): command = 512 + round(data.data / 0.005117188) #chain.goto(5, command, speed=0, blocking=False) if self.excitation: self.chain.set_reg(5, "goal_pos", command) def loop(self): while not rospy.is_shutdown(): rawval = (self.chain.get_reg(2, "present_position") - 2048) * 0.001533203 self.shoulder_pub.publish(Float64(rawval)) rawval = (self.chain.get_reg(3, "present_position") - 2048) * 0.001533203 self.elbow_pub.publish(Float64(rawval)) rawval = (self.chain.get_reg(4, "present_position") - 2048) * 0.001533203 self.wrist_pub.publish(Float64(rawval)) rawval = (self.chain.get_reg(5, "present_position") - 512) * 0.005117188 self.fingerjoint_pub.publish(Float64(rawval)) self.rate.sleep() self.set_excitation(False) self.chain.disable()