def set_velocity(vel):
with MotionManager(ids, dt=.005, options=dopts) as mm:
# vel -128 ~ 128
# Set velocity as vel*0.229 rev/min
mm.device.set_torque_enable(ids, [0] * 1)
mm.device._write_data(ids, XSERIES.GOAL_VELOCITY, [vel] * 1, 4)
mm.device.set_torque_enable(ids, [1] * 1)
def test_set_command_position():
with MotionManager(ids, dt=.005, options=dopts) as mm:
# checking that the set_all/get_all position commands work
position_list = [[0], [-0.6234], [1.8903], [0.6234], [-1.8903],
[0.6234], [-1.8903], [-0.6234], [1.8903], [2.1056]]
for pl in position_list:
mm.set_goal_position(ids, to_player_angle_offset(pl, offset))
# mm.wait(0.8)
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
counter_time = 0
k = 0
# while np.max(abs(np.array(qcurr) - np.array(pl))) >= 0.005:
while k < 500:
start = time.time()
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
end_time = time.time() - start
counter_time += end_time
k = k + 1
# print(qcurr)
print(qcurr)
print(counter_time)
print(counter_time / 500)
def test_velocity():
with MotionManager(ids, dt=.005, options=dopts) as mm:
mm.device.set_torque_enable(ids, [0] * 2)
mm.device._write_data(ids, XSERIES.OPERATING_MODE, [1] * 2,
1) # set motors to be in velocity mode
print(mm.device._read_data(ids, XSERIES.OPERATING_MODE,
1)) # set motors to be in velocity mode
mm.device.set_torque_enable(ids, [1] * 2)
mm.set_goal_velocity(ids, [pi / 2] * 2) # test positive velocity
sleep(4)
allclose(mm.get_present_velocity(ids), [pi / 2] * 2, .1)
mm.set_goal_velocity(ids, [0] * 2)
mm.set_goal_velocity(ids, [-pi / 2] * 2) # test negative velocity
sleep(4)
allclose(mm.get_present_velocity(ids), [-pi / 2] * 2, .1)
mm.set_goal_velocity(ids, [0] * 2)
mm.set_goal_velocity(ids,
[pi / 3, pi / 2]) # test different velocities
sleep(4)
allclose(mm.get_present_velocity(ids), [pi / 3, pi / 2], .1)
mm.set_goal_velocity(ids, [0] * 2)
sleep(1)
allclose(mm.get_present_velocity(ids), [0] * 2,
.1) # test zero velocity
mm.device.set_torque_enable(ids, [0] * 2)
mm.device._write_data(ids, XSERIES.OPERATING_MODE, [3] * 2,
1) # set motors to be in position mode
print(
mm.device._read_data(ids, XSERIES.OPERATING_MODE,
1)) # check that motors are in position mode
def set_vel_limit(maxvel=128):
with MotionManager(ids, dt=.005, options=dopts) as mm:
# Set trapezoidal profile parameters
# 0 means infinite velocity and acceleration
mm.device.set_torque_enable(ids, [0] * 2)
mm.device._write_data(ids, XSERIES.VELOCITY_LIMIT, [maxvel] * 2, 4)
mm.device.set_torque_enable(ids, [1] * 2)
def set_pid_gain(Pgain=800, Igain=0, Dgain=0):
with MotionManager(ids, dt=.005, options=dopts) as mm:
# Set position PID gain
mm.device.set_torque_enable(ids, [0] * 1)
mm.device._write_data(ids, XSERIES.POSITION_P_GAIN, [Pgain] * 1, 2)
mm.device._write_data(ids, XSERIES.POSITION_I_GAIN, [Igain] * 1, 2)
mm.device._write_data(ids, XSERIES.POSITION_D_GAIN, [Dgain] * 1, 2)
mm.device.set_torque_enable(ids, [1] * 1)
def set_traj_params(maxvel=0, maxaccel=0):
with MotionManager(ids, dt=.005, options=dopts) as mm:
# Set trapezoidal profile parameters
# 0 means infinite velocity and acceleration
mm.device.set_torque_enable(ids, [0] * 1)
mm.device._write_data(ids, XSERIES.PROFILE_VELOCITY, [maxvel] * 1, 4)
mm.device._write_data(ids, XSERIES.PROFILE_ACCELERATION,
[maxaccel] * 1, 4)
mm.device.set_torque_enable(ids, [1] * 1)
def set_ff_gain(Gain1=0, Gain2=0):
with MotionManager(ids, dt=.005, options=dopts) as mm:
# Set position Feedforward gain
mm.device.set_torque_enable(ids, [0] * 1)
mm.device._write_data(ids, XSERIES.FEEDFORWARD_1ST_GAIN, [Gain1] * 1,
2)
mm.device._write_data(ids, XSERIES.FEEDFORWARD_2ND_GAIN, [Gain2] * 1,
2)
mm.device.set_torque_enable(ids, [1] * 1)
def test_set_all_command_position():
with MotionManager(ids, dt=.005, options=dopts) as mm:
# checking that the set_all/get_all position commands work
position_list = [[pi, pi], [2, 3], [pi / 2, pi / 2]]
for pl in position_list:
mm.set_all_goal_position(pl)
mm.wait(2.5)
print(mm.get_all_present_position())
get_curr_pos = mm.get_present_position(ids)
get_all_curr_pos = mm.get_all_present_position()
assert (np.allclose(get_curr_pos, get_all_curr_pos, atol=.05))
assert (np.allclose(mm.get_all_present_position(), pl, atol=.05))
def set_single_cmd_position(pos_list, motor_id):
# eg. motor_id = [1], pos_list = [[0],[pi/2]]
with MotionManager(motor_id, dt=.005, options=dopts) as mm:
# move one single motor with id
for pl in pos_list:
mm.set_goal_position(motor_id, to_player_angle_offset(pl, offset))
#mm.wait(2.0)
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
# Keep reading encoder until motor reach goal position
while np.max(abs(np.array(qcurr) - np.array(pl))) >= 0.005:
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
print(qcurr)
def set_all_cmd_position(pos_list):
with MotionManager(ids, dt=.005, options=dopts) as mm:
# move all motors together
# pos_list = [[pi,pi], [0,0], [pi/2,pi/2]]
for pl in pos_list:
mm.set_goal_position(ids, to_player_angle_offset(pl, offset))
# mm.wait(0.3)
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
# Keep reading encoder until motor reach goal position
k = 0
while np.max(abs(np.array(qcurr[1:]) -
np.array(pl[1:]))) >= 0.05 and k < 50:
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
k = k + 1
print(qcurr)
def turn_on_motors(turn_on_time):
motor_id = [1, 2]
dt = .005
if platform.system() == 'Windows':
ports = ['COM15']
else:
ports = ['/dev/ttyUSB0']
dxl_str = "MX28"
dxl_opts = DxlOptions([motor_id],
motor_types=[dxl_str],
ports=ports,
baudrate=3000000,
protocol_version=2)
with MotionManager(motor_id, dt=dt, options=dxl_opts) as mm:
mm.torque_on([1])
mm.torque_on([2])
mm.wait(turn_on_time)
return True
def read_pos(shared_val, new_val, angles, duration=5):
lock = Lock()
lock1 = Lock()
motor_id = [1, 2]
dt = .005
if platform.system() == 'Windows':
ports = ['COM15']
else:
ports = ['/dev/ttyUSB0']
dxl_str = "MX28"
dxl_opts = DxlOptions([motor_id],
motor_types=[dxl_str],
ports=ports,
baudrate=3000000,
protocol_version=2)
start_time = time.time()
cur_time = start_time
with MotionManager(motor_id, dt=dt, options=dxl_opts) as mm:
mm.wait(.1)
di = mm.device
di._write_data(motor_id, 84, [1500] * len(motor_id), 2)
di._write_data(motor_id, 82, [200] * len(motor_id), 2)
for pos in angles:
mm.set_goal_position([2], [pos[0]])
mm.set_goal_position([1], [pos[1]])
line = []
cur_time = time.time()
cur_pos = mm.get_all_present_position()
line.append(cur_time)
line = line + cur_pos
with lock:
for i in range(3):
shared_val[i] = line[i]
shared_val[3] = pos[0]
shared_val[4] = pos[1]
new_val.value = True
mm.wait(dt)
cur_time = time.time()
print(cur_time - start_time)
while cur_time - start_time < duration - 2:
line = []
cur_time = time.time()
cur_pos = mm.get_all_present_position()
line.append(cur_time)
line = line + cur_pos
with lock:
for i in range(3):
shared_val[i] = line[i]
new_val.value = True
mm.wait(dt)
cur_time = time.time()
print(cur_time - start_time)
mm.torque_off([1])
mm.torque_off([2])
while cur_time - start_time < duration:
line = []
cur_time = time.time()
cur_pos = mm.get_all_present_position()
line.append(cur_time)
line = line + cur_pos
with lock:
for i in range(3):
shared_val[i] = line[i]
shared_val[3] = pos[0]
shared_val[4] = pos[1]
new_val.value = True
mm.wait(.005)
return True
motor_id = [1,2]
dt = .005
if platform.system() == 'Windows':
ports = ['COM15']
else:
ports = ['/dev/ttyUSB0']
dxl_str = "MX28"
dxl_opts = DxlOptions([motor_id],
motor_types=[dxl_str],
ports=ports,
baudrate=3000000,
protocol_version=2
)
with MotionManager(motor_id, dt=dt, options=dxl_opts) as mm:
di = mm.device
print("HEREEARTSTARYTDJFUHKLHkjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjj")
print(di._read_data([1], 44, 4))
#di._write_data([1],84,[1500],4)
#print(di._read_data([1], 84, 4))
# mm.torque_on([1])
# mm.torque_on([2])
# for pos in angles:
# mm.set_goal_position([2], [pos[0]])
# mm.set_goal_position([1], [pos[1]])
# mm.wait(0.01)