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