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()
