Fix get_joint_state and add common api(debug)

Author: vimior

xarm/core/wrapper/uxbus_cmd.py

@@ -358,6 +358,16 @@ def move_line(self, mvpose, mvvelo, mvacc, mvtime, only_check_type=0):
  byte_data = bytes([only_check_type])
  return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_LINE, txdata, 9, byte_data, 3, timeout=10)
 
+ def move_line_common(self, mvpose, mvvelo, mvacc, mvtime, radius=-1, coord=0, is_axis_angle=False, only_check_type=0):
+ """
+ 通用指令，固件1.10.0开始支持 
+ """
+ txdata = [mvpose[i] for i in range(6)]
+ _radius = -1 if radius is None else radius
+ txdata += [mvvelo, mvacc, mvtime, _radius]
+ byte_data = bytes([coord, int(is_axis_angle), only_check_type])
+ return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_LINE, txdata, 10, byte_data, 3, timeout=10)
+
  def move_line_aa(self, mvpose, mvvelo, mvacc, mvtime, mvcoord, relative, only_check_type=0):
  float_data = [mvpose[i] for i in range(6)]
  float_data += [mvvelo, mvacc, mvtime]
@@ -496,6 +506,21 @@ def move_circle(self, pose1, pose2, mvvelo, mvacc, mvtime, percent, only_check_t
  byte_data = bytes([only_check_type])
  return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_CIRCLE, txdata, 16, byte_data, 3, timeout=10)
 
+ def move_circle_common(self, pose1, pose2, mvvelo, mvacc, mvtime, percent, coord=0, is_axis_angle=False, only_check_type=0):
+ """
+ 通用指令，固件1.10.0开始支持 
+ """
+ txdata = [0] * 16
+ for i in range(6):
+ txdata[i] = pose1[i]
+ txdata[6 + i] = pose2[i]
+ txdata[12] = mvvelo
+ txdata[13] = mvacc
+ txdata[14] = mvtime
+ txdata[15] = percent
+ byte_data = bytes([coord, int(is_axis_angle), only_check_type])
+ return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_CIRCLE, txdata, 16, byte_data, 3, timeout=10)
+
  def set_tcp_jerk(self, jerk):
  txdata = [jerk]
  return self.set_nfp32(XCONF.UxbusReg.SET_TCP_JERK, txdata, 1)
@@ -540,8 +565,8 @@ def save_conf(self):
  def get_joint_pos(self):
  return self.get_nfp32(XCONF.UxbusReg.GET_JOINT_POS, 7)
 
- def get_joint_states(self):
- return self.get_nfp32_with_datas(XCONF.UxbusReg.GET_JOINT_POS, [3], 1, 21)
+ def get_joint_states(self, num=3):
+ return self.get_nfp32_with_datas(XCONF.UxbusReg.GET_JOINT_POS, [num], 1, 7 * num)
 
  def get_tcp_pose(self):
  return self.get_nfp32(XCONF.UxbusReg.GET_TCP_POSE, 6)

xarm/version.py

@@ -1 +1 @@
-__version__ = '1.9.1'
+__version__ = '1.9.10'

xarm/wrapper/xarm_api.py

@@ -807,7 +807,7 @@ def set_position(self, x=None, y=None, z=None, roll=None, pitch=None, yaw=None,
 
  def set_tool_position(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0,
  speed=None, mvacc=None, mvtime=None, is_radian=None,
- wait=False, timeout=None, **kwargs):
+ wait=False, timeout=None, radius=None, **kwargs):
  """
  Movement relative to the tool coordinate system
  Note:
@@ -828,6 +828,13 @@ def set_tool_position(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0,
  :param is_radian: the roll/pitch/yaw in radians or not, default is self.default_is_radian
  :param wait: whether to wait for the arm to complete, default is False
  :param timeout: maximum waiting time(unit: second), default is None(no timeout), only valid if wait is True
+ :param radius: move radius, if radius is None or radius less than 0, will MoveToolLine, else MoveToolArcLine
+ only available if firmware_version >= 1.10.110
+ MoveToolLine: Linear motion
+ ex: code = arm.set_tool_position(..., radius=None)
+ MoveToolArcLine: Linear arc motion with interpolation
+ ex: code = arm.set_tool_position(..., radius=0)
+ Note: Need to set radius>=0
  :param kwargs: reserved
  :return: code
  code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
@@ -836,9 +843,9 @@ def set_tool_position(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0,
  """
  return self._arm.set_tool_position(x=x, y=y, z=z, roll=roll, pitch=pitch, yaw=yaw,
  speed=speed, mvacc=mvacc, mvtime=mvtime,
- is_radian=is_radian, wait=wait, timeout=timeout, **kwargs)
+ is_radian=is_radian, wait=wait, timeout=timeout, radius=radius, **kwargs)
 
