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)
from dxl.dxlchain import DxlChain
from dxl.dxlcore import Dxl
port="COM21"
timeout=0.01
rates=[2000000/(data+1) for data in range(0,255)]
rates.append(2250000)
rates.append(2500000)
rates.append(3000000)
chain=DxlChain(port,rate=1000000,timeout=timeout)
for rate in rates:
print "rate",rate
chain.reopen(port,rate,timeout=timeout)
try:
chain.factory_reset(Dxl.BROADCAST)
except Exception,e:
print e
chain.close()
from dxl.dxlchain import DxlChain
from dxl.dxlcore import Dxl
port = "/dev/ttyACM0"
timeout = 0.01
rates = [2000000 / (data + 1) for data in range(0, 255)]
rates.append(2250000)
rates.append(2500000)
rates.append(3000000)
chain = DxlChain(port, rate=1000000, timeout=timeout)
for rate in rates:
print("rate", rate)
chain.reopen(port, rate, timeout=timeout)
try:
chain.factory_reset(Dxl.BROADCAST)
except Exception as e:
print(e)
chain.close()
from dxl.dxlchain import DxlChain
import time
dyn_chain = DxlChain("/dev/ttyUSB0", rate=1000000)
print dyn_chain.get_motor_list()
print dyn_chain.get_position()
dyn_chain.close()
