def joint_command_cb(self, joint_desired):
""" Transform subscribed joint command to dynamixel byte information."""
i = 0
while i < 4:
self.q_desired[i] = joint_desired.data[i]
dxl_command = int(
rad2deg(self.q_desired[i]) / self.cfg["DXL_RESOLUTION"] +
self.cfg["DXL_POS_OFFSET"])
if dxl_command > self.cfg["CW_LIMIT"]:
dxl_command = self.cfg["CW_LIMIT"]
elif dxl_command < self.cfg["CCW_LIMIT"]:
dxl_command = self.cfg["CCW_LIMIT"]
self.dxl_goal_position[i] = [
DXL_LOBYTE(DXL_LOWORD(dxl_command)),
DXL_HIBYTE(DXL_LOWORD(dxl_command)),
DXL_LOBYTE(DXL_HIWORD(dxl_command)),
DXL_HIBYTE(DXL_HIWORD(dxl_command)),
]
i += 1
def register_low_endian(self, value, size):
"""Converts a value into a list of bytes in little endian order.
Args:
value (int): the value of the register
size (int): the size of the register
Returns:
(list) List of bytes of len ``size`` with bytes ordered lowest
first.
"""
if size == 1:
return [value]
elif size == 2:
return [DXL_LOBYTE(value), DXL_HIBYTE(value)]
elif size == 4:
lw = DXL_LOWORD(value)
hw = DXL_HIWORD(value)
return [DXL_LOBYTE(lw), DXL_HIBYTE(lw),
DXL_LOBYTE(hw), DXL_HIBYTE(hw)]
else:
logger.error(f'Unexpected register size: {size}')
return None
def byte_array(goal):
return [DXL_LOBYTE(DXL_LOWORD(goal)),
DXL_HIBYTE(DXL_LOWORD(goal)),
DXL_LOBYTE(DXL_HIWORD(goal)),
DXL_HIBYTE(DXL_HIWORD(goal))]
def __init__(self, cfg):
# Dynamixel Setting
rospy.loginfo("Dynamixel Position Controller Created")
self.cfg = cfg
self.portHandler = PortHandler(self.cfg["DEVICENAME"])
self.packetHandler = PacketHandler(self.cfg["PROTOCOL_VERSION"])
self.groupSyncWrite = GroupSyncWrite(
self.portHandler,
self.packetHandler,
self.cfg["ADDR_GOAL_POSITION"],
self.cfg["LEN_GOAL_POSITION"],
)
self.groupBulkReadPosition = GroupBulkRead(self.portHandler,
self.packetHandler)
self.groupBulkReadVelocity = GroupBulkRead(self.portHandler,
self.packetHandler)
self.groupBulkReadCurrent = GroupBulkRead(self.portHandler,
self.packetHandler)
# Port Open
if self.portHandler.openPort():
print("Succeeded to open the port")
else:
print("Failed to open the port")
quit()
# Set port baudrate
if self.portHandler.setBaudRate(self.cfg["BAUDRATE"]):
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
quit()
self.packetHandler.write1ByteTxRx(self.portHandler,
self.cfg["DXL1_ID"],
self.cfg["ADDR_OP_MODE"], 3)
self.packetHandler.write1ByteTxRx(self.portHandler,
self.cfg["DXL2_ID"],
self.cfg["ADDR_OP_MODE"], 3)
self.packetHandler.write1ByteTxRx(self.portHandler,
self.cfg["DXL3_ID"],
self.cfg["ADDR_OP_MODE"], 3)
self.packetHandler.write1ByteTxRx(self.portHandler,
self.cfg["DXL4_ID"],
self.cfg["ADDR_OP_MODE"], 3)
self.groupBulkReadPosition.addParam(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadPosition.addParam(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadPosition.addParam(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadPosition.addParam(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_POSITION"],
self.cfg["LEN_PRESENT_POSITION"],
)
self.groupBulkReadVelocity.addParam(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadVelocity.addParam(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadVelocity.addParam(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadVelocity.addParam(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_VELOCITY"],
self.cfg["LEN_PRESENT_VELOCITY"],
)
self.groupBulkReadCurrent.addParam(
self.cfg["DXL1_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.groupBulkReadCurrent.addParam(
self.cfg["DXL2_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.groupBulkReadCurrent.addParam(
self.cfg["DXL3_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
self.groupBulkReadCurrent.addParam(
self.cfg["DXL4_ID"],
self.cfg["ADDR_PRESENT_CURRENT"],
self.cfg["LEN_PRESENT_CURRENT"],
)
# Enable Dynamixel Torque
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL1_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_ENABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL2_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_ENABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL3_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_ENABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL4_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_ENABLE"],
)
# ROS Publisher
self.joint_states_pub = rospy.Publisher(
"/open_manipulator/joint_states_real", JointState, queue_size=3)
# ROS Subcriber
self.joint_pos_command_sub = rospy.Subscriber(
"/open_manipulator/joint_position/command",
Float64MultiArray,
self.joint_command_cb,
)
self.joint_states = JointState()
self.dxl_present_position = np.zeros(4)
self.dxl_present_velocity = np.zeros(4)
self.dxl_present_current = np.zeros(4)
self.q_desired = np.zeros(4)
self.dxl_goal_position = [
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
]
self.read_dxl()
for i in range(4):
self.dxl_goal_position[i] = [
DXL_LOBYTE(DXL_LOWORD(int(self.dxl_present_position[i]))),
DXL_HIBYTE(DXL_LOWORD(int(self.dxl_present_position[i]))),
DXL_LOBYTE(DXL_HIWORD(int(self.dxl_present_position[i]))),
DXL_HIBYTE(DXL_HIWORD(int(self.dxl_present_position[i]))),
]
self.r = rospy.Rate(100)
try:
while not rospy.is_shutdown():
self.read_dxl()
self.write_dxl()
self.r.sleep()
except KeyboardInterrupt:
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL1_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_DISABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL2_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_DISABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL3_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_DISABLE"],
)
self.packetHandler.write1ByteTxRx(
self.portHandler,
self.cfg["DXL4_ID"],
self.cfg["ADDR_TORQUE_ENABLE"],
self.cfg["TORQUE_DISABLE"],
)
def grip(self, input):
CLOSE_POS = 530
OPEN_POS = 444 # 20 degrees
if input == 1:
# FIRST LOOP
param_goal_position = [
DXL_LOBYTE(DXL_LOWORD(CLOSE_POS)),
DXL_HIBYTE(DXL_LOWORD(CLOSE_POS))
]
# Add Dynamixels goal position value to the Syncwrite parameter storage
self.add_params(self.DXL5_ID, param_goal_position)
# Syncwrite goal position
dxl_comm_result = self.groupSyncWrite.txPacket()
if dxl_comm_result != COMM_SUCCESS:
print("dynamixel_write result error %s" %
self.packetHandler.getTxRxResult(dxl_comm_result))
# Clear syncwrite parameter storage
self.groupSyncWrite.clearParam()
print("Waiting to stop moving...")
while 1:
# both angle and present_position already offset
dxl_present_position = int(
self.read_pos(self.DXL5_ID, 0) / 300 / np.pi * 180 * 1024)
if ((abs(CLOSE_POS - dxl_present_position) <=
self.DXL_MOVING_STATUS_THRESHOLD)):
break
print("Stopped")
# SECOND LOOP
# Add Dynamixels goal position value to the Syncwrite parameter storage
self.add_params(self.DXL5_ID, param_goal_position)
# Syncwrite goal position
dxl_comm_result = self.groupSyncWrite.txPacket()
if dxl_comm_result != COMM_SUCCESS:
print("dynamixel_write result error %s" %
self.packetHandler.getTxRxResult(dxl_comm_result))
# Clear syncwrite parameter storage
self.groupSyncWrite.clearParam()
print("Waiting to stop moving...")
while 1:
# both angle and present_position already offset
dxl_present_position = int(
self.read_pos(self.DXL5_ID, 0) / 300 / np.pi * 180 * 1024)
if ((abs(CLOSE_POS - dxl_present_position) <=
self.DXL_MOVING_STATUS_THRESHOLD)):
break
print("Stopped")
print("Gripper activated!")
if input == 0:
#FIRST LOOP
param_goal_position = [
DXL_LOBYTE(DXL_LOWORD(OPEN_POS)),
DXL_HIBYTE(DXL_LOWORD(OPEN_POS))
]
# Add Dynamixels goal position value to the Syncwrite parameter storage
self.add_params(self.DXL5_ID, param_goal_position)
# Syncwrite goal position
dxl_comm_result = self.groupSyncWrite.txPacket()
if dxl_comm_result != COMM_SUCCESS:
print("dynamixel_write result error %s" %
self.packetHandler.getTxRxResult(dxl_comm_result))
# Clear syncwrite parameter storage
self.groupSyncWrite.clearParam()
print("Waiting to stop moving...")
while 1:
# both angle and present_position already offset
dxl_present_position = int(
self.read_pos(self.DXL5_ID, 0) / 300 / np.pi * 180 * 1024)
if ((abs(OPEN_POS - dxl_present_position) <=
self.DXL_MOVING_STATUS_THRESHOLD)):
break
print("Stopped")
# SECOND LOOP
# Add Dynamixels goal position value to the Syncwrite parameter storage
self.add_params(self.DXL5_ID, param_goal_position)
# Syncwrite goal position
dxl_comm_result = self.groupSyncWrite.txPacket()
if dxl_comm_result != COMM_SUCCESS:
print("dynamixel_write result error %s" %
self.packetHandler.getTxRxResult(dxl_comm_result))
# Clear syncwrite parameter storage
self.groupSyncWrite.clearParam()
print("Waiting to stop moving...")
while 1:
# both angle and present_position already offset
dxl_present_position = int(
self.read_pos(self.DXL5_ID, 0) / 300 / np.pi * 180 * 1024)
if ((abs(OPEN_POS - dxl_present_position) <=
self.DXL_MOVING_STATUS_THRESHOLD)):
break
print("Stopped")
print("Gripper deactivated!")
return 1
def dynamixel_write(self, joint):
print(
"-Start- \ndynamixel_write for angles %s before offset [limit (270,360) and (0,90)]"
% [round(np.rad2deg(joint), 2) for joint in joint])
# function to round off angles to principal angles [0,360] in radians and add offset by 150 degrees
def offset_angle(x):
x += 150 / 180 * np.pi # offset, then reduce to 0,360
if x < 0:
while x < 0:
x += np.pi * 2
return x
if x > 2 * np.pi:
while x > 2 * np.pi:
x -= np.pi * 2
return x
else:
return x
joint = [
int(offset_angle(joint) / 300 / np.pi * 180 * 1024)
for joint in joint
]
print("after offset %s" % [
round(np.rad2deg(joint * 300 * np.pi / 180 / 1024), 2)
for joint in joint
])
angle1 = joint[0]
angle2 = joint[1]
angle3 = joint[2]
angle4 = joint[3]
# FIRST LOOP
# Allocate goal position value into byte array
param_goal_position_1 = [
DXL_LOBYTE(DXL_LOWORD(angle1)),
DXL_HIBYTE(DXL_LOWORD(angle1))
]
param_goal_position_2 = [
DXL_LOBYTE(DXL_LOWORD(angle2)),
DXL_HIBYTE(DXL_LOWORD(angle2))
]
param_goal_position_3 = [
DXL_LOBYTE(DXL_LOWORD(angle3)),
DXL_HIBYTE(DXL_LOWORD(angle3))
]
param_goal_position_4 = [
DXL_LOBYTE(DXL_LOWORD(angle4)),
DXL_HIBYTE(DXL_LOWORD(angle4))
]
# Add Dynamixels goal position value to the Syncwrite parameter storage
self.add_params(self.DXL1_ID, param_goal_position_1)
self.add_params(self.DXL2_ID, param_goal_position_2)
self.add_params(self.DXL3_ID, param_goal_position_3)
self.add_params(self.DXL4_ID, param_goal_position_4)
# Syncwrite goal position
dxl_comm_result = self.groupSyncWrite.txPacket()
if dxl_comm_result != COMM_SUCCESS:
print("dynamixel_write result error %s" %
self.packetHandler.getTxRxResult(dxl_comm_result))
# Clear syncwrite parameter storage
self.groupSyncWrite.clearParam()
print("Waiting to stop moving...")
wait = time.monotonic()
while 1:
# both angle and present_position already offset
dxl1_present_position = int(
self.read_pos(self.DXL1_ID, 0) / 300 / np.pi * 180 * 1024)
dxl2_present_position = int(
self.read_pos(self.DXL2_ID, 0) / 300 / np.pi * 180 * 1024)
dxl3_present_position = int(
self.read_pos(self.DXL3_ID, 0) / 300 / np.pi * 180 * 1024)
dxl4_present_position = int(
self.read_pos(self.DXL4_ID, 0) / 300 / np.pi * 180 * 1024)
if ((abs(angle1 - dxl1_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
(abs(angle2 - dxl2_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
(abs(angle3 - dxl3_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
(abs(angle4 - dxl4_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD)):
break
elif time.monotonic() - wait > 10:
print("arm is Blocked, breaking loop")
break
print("Stopped")
# SECOND LOOP
# Add Dynamixels goal position value to the Syncwrite parameter storage
self.add_params(self.DXL1_ID, param_goal_position_1)
self.add_params(self.DXL2_ID, param_goal_position_2)
self.add_params(self.DXL3_ID, param_goal_position_3)
self.add_params(self.DXL4_ID, param_goal_position_4)
# Syncwrite goal position
dxl_comm_result = self.groupSyncWrite.txPacket()
if dxl_comm_result != COMM_SUCCESS:
print("dynamixel_write result error %s" %
self.packetHandler.getTxRxResult(dxl_comm_result))
# Clear syncwrite parameter storage
self.groupSyncWrite.clearParam()
print("Waiting to stop moving...")
while 1:
# both angle and present_position already offset
dxl1_present_position = int(
self.read_pos(self.DXL1_ID, 0) / 300 / np.pi * 180 * 1024)
dxl2_present_position = int(
self.read_pos(self.DXL2_ID, 0) / 300 / np.pi * 180 * 1024)
dxl3_present_position = int(
self.read_pos(self.DXL3_ID, 0) / 300 / np.pi * 180 * 1024)
dxl4_present_position = int(
self.read_pos(self.DXL4_ID, 0) / 300 / np.pi * 180 * 1024)
if ((abs(angle1 - dxl1_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
(abs(angle2 - dxl2_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
(abs(angle3 - dxl3_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
(abs(angle4 - dxl4_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD)):
break
print("Stopped")
# Update final angles, save as unoffset for easier analysis
print("Verifying final angles...")
self.dynamixel_read()
print("Written the following new angles: %s" %
[round(np.rad2deg(joint), 2) for joint in self.joint])
print("-end-")
return 1
def create1ByteArray(bin_value):
byte_array = [DXL_LOBYTE(DXL_LOWORD(bin_value))]
return byte_array
def dynamixel_write(self, joint):
print("-Start- \ndynamixel_write for angles %s before offset [limit (270,360) and (0,90)]" % [round(np.rad2deg(joint),2) for joint in joint])
# function to round off angles to principal angles [0,360] in radians and add offset by 150 degrees
def offset_angle(x):
x += 150/180*np.pi # offset, then reduce to 0,360
if x < 0:
while x < 0:
x += np.pi*2
return x
if x > 2*np.pi:
while x > 2*np.pi:
x -= np.pi*2
return x
else:
return x
joint = [int(offset_angle(joint)/300/np.pi*180*1024) for joint in joint]
print("after offset %s" % [round(np.rad2deg(joint*300*np.pi/180/1024),2) for joint in joint])
# motor 2 and 3 flipped over
angle1 = joint[0]
angle2 = 1024-joint[1]
angle3 = 1024-joint[2]
angle4 = joint[3]
current = [offset_angle(x)/np.deg2rad(300)*1024 for x in self.joint]
diff1 = abs(current[0] - angle1)
diff2 = abs(1024-current[1] - angle2)
diff3 = abs(1024-current[2] - angle3)
diff4 = abs(current[3] - angle4)
reduct = 1
print("Angle difference is %s and reduction value is %s" %([diff1,diff2,diff3,diff4],reduct))
if diff1 > 100:
print("apply speed limit 1")
diff2 = diff2 * reduct * 100/diff1
diff3 = diff3 * reduct * 100/diff1
diff4 = diff4 * reduct * 100/diff1
diff1 = diff1 * reduct * 100/diff1
if diff2 > 100:
print("apply speed limit 2")
diff1 = diff1 * reduct * 100/diff2
diff3 = diff3 * reduct * 100/diff2
diff4 = diff4 * reduct * 100/diff2
diff2 = diff2 * reduct * 100/diff2
if diff3 > 100:
print("apply speed limit 3")
diff1 = diff1 * reduct * 100/diff3
diff2 = diff2 * reduct * 100/diff3
diff4 = diff4 * reduct * 100/diff3
diff3 = diff3 * reduct * 100/diff3
if diff4 > 100:
print("apply speed limit 4")
diff1 = diff1 * reduct * 100/diff4
diff2 = diff2 * reduct * 100/diff4
diff3 = diff3 * reduct * 100/diff4
diff4 = diff4 * reduct * 100/diff4
diff1 = int(diff1)
diff2 = int(diff2)
diff3 = int(diff3)
diff4 = int(diff4)
if (diff1 == 0):
diff1 = 1
if (diff2 == 0):
diff2 = 1
if (diff3 == 0):
diff3 = 1
if (diff4 == 0):
diff4 = 1
self.set_joint_speed(self.DXL1_ID,(diff1))
self.set_joint_speed(self.DXL2_ID,(diff2))
self.set_joint_speed(self.DXL3_ID,(diff3))
self.set_joint_speed(self.DXL4_ID,(diff4))
# FIRST LOOP
# Allocate goal position value into byte array
param_goal_position_1 = [DXL_LOBYTE(DXL_LOWORD(angle1)), DXL_HIBYTE(DXL_LOWORD(angle1))]
param_goal_position_2 = [DXL_LOBYTE(DXL_LOWORD(angle2)), DXL_HIBYTE(DXL_LOWORD(angle2))]
param_goal_position_3 = [DXL_LOBYTE(DXL_LOWORD(angle3)), DXL_HIBYTE(DXL_LOWORD(angle3))]
param_goal_position_4 = [DXL_LOBYTE(DXL_LOWORD(angle4)), DXL_HIBYTE(DXL_LOWORD(angle4))]
# Add Dynamixels goal position value to the Syncwrite parameter storage
self.add_params(self.DXL1_ID, param_goal_position_1)
self.add_params(self.DXL2_ID, param_goal_position_2)
self.add_params(self.DXL3_ID, param_goal_position_3)
self.add_params(self.DXL4_ID, param_goal_position_4)
# Syncwrite goal position
dxl_comm_result = self.groupSyncWrite.txPacket()
if dxl_comm_result != COMM_SUCCESS:
print("dynamixel_write result error %s" % self.packetHandler.getTxRxResult(dxl_comm_result))
# Clear syncwrite parameter storage
self.groupSyncWrite.clearParam()
print("Waiting to stop moving...")
wait = time.monotonic()
while 1:
# both angle and present_position already offset
# dxl1_present_position = int(self.read_pos(self.DXL1_ID,0)/300/np.pi*180*1024)
# dxl2_present_position = int(self.read_pos(self.DXL2_ID,0)/300/np.pi*180*1024)
# dxl3_present_position = int(self.read_pos(self.DXL3_ID,0)/300/np.pi*180*1024)
# dxl4_present_position = int(self.read_pos(self.DXL4_ID,0)/300/np.pi*180*1024)
# if ((abs(angle1 - dxl1_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
# (abs(angle2 - dxl2_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
# (abs(angle3 - dxl3_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
# (abs(angle4 - dxl4_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD)):
# break
dxl1_is_move = self.is_move(self.DXL1_ID,0)
dxl2_is_move = self.is_move(self.DXL2_ID,0)
dxl3_is_move = self.is_move(self.DXL3_ID,0)
dxl4_is_move = self.is_move(self.DXL4_ID,0)
if (not(dxl1_is_move) and not(dxl2_is_move) and not(dxl3_is_move) and not(dxl4_is_move)):
break
elif time.monotonic() - wait > 10:
print("arm is Blocked, breaking loop")
break
print("Stopped")
# SECOND LOOP
# Add Dynamixels goal position value to the Syncwrite parameter storage
self.add_params(self.DXL1_ID, param_goal_position_1)
self.add_params(self.DXL2_ID, param_goal_position_2)
self.add_params(self.DXL3_ID, param_goal_position_3)
self.add_params(self.DXL4_ID, param_goal_position_4)
# Syncwrite goal position
dxl_comm_result = self.groupSyncWrite.txPacket()
if dxl_comm_result != COMM_SUCCESS:
print("dynamixel_write result error %s" % self.packetHandler.getTxRxResult(dxl_comm_result))
# Clear syncwrite parameter storage
self.groupSyncWrite.clearParam()
print("Waiting to stop moving...")
wait = time.monotonic()
while 1:
# both angle and present_position already offset
# dxl1_present_position = int(self.read_pos(self.DXL1_ID,0)/300/np.pi*180*1024)
# dxl2_present_position = int(self.read_pos(self.DXL2_ID,0)/300/np.pi*180*1024)
# dxl3_present_position = int(self.read_pos(self.DXL3_ID,0)/300/np.pi*180*1024)
# dxl4_present_position = int(self.read_pos(self.DXL4_ID,0)/300/np.pi*180*1024)
# if ((abs(angle1 - dxl1_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
# (abs(angle2 - dxl2_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
# (abs(angle3 - dxl3_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD) and \
# (abs(angle4 - dxl4_present_position) <= self.DXL_MOVING_STATUS_THRESHOLD)):
# break
dxl1_is_move = self.is_move(self.DXL1_ID,0)
dxl2_is_move = self.is_move(self.DXL2_ID,0)
dxl3_is_move = self.is_move(self.DXL3_ID,0)
dxl4_is_move = self.is_move(self.DXL4_ID,0)
if (not(dxl1_is_move) and not(dxl2_is_move) and not(dxl3_is_move) and not(dxl4_is_move)):
break
elif time.monotonic() - wait > 10:
print("arm is Blocked, breaking loop")
break
print("Stopped")
# Update final angles, save as unoffset for easier analysis
print("Verifying final angles...")
self.dynamixel_read()
print("Written the following new angles: %s" % [round(np.rad2deg(joint),2) for joint in self.joint])
print("-end-")
return 1