- def get_servo_angle(self, servo_id=None, is_radian=None):
+ def get_servo_angle(self, servo_id=None, is_radian=None, is_real=False):
  """
  Get the servo angle
  Note:
@@ -853,7 +860,7 @@ def get_servo_angle(self, servo_id=None, is_radian=None):
  :return: tuple((code, angle list if servo_id is None or 8 else angle)), only when code is 0, the returned result is correct.
  code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
  """
- return self._arm.get_servo_angle(servo_id=servo_id, is_radian=is_radian)
+ return self._arm.get_servo_angle(servo_id=servo_id, is_radian=is_radian, is_real=is_real)
 
  def set_servo_angle(self, servo_id=None, angle=None, speed=None, mvacc=None, mvtime=None,
  relative=False, is_radian=None, wait=False, timeout=None, radius=None, **kwargs):
@@ -936,7 +943,8 @@ def set_servo_cartesian(self, mvpose, speed=None, mvacc=None, mvtime=0, is_radia
  return self._arm.set_servo_cartesian(mvpose, speed=speed, mvacc=mvacc, mvtime=mvtime, is_radian=is_radian,
  is_tool_coord=is_tool_coord, **kwargs)
 
- def move_circle(self, pose1, pose2, percent, speed=None, mvacc=None, mvtime=None, is_radian=None, wait=False, timeout=None, **kwargs):
+ def move_circle(self, pose1, pose2, percent, speed=None, mvacc=None, mvtime=None, is_radian=None,
+ wait=False, timeout=None, is_tool_coord=False, is_axis_angle=False, **kwargs):
  """
  The motion calculates the trajectory of the space circle according to the three-point coordinates.
  The three-point coordinates are (current starting point, pose1, pose2).
@@ -950,13 +958,17 @@ def move_circle(self, pose1, pose2, percent, speed=None, mvacc=None, mvtime=None
  :param is_radian: roll/pitch/yaw value is radians or not, default is self.default_is_radian
  :param wait: whether to wait for the arm to complete, default is False
  :param timeout: maximum waiting time(unit: second), default is None(no timeout), only valid if wait is True
+ :param is_tool_coord: is tool coord or not, default is False, only available if firmware_version >= 1.10.110
+ :param is_axis_angle: is axis angle or not, default is False, only available if firmware_version >= 1.10.110
  :param kwargs: reserved
  :return: code
  code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
  code < 0: the last_used_tcp_speed/last_used_tcp_acc will not be modified
  code >= 0: the last_used_tcp_speed/last_used_tcp_acc will be modified
  """
- return self._arm.move_circle(pose1, pose2, percent, speed=speed, mvacc=mvacc, mvtime=mvtime, is_radian=is_radian, wait=wait, timeout=timeout, **kwargs)
+ return self._arm.move_circle(pose1, pose2, percent, speed=speed, mvacc=mvacc, mvtime=mvtime,
+ is_radian=is_radian, wait=wait, timeout=timeout,
+ is_tool_coord=is_tool_coord, is_axis_angle=is_axis_angle, **kwargs)
 
  def move_gohome(self, speed=None, mvacc=None, mvtime=None, is_radian=None, wait=False, timeout=None, **kwargs):
  """
@@ -2395,7 +2407,7 @@ def config_cgpio_reset_when_stop(self, on_off):
  return self._arm.config_io_reset_when_stop(0, on_off)
 
  def set_position_aa(self, axis_angle_pose, speed=None, mvacc=None, mvtime=None,
- is_radian=None, is_tool_coord=False, relative=False, wait=False, timeout=None, **kwargs):
+ is_radian=None, is_tool_coord=False, relative=False, wait=False, timeout=None, radius=None, **kwargs):
  """
  Set the pose represented by the axis angle pose
  
@@ -2408,12 +2420,19 @@ def set_position_aa(self, axis_angle_pose, speed=None, mvacc=None, mvtime=None,
  :param relative: relative move or not
  :param wait: whether to wait for the arm to complete, default is False
  :param timeout: maximum waiting time(unit: second), default is None(no timeout), only valid if wait is True
+ :param radius: move radius, if radius is None or radius less than 0, will MoveLineAA, else MoveArcLineAA
+ only available if firmware_version >= 1.10.110
+ MoveLineAA: Linear motion
+ ex: code = arm.set_position_aa(..., radius=None)
+ MoveArcLineAA: Linear arc motion with interpolation
+ ex: code = arm.set_position_aa(..., radius=0)
+ Note: Need to set radius>=0
  :return: code
  code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
  """
  return self._arm.set_position_aa(axis_angle_pose, speed=speed, mvacc=mvacc, mvtime=mvtime,
  is_radian=is_radian, is_tool_coord=is_tool_coord, relative=relative,
- wait=wait, timeout=timeout, **kwargs)
+ wait=wait, timeout=timeout, radius=radius, **kwargs)
 
  def set_servo_cartesian_aa(self, axis_angle_pose, speed=None, mvacc=None, is_radian=None, is_tool_coord=False, relative=False, **kwargs):
  """
@@ -3501,7 +3520,7 @@ def set_allow_approx_motion(self, on_off):
  """
  return self._arm.set_allow_approx_motion(on_off)
 
- def get_joint_states(self, is_radian=None):
+ def get_joint_states(self, is_radian=None, num=3):
  """
  Get the joint states
  Note:
@@ -3514,7 +3533,7 @@ def get_joint_states(self, is_radian=None):
  velocity: the velocities of joints, like [velo-1, ..., velo-7]
  effort: the efforts of joints, like [effort-1, ..., effort-7]
  """
- return self._arm.get_joint_states(is_radian=is_radian)
+ return self._arm.get_joint_states(is_radian=is_radian, num=num)
 
  def iden_joint_friction(self, sn=None):
  """

xarm/x3/base.py

@@ -454,6 +454,10 @@ def is_simulation_robot(self):
  def check_is_simulation_robot(self):
  return self._check_simulation_mode and self.is_simulation_robot
  # return self._check_simulation_mode and self.mode != 4
+ 
+ @property
+ def is_lite6(self):
+ return self.axis == 6 and self.device_type == 9
 
  @property
  def version(self):
@@ -762,7 +766,7 @@ def connect(self, port=None, baudrate=None, timeout=None, axis=None, arm_type=No
  return
  if axis in [5, 6, 7]:
  self._arm_axis = axis
- if arm_type in [3, 5, 6, 7]:
+ if arm_type in [3, 5, 6, 7, 8, 9, 11]:
  self._arm_type = arm_type
  self._is_ready = True
  self._port = port if port is not None else self._port
@@ -1950,9 +1954,12 @@ def get_position(self, is_radian=None):
  i in range(len(self._position))]
 
  @xarm_is_connected(_type='get')
- def get_servo_angle(self, servo_id=None, is_radian=None):
+ def get_servo_angle(self, servo_id=None, is_radian=None, is_real=False):
  is_radian = self._default_is_radian if is_radian is None else is_radian
- ret = self.arm_cmd.get_joint_pos()
+ if is_real and self.version_is_ge(1, 9, 110):
+ ret = self.arm_cmd.get_joint_states(num=1)
+ else:
+ ret = self.arm_cmd.get_joint_pos()
  ret[0] = self._check_code(ret[0])
  if ret[0] == 0 and len(ret) > 7:
  self._angles = [filter_invaild_number(ret[i], 6, default=self._angles[i-1]) for i in range(1, 8)]
@@ -1964,19 +1971,25 @@ def get_servo_angle(self, servo_id=None, is_radian=None):
  '{:.6f}'.format(self._angles[servo_id - 1] if is_radian else math.degrees(self._angles[servo_id - 1])))
 
  @xarm_is_connected(_type='get')
- def get_joint_states(self, is_radian=None):
+ def get_joint_states(self, is_radian=None, num=3):
  is_radian = self._default_is_radian if is_radian is None else is_radian
- ret = self.arm_cmd.get_joint_states()
+ ret = self.arm_cmd.get_joint_states(num=num)
  ret[0] = self._check_code(ret[0])
  positon = ret[1:8]
- velocity = ret[8:15]
- effort = ret[15:22]
+ result = [positon]
+ if num >= 2:
+ velocity = ret[8:15]
+ result.append(velocity)
+ if num >= 3:
+ effort = ret[15:22]
+ result.append(effort)
  if ret[0] == 0:
  if not is_radian:
  for i in range(7):
  positon[i] = math.degrees(positon[i])
- velocity[i] = math.degrees(velocity[i])
- return ret[0], [positon, velocity, effort]
+ if num >= 2:
+ velocity[i] = math.degrees(velocity[i])
+ return ret[0], result
 
  @xarm_is_connected(_type='get')
  def get_position_aa(self, is_radian=None):