import magician
import math
import struct
from magician.message import Message
import struct
import math
import logging
from enum import IntEnum
from threading import RLock
from typing import NamedTuple, Set, Optional
import time
import serial
from serial.tools import list_ports
from collections import deque
# CONSTANTS --------------------------------------------------------------------
# ⚠ Don't change these values unless you are SURE know EXACTLY what you are doing.
MAX_QUEUE_LEN = 32
STEP_PER_CIRCLE = 360.0 / 1.8 * 10.0 * 16.0
MM_PER_CIRCLE = 3.1415926535898 * 36.0
# -----------------------------------------------------------------------------
[docs]
class BotException(Exception):
"""
Exception raised for errors related to the bot operations.
Attributes
----------
message : str, optional
A description of the error. The default is "Bot Exception".
Examples
--------
>>> raise BotException("Device not found.")
>>> raise BotException()
"""
def __init__(self, message="Bot Exception"):
self.message = message
super().__init__(self.message)
[docs]
class BotBoundsException(Exception):
"""
Exception raised when bot movement is set to go out of bounds.
Attributes
----------
message : str, optional
A description of the error. The default is "Bot Bounds Exception".
Examples
--------
>>> raise BotBoundsException("j1 angle out of bonds, must be in range [-125º,125º]")
>>> raise BotBoundsException()
"""
def __init__(self, message="Bot Bounds Exception"):
self.message = message
super().__init__(self.message)
[docs]
class MOVE_TYPE(IntEnum):
"""
The MOVE_TYPE enum defines the types of movement available for the Robotic Arm.
Parameters
----------
MOVJ : int
Joint movement. From point A to point B, each joint will run from initial angle
to its target angle, regardless of the trajectory.
MOVL : int
Rectilinear movement. The joints will perform a straight line trajectory from
point A to point B.
JUMP : int
From point A to point B the end effector will lift upwards by amount of Height (in mm)
and move horizontally to a point that is above B by Height and then move down to Point B.
Examples
--------
>>> linear_movement = MOVE_TYPE.MOVL
>>> move_to_xyz(linear_movement,x=200,y=0,z=0)
"""
MOVJ = 0
MOVL = 1
JUMP = 2
[docs]
class MODE_PTP(IntEnum):
"""
Enumeration for types of Point to Point Movement message payload for the Dobot Magician.
The MODE_PTP enum defines the Point to Point number code definitions.
Parameters
----------
JUMP_XYZ : int
JUMP mode, (x, y, z, r) is the target point in Cartesian coordinate system.
MOVJ_XYZ : int
MOVJ mode, (x, y, z, r) is the target point in Cartesian coordinate system.
MOVL_XYZ : int
MOVL mode, (x, y, z, r) is the target point in Cartesian coordinate system.
JUMP_ANGLE : int
JUMP mode, (x, y, z, r) is the target point in Joint coordinate system.
MOVJ_ANGLE : int
MOVJ mode, (x, y, z, r) is the target point in Joint coordinate system.
MOVL_ANGLE : int
MOVL mode, (x, y, z, r) is the target point in Joint coordinate system.
MOVJ_INC : int
MOVJ mode, (x, y, z, r) is the angle increment in Joint coordinate system.
MOVL_INC : int
MOVL mode, (x, y, z, r) is the Cartesian coordinate increment in Joint coordinate system.
MOVJ_XYZ_INC : int
MOVJ mode, (x, y, z, r) is the Cartesian coordinate increment in Cartesian coordinate system.
JUMP_MOVL_XYZ:
JUMP mode, (x, y, z, r) is the Cartesian coordinate increment in Cartesian coordinate system.
Notes
-----
Do the chosen names make sense? No ... But this is what's given by the manufacturer in the
API description. That's also why this enum is not meant to be used by people using this lib,
only for those maintaining it.
"""
JUMP_XYZ = 0x00
MOVJ_XYZ = 0x01
MOVL_XYZ = 0x02
JUMP_ANGLE = 0x03
MOVJ_ANGLE = 0x04
MOVL_ANGLE = 0x05
MOVJ_INC = 0x06
MOVL_INC = 0x07
MOVJ_XYZ_INC = 0x08
JUMP_MOVL_XYZ = 0x09
[docs]
class Position(NamedTuple):
"""
Defines a position in the Cartesian Plane.
Parameters
----------
x : float
x-coordinate in the Cartesian Plane.
y : float
y-coordinate in the Cartesian Plane.
z : float
z-coordinate in the Cartesian Plane.
r : float
Effector orientation angle.
"""
x: float
y: float
z: float
r: float
[docs]
class Joints(NamedTuple):
"""
Defines a position in the Joints Plane.
Parameters
----------
j1 : float
j1 joint angle in the Joints Plane.
j2 : float
j2 joint angle in the Joints Plane.
j3 : float
j3 joint angle in the Joints Plane.
j4 : float
j4 joint (effector) angle in the Joints Plane.
"""
j1: float
j2: float
j3: float
j4: float
[docs]
def in_radians(self) -> "Joints":
"""Returns joints angles in radians."""
return Joints(math.radians(self.j1), math.radians(self.j2), math.radians(self.j3), math.radians(self.j4))
[docs]
class Pose(NamedTuple):
"""
NamedTupple that defines position based on the Position and Joints Objects.
Parameters
----------
position : Position
joints : Joints
See Also
--------
bot.Position
bot.Joints
"""
position: Position
joints: Joints
[docs]
class Alarm(IntEnum):
"""
Point number codes that represent the alarms used by Dobot Magician.
Parameters
----------
COMMON_RESETTING : int, default = 0x00
The alarm will be triggered after resetting system.
COMMON_UNDEFINED_INSTRUCTION : int, default = 0x01
Receive undefined instruction.
COMMON_FILE_SYSTEM : int, default = 0x02
File system error.
COMMON_MCU_FPGA_COMM : int, default = 0x03
There is a failed communication between MCU and FPGA when system is
initializing
COMMON_ANGLE_SENSOR : int, default = 0x04
Get an error value of angle sensor
PLAN_INV_SINGULARITY : int, default = 0x10
The target point (beginning point or ending point) is in abnormal position
in Cartesian coordinate system.
PLAN_INV_CALC : int, default = 0x11
The target point is out of the workplace in all motion mode.
PLAN_INV_LIMIT : int, default = 0x12
The inverse resolve of target point is out of the limitation.
PLAN_PUSH_DATA_REPEAT : int, default = 0x13
There are some repetitive points in ARC or JUMP_MOVEL mode.
PLAN_ARC_INPUT_PARAM : int, default = 0x14
Missing manufacturer description.
PLAN_JUMP_PARAM : int, default = 0x15
Set a wrong parameter in JUMP mode.
PLAN_LINE_HAND : int, default = 0x16
Missing manufacturer description.
PLAN_LINE_OUT_SPACE : int, default = 0x17
Missing manufacturer description.
PLAN_ARC_OUT_SPACE : int, default = 0x18
Missing manufacturer description.
PLAN_MOTIONTYPE : int, default = 0x19
Missing manufacturer description.
PLAN_SPEED_INPUT_PARAM : int, default = 0x1A
Missing manufacturer description.
PLAN_CP_CALC : int, default = 0x1B
Missing manufacturer description.
MOVE_INV_SINGULARITY : int, default = 0x20
Motion trajectory is in singularity position that causes inverse resolve alarm
in Cartesian coordinate syste.
MOVE_INV_CALC : int, default = 0x21
Motion is out of workspace that causes inverse resolve alarm when Dobot Magician moves.
MOVE_INV_LIMIT : int, default = 0x22
The inverse resolve is out of the limitation when Dobot Magician moves.
OVERSPEED_AXIS1 : int, default = 0x30
Joint 1 is overspeed.
OVERSPEED_AXIS2 : int, default = 0x31
Joint 2 is overspeed.
OVERSPEED_AXIS3 : int, default = 0x32
Joint 3 is overspeed.
OVERSPEED_AXIS4 : int, default = 0x33
Joint 4 is overspeed.
LIMIT_AXIS1_POS : int, default = 0x40
Joint 1 moves to the positive limitation area.
LIMIT_AXIS1_NEG : int, default = 0x41
Joint 1 moves to the negative limitation area.
LIMIT_AXIS2_POS : int, default = 0x42
Joint 2 moves to the positive limitation area.
LIMIT_AXIS2_NEG : int, default = 0x43
Joint 2 moves to the negative limitation area.
LIMIT_AXIS3_POS : int, default = 0x44
Joint 3 moves to the positive limitation area.
LIMIT_AXIS3_NEG : int, default = 0x45
Joint 3 moves to the negative limitation area.
LIMIT_AXIS4_POS : int, default = 0x46
Joint 4 moves to the positive limitation area.
LIMIT_AXIS4_NEG : int, default = 0x47
Joint 4 moves to the negative limitation area.
LIMIT_AXIS23_POS : int, default = 0x48
Parallelogram is stretched to the limitation area.
LIMIT_AXIS23_NEG : int, default = 0x49
Parallelogram is stretched to the limitation area.
LOSE_STEP_AXIS1 : int, default = 0x50
Joint 1 loses step.
LOSE_STEP_AXIS2 : int, default = 0x51
Joint 2 loses step.
LOSE_STEP_AXIS3 : int, default = 0x52
Joint 3 loses step.
LOSE_STEP_AXIS4 : int, default = 0x53
Joint 4 loses step.
OTHER_AXIS1_DRV_ALARM : int, default = 0x60
Missing manufacturer description.
OTHER_AXIS1_OVERFLOW : int, default = 0x61
Missing manufacturer description.
OTHER_AXIS1_FOLLOW : int, default = 0x62
Missing manufacturer description.
OTHER_AXIS2_DRV_ALARM : int, default = 0x63
Missing manufacturer description.
OTHER_AXIS2_OVERFLOW : int, default = 0x64
Missing manufacturer description.
OTHER_AXIS2_FOLLOW : int, default = 0x65
Missing manufacturer description.
OTHER_AXIS3_DRV_ALARM : int, default = 0x66
Missing manufacturer description.
OTHER_AXIS3_OVERFLOW : int, default = 0x67
Missing manufacturer description.
OTHER_AXIS3_FOLLOW : int, default = 0x68
Missing manufacturer description.
OTHER_AXIS4_DRV_ALARM : int, default = 0x69
Missing manufacturer description.
OTHER_AXIS4_OVERFLOW : int, default = 0x6A
Missing manufacturer description.
OTHER_AXIS4_FOLLOW : int, default = 0x6B
Missing manufacturer description.
MOTOR_REAR_ENCODER : int, default = 0x70
Missing manufacturer description.
MOTOR_REAR_TEMPERATURE_HIGH : int, default = 0x71
Missing manufacturer description.
MOTOR_REAR_TEMPERATURE_LOW : int, default = 0x72
Missing manufacturer description.
MOTOR_REAR_LOCK_CURRENT : int, default = 0x73
Missing manufacturer description.
MOTOR_REAR_BUSV_HIGH : int, default = 0x74
Missing manufacturer description.
MOTOR_REAR_BUSV_LOW : int, default = 0x75
Missing manufacturer description.
MOTOR_REAR_OVERHEAT : int, default = 0x76
Missing manufacturer description.
MOTOR_REAR_RUNAWAY : int, default = 0x77
Missing manufacturer description.
MOTOR_REAR_BATTERY_LOW : int, default = 0x78
Missing manufacturer description.
MOTOR_REAR_PHASE_SHORT : int, default = 0x79
Missing manufacturer description.
MOTOR_REAR_PHASE_WRONG : int, default = 0x7A
Missing manufacturer description.
MOTOR_REAR_LOST_SPEED : int, default = 0x7B
Missing manufacturer description.
MOTOR_REAR_NOT_STANDARDIZE : int, default = 0x7C
Missing manufacturer description.
ENCODER_REAR_NOT_STANDARDIZE : int, default = 0x7D
Missing manufacturer description.
MOTOR_REAR_CAN_BROKE : int, default = 0x7E
Missing manufacturer description.
MOTOR_FRONT_ENCODER : int, default = 0x80
Missing manufacturer description.
MOTOR_FRONT_TEMPERATURE_HIGH : int, default = 0x81
Missing manufacturer description.
MOTOR_FRONT_TEMPERATURE_LOW : int, default = 0x82
Missing manufacturer description.
MOTOR_FRONT_LOCK_CURRENT : int, default = 0x83
Missing manufacturer description.
MOTOR_FRONT_BUSV_HIGH : int, default = 0x84
Missing manufacturer description.
MOTOR_FRONT_BUSV_LOW : int, default = 0x85
Missing manufacturer description.
MOTOR_FRONT_OVERHEAT : int, default = 0x86
Missing manufacturer description.
MOTOR_FRONT_RUNAWAY : int, default = 0x87
Missing manufacturer description.
MOTOR_FRONT_BATTERY_LOW : int, default = 0x88
Missing manufacturer description.
MOTOR_FRONT_PHASE_SHORT : int, default = 0x89
Missing manufacturer description.
MOTOR_FRONT_PHASE_WRONG : int, default = 0x8A
Missing manufacturer description.
MOTOR_FRONT_LOST_SPEED : int, default = 0x8B
Missing manufacturer description.
MOTOR_FRONT_NOT_STANDARDIZE : int, default = 0x8C
Missing manufacturer description.
ENCODER_FRONT_NOT_STANDARDIZE : int, default = 0x8D
Missing manufacturer description.
MOTOR_FRONT_CAN_BROKE : int, default = 0x8E
Missing manufacturer description.
MOTOR_Z_ENCODER : int, default = 0x90
Missing manufacturer description.
MOTOR_Z_TEMPERATURE_HIGH : int, default = 0x91
Missing manufacturer description.
MOTOR_Z_TEMPERATURE_LOW : int, default = 0x92
Missing manufacturer description.
MOTOR_Z_LOCK_CURRENT : int, default = 0x93
Missing manufacturer description.
MOTOR_Z_BUSV_HIGH : int, default = 0x94
Missing manufacturer description.
MOTOR_Z_BUSV_LOW : int, default = 0x95
Missing manufacturer description.
MOTOR_Z_OVERHEAT : int, default = 0x96
Missing manufacturer description.
MOTOR_Z_RUNAWAY : int, default = 0x97
Missing manufacturer description.
MOTOR_Z_BATTERY_LOW : int, default = 0x98
Missing manufacturer description.
MOTOR_Z_PHASE_SHORT : int, default = 0x99
Missing manufacturer description.
MOTOR_Z_PHASE_WRONG : int, default = 0x9A
Missing manufacturer description.
MOTOR_Z_LOST_SPEED : int, default = 0x9B
Missing manufacturer description.
MOTOR_Z_NOT_STANDARDIZE : int, default = 0x9C
Missing manufacturer description.
ENCODER_Z_NOT_STANDARDIZE : int, default = 0x9D
Missing manufacturer description.
MOTOR_Z_CAN_BROKE : int, default = 0x9E
Missing manufacturer description.
MOTOR_R_ENCODER : int, default = 0xA0
Missing manufacturer description.
MOTOR_R_TEMPERATURE_HIGH : int, default = 0xA1
Missing manufacturer description.
MOTOR_R_TEMPERATURE_LOW : int, default = 0xA2
Missing manufacturer description.
MOTOR_R_LOCK_CURRENT : int, default = 0xA3
Missing manufacturer description.
MOTOR_R_BUSV_HIGH : int, default = 0xA4
Missing manufacturer description.
MOTOR_R_BUSV_LOW : int, default = 0xA5
Missing manufacturer description.
MOTOR_R_OVERHEAT : int, default = 0xA6
Missing manufacturer description.
MOTOR_R_RUNAWAY : int, default = 0xA7
Missing manufacturer description.
MOTOR_R_BATTERY_LOW : int, default = 0xA8
Missing manufacturer description.
MOTOR_R_PHASE_SHORT : int, default = 0xA9
Missing manufacturer description.
MOTOR_R_PHASE_WRONG : int, default = 0xAA
Missing manufacturer description.
MOTOR_R_LOST_SPEED : int, default = 0xAB
Missing manufacturer description.
MOTOR_R_NOT_STANDARDIZE : int, default = 0xAC
Missing manufacturer description.
ENCODER_R_NOT_STANDARDIZE : int, default = 0xAD
Missing manufacturer description.
MOTOR_R_CAN_BROKE : int, default = 0xAE
Missing manufacturer description.
MOTOR_ENDIO_IO : int, default = 0xB0
Missing manufacturer description.
MOTOR_ENDIO_RS485_WRONG : int, default = 0xB1
Missing manufacturer description.
MOTOR_ENDIO_CAN_BROKE : int, default = 0xB2
Missing manufacturer description.
References
----------
https://download.dobot.cc/product-manual/dobot-magician/pdf/en/Dobot-Magician-ALARM-Description.pdf
"""
COMMON_RESETTING = 0x00,
COMMON_UNDEFINED_INSTRUCTION = 0x01,
COMMON_FILE_SYSTEM = 0x02,
COMMON_MCU_FPGA_COMM = 0x03,
COMMON_ANGLE_SENSOR = 0x04
PLAN_INV_SINGULARITY = 0x10,
PLAN_INV_CALC = 0x11,
PLAN_INV_LIMIT = 0x12, # !!!
PLAN_PUSH_DATA_REPEAT = 0x13,
PLAN_ARC_INPUT_PARAM = 0x14,
PLAN_JUMP_PARAM = 0x15,
PLAN_LINE_HAND = 0x16,
PLAN_LINE_OUT_SPACE = 0x17,
PLAN_ARC_OUT_SPACE = 0x18,
PLAN_MOTIONTYPE = 0x19,
PLAN_SPEED_INPUT_PARAM = 0x1A,
PLAN_CP_CALC = 0x1B,
MOVE_INV_SINGULARITY = 0x20,
MOVE_INV_CALC = 0x21,
MOVE_INV_LIMIT = 0x22,
OVERSPEED_AXIS1 = 0x30,
OVERSPEED_AXIS2 = 0x31,
OVERSPEED_AXIS3 = 0x32,
OVERSPEED_AXIS4 = 0x33,
LIMIT_AXIS1_POS = 0x40,
LIMIT_AXIS1_NEG = 0x41,
LIMIT_AXIS2_POS = 0x42,
LIMIT_AXIS2_NEG = 0x43,
LIMIT_AXIS3_POS = 0x44,
LIMIT_AXIS3_NEG = 0x45,
LIMIT_AXIS4_POS = 0x46,
LIMIT_AXIS4_NEG = 0x47,
LIMIT_AXIS23_POS = 0x48,
LIMIT_AXIS23_NEG = 0x49
LOSE_STEP_AXIS1 = 0x50,
LOSE_STEP_AXIS2 = 0x51
LOSE_STEP_AXIS3 = 0x52
LOSE_STEP_AXIS4 = 0x53
OTHER_AXIS1_DRV_ALARM = 0x60,
OTHER_AXIS1_OVERFLOW = 0x61,
OTHER_AXIS1_FOLLOW = 0x62,
OTHER_AXIS2_DRV_ALARM = 0x63,
OTHER_AXIS2_OVERFLOW = 0x64,
OTHER_AXIS2_FOLLOW = 0x65,
OTHER_AXIS3_DRV_ALARM = 0x66,
OTHER_AXIS3_OVERFLOW = 0x67,
OTHER_AXIS3_FOLLOW = 0x68,
OTHER_AXIS4_DRV_ALARM = 0x69,
OTHER_AXIS4_OVERFLOW = 0x6A,
OTHER_AXIS4_FOLLOW = 0x6B,
MOTOR_REAR_ENCODER = 0x70,
MOTOR_REAR_TEMPERATURE_HIGH = 0x71
MOTOR_REAR_TEMPERATURE_LOW = 0x72,
MOTOR_REAR_LOCK_CURRENT = 0x73,
MOTOR_REAR_BUSV_HIGH = 0x74,
MOTOR_REAR_BUSV_LOW = 0x75,
MOTOR_REAR_OVERHEAT = 0x76,
MOTOR_REAR_RUNAWAY = 0x77,
MOTOR_REAR_BATTERY_LOW = 0x78,
MOTOR_REAR_PHASE_SHORT = 0x79,
MOTOR_REAR_PHASE_WRONG = 0x7A,
MOTOR_REAR_LOST_SPEED = 0x7B,
MOTOR_REAR_NOT_STANDARDIZE = 0x7C,
ENCODER_REAR_NOT_STANDARDIZE = 0x7D,
MOTOR_REAR_CAN_BROKE = 0x7E,
MOTOR_FRONT_ENCODER = 0x80,
MOTOR_FRONT_TEMPERATURE_HIGH = 0x81,
MOTOR_FRONT_TEMPERATURE_LOW = 0x82,
MOTOR_FRONT_LOCK_CURRENT = 0x83,
MOTOR_FRONT_BUSV_HIGH = 0x84,
MOTOR_FRONT_BUSV_LOW = 0x85,
MOTOR_FRONT_OVERHEAT = 0x86,
MOTOR_FRONT_RUNAWAY = 0x87,
MOTOR_FRONT_BATTERY_LOW = 0x88,
MOTOR_FRONT_PHASE_SHORT = 0x89,
MOTOR_FRONT_PHASE_WRONG = 0x8A,
MOTOR_FRONT_LOST_SPEED = 0x8B,
MOTOR_FRONT_NOT_STANDARDIZE = 0x8C,
ENCODER_FRONT_NOT_STANDARDIZE = 0x8D,
MOTOR_FRONT_CAN_BROKE = 0x8E,
MOTOR_Z_ENCODER = 0x90,
MOTOR_Z_TEMPERATURE_HIGH = 0x91,
MOTOR_Z_TEMPERATURE_LOW = 0x92,
MOTOR_Z_LOCK_CURRENT = 0x93,
MOTOR_Z_BUSV_HIGH = 0x94,
MOTOR_Z_BUSV_LOW = 0x95,
MOTOR_Z_OVERHEAT = 0x96,
MOTOR_Z_RUNAWAY = 0x97,
MOTOR_Z_BATTERY_LOW = 0x98,
MOTOR_Z_PHASE_SHORT = 0x99,
MOTOR_Z_PHASE_WRONG = 0x9A,
MOTOR_Z_LOST_SPEED = 0x9B,
MOTOR_Z_NOT_STANDARDIZE = 0x9C,
ENCODER_Z_NOT_STANDARDIZE = 0x9D,
MOTOR_Z_CAN_BROKE = 0x9E,
MOTOR_R_ENCODER = 0xA0,
MOTOR_R_TEMPERATURE_HIGH = 0xA1,
MOTOR_R_TEMPERATURE_LOW = 0xA2,
MOTOR_R_LOCK_CURRENT = 0xA3,
MOTOR_R_BUSV_HIGH = 0xA4,
MOTOR_R_BUSV_LOW = 0xA5,
MOTOR_R_OVERHEAT = 0xA6,
MOTOR_R_RUNAWAY = 0xA7,
MOTOR_R_BATTERY_LOW = 0xA8,
MOTOR_R_PHASE_SHORT = 0xA9
MOTOR_R_PHASE_WRONG = 0xAA,
MOTOR_R_LOST_SPEED = 0xAB,
MOTOR_R_NOT_STANDARDIZE = 0xAC,
ENCODER_R_NOT_STANDARDIZE = 0xAD,
MOTOR_R_CAN_BROKE = 0xAE,
MOTOR_ENDIO_IO = 0xB0,
MOTOR_ENDIO_RS485_WRONG = 0xB1,
MOTOR_ENDIO_CAN_BROKE = 0xB2
[docs]
class Bot:
"""
Creates an instance of Bot connected to the given serial port.
Parameters
----------
port: str, optional
Name of the serial port to connect
"""
J1_MIN = -120
"""Joint 1 (Base) - minimum allowed angle in degrees"""
J1_MAX = 120
"""Joint 1 (Base) - maximum allowed angle in degrees"""
J2_MIN = -5
"""Joint 2 (Rear arm) - minimum allowed angle in degrees"""
J2_MAX = 90
"""Joint 2 (Rear arm) - maximum allowed angle in degrees"""
J3_MIN = -15
"""Joint 3 (Forearm) - minimum allowed angle in degrees"""
J3_MAX = 90
"""Joint 3 (Forearm) - maximum allowed angle in degrees"""
J4_MIN = -140
"""Joint 4 (Rotation servo) - minimum allowed angle in degrees"""
J4_MAX = 140
"""Joint 4 (Rotation servo) - maximum allowed angle in degrees"""
def __init__(self, port: Optional[str] = None) -> None:
self.logger = logging.Logger(__name__)
self._lock = RLock()
if port is None:
# Find the serial port
ports = list_ports.comports()
for thing in ports:
if thing.vid in (4292, 6790):
self.logger.debug(f"Found a com port to talk to DOBOT ({thing}).")
port = thing.device
break
else:
raise BotException("Device not found!")
try:
self._ser = serial.Serial(
port,
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS)
except serial.serialutil.SerialException as e:
raise BotException from e
self.logger.debug('bot: %s open' % self._ser.name if self._ser.isOpen() else 'failed to open serial port')
self._set_queued_cmd_start_exec()
self._set_queued_cmd_clear()
self._set_ptp_joint_params(200, 200, 200, 200, 200, 200, 200, 200)
self._set_ptp_coordinate_params(velocity=200, acceleration=200)
self._set_ptp_jump_params(10, 200)
self._set_ptp_common_params(velocity=100, acceleration=100)
alarms = self.get_alarms()
if alarms:
self.logger.warning(f"Clearing alarms: {', '.join(map(str, alarms))}.")
self.clear_alarms()
[docs]
def close(self) -> None:
"""
Close serial connection.
See Also
--------
serial.Serial.close
"""
with self._lock:
self._ser.close()
self.logger.debug('bot: %s closed' % self._ser.name)
def _send_command(self, msg, wait = False) -> Message:
with self._lock:
self._ser.reset_input_buffer()
self._send_message(msg)
msg = self._read_message()
if msg is None:
raise BotException("No response!")
if not wait:
return msg
expected_idx = struct.unpack_from('L', msg.params, 0)[0]
while True:
current_idx = self._get_queued_cmd_current_index()
if current_idx != expected_idx:
time.sleep(0.1)
continue
break
return msg
def _send_message(self, msg) -> None:
self.logger.debug('bot: >>', msg)
with self._lock:
self._ser.write(msg.bytes())
def _read_message(self) -> Optional[Message]:
# Search for begin
begin_found = False
last_byte = None
tries = 5
while not begin_found and tries > 0:
current_byte = ord(self._ser.read(1))
if current_byte == 170:
if last_byte == 170:
begin_found = True
last_byte = current_byte
tries = tries - 1
if begin_found:
payload_length = ord(self._ser.read(1))
payload_checksum = self._ser.read(payload_length + 1)
if len(payload_checksum) == payload_length + 1:
b = bytearray([0xAA, 0xAA])
b.extend(bytearray([payload_length]))
b.extend(payload_checksum)
msg = Message(b)
self.logger.debug('Lenght', payload_length)
self.logger.debug(payload_checksum)
self.logger.debug('MessageID:', payload_checksum[0])
self.logger.debug('bot: <<', ":".join('{:02x}'.format(x) for x in b))
return msg
return None
[docs]
def get_pose(self) -> Pose:
"""
Returns the current position position of the robotic arm, both in the cartesian plane and the joints plane.
See Also
--------
bot.Pose
"""
msg = Message()
msg.id = 10
response = self._send_command(msg)
#TODO: Check the response return string.
return Pose(
Position(
struct.unpack_from('f', response.params, 0)[0],
struct.unpack_from('f', response.params, 4)[0],
struct.unpack_from('f', response.params, 8)[0],
struct.unpack_from('f', response.params, 12)[0]
),
Joints(
struct.unpack_from('f', response.params, 16)[0],
struct.unpack_from('f', response.params, 20)[0],
struct.unpack_from('f', response.params, 24)[0],
struct.unpack_from('f', response.params, 28)[0]
)
)
def get_alarms(self) -> Set[Alarm]:
msg = Message()
msg.id = 20
response = self._send_command(msg) # 32 bytes
ret: Set[Alarm] = set()
for idx in range(16):
alarm_byte = struct.unpack_from('B', response.params, idx)[0]
for alarm_index in [i for i in range(alarm_byte.bit_length()) if alarm_byte & (1 << i)]:
ret.add(Alarm(idx*8+alarm_index))
return ret
[docs]
def clear_alarms(self) -> None:
"""
Clears all alarms of the Robot.
"""
msg = Message()
msg.id = 20
msg.ctrl = 0x01
self._send_command(msg) # empty response
def _set_cp_cmd(self, x, y, z):
msg = Message()
msg.id = 91
msg.ctrl = 0x03
msg.params = bytearray(bytes([0x01]))
msg.params.extend(bytearray(struct.pack('f', x)))
msg.params.extend(bytearray(struct.pack('f', y)))
msg.params.extend(bytearray(struct.pack('f', z)))
msg.params.append(0x00)
return self._send_command(msg)
def _set_ptp_joint_params(self, v_x, v_y, v_z, v_r, a_x, a_y, a_z, a_r):
msg = Message()
msg.id = 80
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', v_x)))
msg.params.extend(bytearray(struct.pack('f', v_y)))
msg.params.extend(bytearray(struct.pack('f', v_z)))
msg.params.extend(bytearray(struct.pack('f', v_r)))
msg.params.extend(bytearray(struct.pack('f', a_x)))
msg.params.extend(bytearray(struct.pack('f', a_y)))
msg.params.extend(bytearray(struct.pack('f', a_z)))
msg.params.extend(bytearray(struct.pack('f', a_r)))
return self._send_command(msg)
def _set_ptp_coordinate_params(self, velocity, acceleration):
msg = Message()
msg.id = 81
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', velocity)))
msg.params.extend(bytearray(struct.pack('f', velocity)))
msg.params.extend(bytearray(struct.pack('f', acceleration)))
msg.params.extend(bytearray(struct.pack('f', acceleration)))
return self._send_command(msg)
def _set_ptp_jump_params(self, jump, limit):
msg = Message()
msg.id = 82
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', jump)))
msg.params.extend(bytearray(struct.pack('f', limit)))
return self._send_command(msg)
def _set_ptp_common_params(self, velocity, acceleration):
msg = Message()
msg.id = 83
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', velocity)))
msg.params.extend(bytearray(struct.pack('f', acceleration)))
return self._send_command(msg)
def _set_ptp_cmd(self, x, y, z, r, mode, wait):
msg = Message()
msg.id = 84
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([mode]))
msg.params.extend(bytearray(struct.pack('f', x)))
msg.params.extend(bytearray(struct.pack('f', y)))
msg.params.extend(bytearray(struct.pack('f', z)))
msg.params.extend(bytearray(struct.pack('f', r)))
return self._send_command(msg, wait)
def _set_end_effector_suction_cup(self, enable=False):
msg = Message()
msg.id = 62
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([0x01]))
if enable is True:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
return self._send_command(msg)
def _set_end_effector_gripper(self, enable=False):
msg = Message()
msg.id = 63
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([0x01]))
if enable is True:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
return self._send_command(msg)
def _set_end_effector_laser(self, power=255, enable=False):
"""Enables the laser. Power from 0 to 255. """
msg = Message()
msg.id = 61
msg.ctrl = 0x03
msg.params = bytearray([])
# msg.params.extend(bytearray([0x01]))
if enable is True:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
# Assuming the last byte is power. Seems to have little effect
msg.params.extend(bytearray([power]))
return self._send_command(msg)
def _set_queued_cmd_start_exec(self):
msg = Message()
msg.id = 240
msg.ctrl = 0x01
return self._send_command(msg)
def _set_queued_cmd_stop_exec(self):
msg = Message()
msg.id = 241
msg.ctrl = 0x01
return self._send_command(msg)
def _set_queued_cmd_clear(self):
msg = Message()
msg.id = 245
msg.ctrl = 0x01
return self._send_command(msg)
def _get_queued_cmd_current_index(self):
msg = Message()
msg.id = 246
response = self._send_command(msg)
if response and response.id == 246:
return self._extract_cmd_index(response)
else:
return -1
@staticmethod
def _extract_cmd_index(response):
return struct.unpack_from('I', response.params, 0)[0]
def wait_for_cmd(self, cmd_id):
current_cmd_id = self._get_queued_cmd_current_index()
while cmd_id > current_cmd_id:
self.logger.debug("Current-ID", current_cmd_id)
self.logger.debug("Waiting for", cmd_id)
current_cmd_id = self._get_queued_cmd_current_index()
def _set_home_cmd(self):
msg = Message()
msg.id = 31
msg.ctrl = 0x03
msg.params = bytearray([])
return self._send_command(msg)
def _set_arc_cmd(self, x, y, z, r, cir_x, cir_y, cir_z, cir_r):
msg = Message()
msg.id = 101
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', cir_x)))
msg.params.extend(bytearray(struct.pack('f', cir_y)))
msg.params.extend(bytearray(struct.pack('f', cir_z)))
msg.params.extend(bytearray(struct.pack('f', cir_r)))
msg.params.extend(bytearray(struct.pack('f', x)))
msg.params.extend(bytearray(struct.pack('f', y)))
msg.params.extend(bytearray(struct.pack('f', z)))
msg.params.extend(bytearray(struct.pack('f', r)))
return self._send_command(msg)
def _set_home_coordinate(self, x, y, z, r):
msg = Message()
msg.id = 30
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', x)))
msg.params.extend(bytearray(struct.pack('f', y)))
msg.params.extend(bytearray(struct.pack('f', z)))
msg.params.extend(bytearray(struct.pack('f', r)))
return self._send_command(msg)
def _set_jog_coordinate_params(self, vx, vy, vz, vr, ax=100, ay=100, az=100, ar=100):
msg = Message()
msg.id = 71
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', vx)))
msg.params.extend(bytearray(struct.pack('f', vy)))
msg.params.extend(bytearray(struct.pack('f', vz)))
msg.params.extend(bytearray(struct.pack('f', vr)))
msg.params.extend(bytearray(struct.pack('f', ax)))
msg.params.extend(bytearray(struct.pack('f', ay)))
msg.params.extend(bytearray(struct.pack('f', az)))
msg.params.extend(bytearray(struct.pack('f', ar)))
return self._send_command(msg)
def _set_jog_command(self, cmd):
msg = Message()
msg.id = 73
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([0x0]))
msg.params.extend(bytearray([cmd]))
return self._send_command(msg)
def jog_x(self, v):
self._set_jog_coordinate_params(abs(v), 0, 0, 0,)
if v > 0:
cmd = 1
elif v < 0:
cmd = 2
else:
cmd = 0
self.wait_for_cmd(self._extract_cmd_index(self._set_jog_command(cmd)))
def jog_y(self, v):
self._set_jog_coordinate_params(0, abs(v), 0, 0)
if v > 0:
cmd = 3
elif v < 0:
cmd = 4
else:
cmd = 0
self.wait_for_cmd(self._extract_cmd_index(self._set_jog_command(cmd)))
def jog_z(self, v):
self._set_jog_coordinate_params(0, 0, abs(v), 0)
if v > 0:
cmd = 5
elif v < 0:
cmd = 6
else:
cmd = 0
self.wait_for_cmd(self._extract_cmd_index(self._set_jog_command(cmd)))
def jog_r(self, v):
self._set_jog_coordinate_params(0, 0, 0, abs(v))
if v > 0:
cmd = 7
elif v < 0:
cmd = 8
else:
cmd = 0
self.wait_for_cmd(self._extract_cmd_index(self._set_jog_command(cmd)))
def set_io(self, address: int, state: bool):
if not 1 <= address <= 22:
raise BotException("Invalid address range.")
msg = Message()
msg.id = 131
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('B', address)))
msg.params.extend(bytearray(struct.pack('B', int(state))))
self.wait_for_cmd(self._extract_cmd_index(self._send_command(msg)))
def set_hht_trig_output(self, state: bool) -> None:
msg = Message()
msg.id = 41
msg.ctrl = 0x02
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('B', int(state))))
self._send_command(msg)
def get_hht_trig_output(self) -> bool:
msg = Message()
msg.id = 41
msg.ctrl = 0
response = self._send_command(msg)
return bool(struct.unpack_from('B', response.params, 0)[0])
def go_arc(self, x, y, z, r, cir_x, cir_y, cir_z, cir_r):
return self._extract_cmd_index(self._set_arc_cmd(x, y, z, r, cir_x, cir_y, cir_z, cir_r))
[docs]
def suck(self, enable=False):
"""
Control suction in the end effector.
Parameters
----------
enable: bool, default = False
Notes
-----
Will only have effect if the suction module is mounted and the presure module is connected.
"""
return self._extract_cmd_index(self._set_end_effector_suction_cup(enable))
[docs]
def set_home(self, x, y, z, r=0.):
"""
Defines the dobot robotic arm home position in the cartesian plane.
Parameters
----------
x : float
y : float
z : float
r : float, default = 0.
See Also
--------
bot.home
"""
self._set_home_coordinate(x, y, z, r)
[docs]
def home(self):
"""
Send the robotic arm to the defined home position.
Notes
-----
This is an important step in the intialization and calibration of the robot.
See Also
--------
bot.set_home
"""
return self._extract_cmd_index(self._set_home_cmd())
[docs]
def grip(self, enable=False):
"""
Control claw grip in the end effector.
Parameters
----------
enable: bool, default = False
Notes
-----
Will only have effect if the claw module is mounted and the presure module is connected.
"""
return self._extract_cmd_index(self._set_end_effector_gripper(enable))
def laze(self, power=0, enable=False):
return self._extract_cmd_index(self._set_end_effector_laser(power, enable))
def speed(self, velocity=100., acceleration=100.):
self.wait_for_cmd(self._extract_cmd_index(self._set_ptp_common_params(velocity, acceleration)))
self.wait_for_cmd(self._extract_cmd_index(self._set_ptp_coordinate_params(velocity, acceleration)))
def conveyor_belt(self, speed, direction=1, interface=0):
if 0.0 <= speed <= 1.0 and (direction == 1 or direction == -1):
motor_speed = int(50 * speed * STEP_PER_CIRCLE / MM_PER_CIRCLE * direction)
self._set_stepper_motor(motor_speed, interface)
else:
raise BotException("Wrong Parameter")
def _set_stepper_motor(self, speed, interface=0, motor_control=True):
msg = Message()
msg.id = 0x87
msg.ctrl = 0x03
msg.params = bytearray([])
if interface == 1:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
if motor_control is True:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
msg.params.extend(bytearray(struct.pack('i', speed)))
return self._send_command(msg)
def _set_stepper_motor_distance(self, speed, distance, interface=0, motor_control=True):
msg = Message()
msg.id = 0x88
msg.ctrl = 0x03
msg.params = bytearray([])
if interface == 1:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
if motor_control is True:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
msg.params.extend(bytearray(struct.pack('i', speed)))
msg.params.extend(bytearray(struct.pack('I', distance)))
return self._send_command(msg)
def _set_cp_params(self, velocity, acceleration, period):
msg = Message()
msg.id = 90
msg.ctrl = 0x3
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', acceleration)))
msg.params.extend(bytearray(struct.pack('f', velocity)))
msg.params.extend(bytearray(struct.pack('f', period)))
msg.params.extend(bytearray([0x0])) # non real-time mode (what does it mean??)
return self._send_command(msg)
def _set_cple_cmd(self, x, y, z, power, absolute=False):
assert 0 <= power <= 100
msg = Message()
msg.id = 92
msg.ctrl = 0x3
msg.params = bytearray([int(absolute)])
msg.params.extend(bytearray(struct.pack('f', x)))
msg.params.extend(bytearray(struct.pack('f', y)))
msg.params.extend(bytearray(struct.pack('f', z)))
msg.params.extend(bytearray(struct.pack('f', power)))
return self._send_command(msg)
def engrave(self, image, pixel_size, low=0.0, high=40.0, velocity=5, acceleration=5, actual_acceleration=5):
image = image.astype("float64")
image = 255.0 - image
image = (image - image.min()) / (image.max() - image.min()) * (high - low) + low
x, y, z = self.get_pose().position[0:3] # get current/starting position
self.wait_for_cmd(self.laze(0, False))
self._set_queued_cmd_clear()
self.wait_for_cmd(self._extract_cmd_index(self._set_cp_params(velocity, acceleration, actual_acceleration)))
self._set_queued_cmd_stop_exec()
stopped = True
indexes = deque()
for row_idx, row in enumerate(image):
# first feed the queue to be almost full
if stopped and len(indexes) > MAX_QUEUE_LEN-2:
self._set_queued_cmd_start_exec()
stopped = False
if row_idx % 2 == 1:
data = reversed(row)
rev = True
else:
data = row
rev = False
for col_idx, ld in enumerate(data):
if not rev:
y_ofs = col_idx * pixel_size
else:
y_ofs = (len(row)-1 - col_idx) * pixel_size
indexes.append(
self._extract_cmd_index(self._set_cple_cmd(x + row_idx * pixel_size, y + y_ofs, z, ld, True)))
# then feed it as necessary to keep it almost full
while not stopped and len(indexes) > MAX_QUEUE_LEN-12:
self.wait_for_cmd(indexes.popleft())
self.wait_for_cmd(self.laze(0, False))
PORT_GP1 = 0x00
PORT_GP2 = 0x01
PORT_GP4 = 0x02
PORT_GP5 = 0x03
def conveyor_belt_distance(self, speed_mm_per_sec, distance_mm, direction=1, interface=0):
if speed_mm_per_sec > 100:
raise magician.dobot.BotException("Speed must be <= 100 mm/s")
MM_PER_REV = 34 * math.pi # Seems to actually be closer to 36mm when measured but 34 works better
STEP_ANGLE_DEG = 1.8
STEPS_PER_REV = 360.0 / STEP_ANGLE_DEG * 10.0 * 16.0 / 2.0 # Spec sheet says that it can do 1.8deg increments, no idea what the 10 * 16 / 2 fuck factor is....
distance_steps = distance_mm / MM_PER_REV * STEPS_PER_REV
speed_steps_per_sec = speed_mm_per_sec / MM_PER_REV * STEPS_PER_REV * direction
return self._extract_cmd_index(self._set_stepper_motor_distance(int(speed_steps_per_sec), int(distance_steps), interface))
def set_color(self, enable=True, port=PORT_GP2, version=0x1):
msg = Message()
msg.id = 137
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([int(enable)]))
msg.params.extend(bytearray([port]))
msg.params.extend(bytearray([version])) # Version1=0, Version2=1
return self._extract_cmd_index(self._send_command(msg))
def get_color(self, port=PORT_GP2, version=0x1):
msg = Message()
msg.id = 137
msg.ctrl = 0x00
msg.params = bytearray([])
msg.params.extend(bytearray([port]))
msg.params.extend(bytearray([0x01]))
msg.params.extend(bytearray([version])) # Version1=0, Version2=1
response = self._send_command(msg)
print(response)
r = struct.unpack_from('?', response.params, 0)[0]
g = struct.unpack_from('?', response.params, 1)[0]
b = struct.unpack_from('?', response.params, 2)[0]
return [r, g, b]
def set_ir(self, enable=True, port=PORT_GP4):
msg = Message()
msg.id = 138
msg.ctrl = 0x02
msg.params = bytearray([])
msg.params.extend(bytearray([int(enable)]))
msg.params.extend(bytearray([port]))
return self._extract_cmd_index(self._send_command(msg))
def get_ir(self, port=PORT_GP4):
msg = Message()
msg.id = 138
msg.ctrl = 0x00
msg.params = bytearray([])
msg.params.extend(bytearray([port]))
response = self._send_command(msg)
state = struct.unpack_from('?', response.params, 0)[0]
return state
# Do not use! Not tested and poorly implemented.
[docs]
def check_bounds(self,joints: Joints = None):
"""
Validate the bounds of the robot desired end position.
Parameters
----------
joints : bot.Joints
The joints object to check.
Returns
-------
Nothing
Raises
------
ValueError
If joints are not provided.
BotBoundsException
If any joint is out of bounds.
"""
if (joints == None):
raise ValueError("Either Position or Joints must be provided.")
joint_limits = {
'j1': (self.J1_MIN, self.J1_MAX),
'j2': (self.J2_MIN, self.J2_MAX),
'j3': (self.J3_MIN, self.J3_MAX),
'j4': (self.J4_MIN, self.J4_MAX),
}
for j_name, (j_min, j_max) in joint_limits.items():
j_value = getattr(joints, j_name)
if j_value > j_max or j_value < j_min:
raise BotBoundsException(f"Joint {j_name} out of bounds: {j_min} <= {j_name} <= {j_max}")
# Absolute Movement Functions
[docs]
def move_to_xyz(self, mode: MOVE_TYPE, x=None, y=None, z=None, r=None, wait = True, position: Position = None):
"""
Move robotic arm to the (x,y,z) position in the Cartesian plane and set the end effector angle to r.
Parameters
----------
mode : bot.MOVE_TYPE
Type of movement to be performed. Valid options { MOVE_TYPE.JUMP, MOVE_TYPE.MOVJ, MOVE_TYPE.MOVL }.
x : float, optional
Target end effector x-coordinate. Defaults to the current x-coordinate if None.
y : float, optional
Target end effector y-coordinate. Defaults to the current y-coordinate if None.
z : float, optional
Target end effector z-coordinate. Defaults to the current z-coordinate if None.
r : float, optional
Target end effector rotation servo orientation. Defaults to 0.
wait : bool, optional
If True, wait for the previous movement to complete before executing this new one. Defaults to True.
position : bot.Position, optional
A Position object containing x, y, z, and r values. If provided, overrides x, y, z, and r parameters.
Returns
-------
Robot ptp movement instruction.
Raises
------
ValueError
If both position and (x, y, z) args are None, or if an invalid movement type is provided.
Examples
--------
>>> move_to_xyz(MODE_TYPE.MOVJ,x=200,y=200,z=10)
>>> move_to_xyz(MODE_TYPE.MOVJ,z=0)
>>> pos = Position(x=0,y=200,z=0,r=0)
>>> move_to_xyz(MODE_TYPE.JUMP,position=pos)
"""
if position is not None:
x, y, z, r = position.x, position.y, position.z, position.r
elif x is None and y is None and z is None:
raise ValueError("Either a Position object or x, y, z coordinates must be provided")
c_pose = self.get_pose().position
if x is None: x = c_pose.x
if y is None: y = c_pose.y
if z is None: z = c_pose.z
if r is None: r = c_pose.r
match mode:
case MOVE_TYPE.JUMP:
mode = MODE_PTP.JUMP_XYZ
case MOVE_TYPE.MOVJ:
mode = MODE_PTP.MOVJ_XYZ
case MOVE_TYPE.MOVL:
mode = MODE_PTP.MOVL_XYZ
case _:
raise ValueError("Invalid movement type provided. Valid movements: MOVE_TYPE.JUMP,MOVE_TYPE.MOVJ,MOVE_TYPE.MOVL")
return self._extract_cmd_index(self._set_ptp_cmd(x, y, z, r, mode, wait = wait))
[docs]
def move_to_ang(self, mode: MOVE_TYPE, j1=None, j2=None, j3=None, j4=None, wait = True, joints: Joints = None):
"""
Move robotic arm to the (j1,j2,j3) position in the joints plane and set the end effector rotation
servo angle to j4.
Parameters
----------
mode : bot.MOVE_TYPE
Type of movement to be performed. Valid options { MOVE_TYPE.JUMP, MOVE_TYPE.MOVJ, MOVE_TYPE.MOVL }.
j1 : float, optional
Target J1 angle. Defaults to the current j1 angle if None.
j2 : float, optional
Target J2 angle. Defaults to the current j2 angle if None.
j3 : float, optional
Target J4 angle. Defaults to the current j3 angle if None.
j4 : float, optional
Target J4 rotation servo angle. Defaults to 0.
wait : bool, optional
If True, wait for the previous movement to complete before executing this new one. Defaults to True.
joints : Joints, optional
A Joints object containing j1, j2, j3, and j4 values. If provided, overrides j1, j2, j3, and j4 parameters.
Returns
-------
Robot ptp movement instruction.
Raises
------
ValueError
If both joints and (j1, j2, j3) args are None, or if an invalid movement type is provided.
Examples
--------
>>> move_to_ang(MODE_TYPE.MOVJ,x=-100,y=0,z=0)
>>> move_to_ang(MODE_TYPE.MOVJ,x=0)
>>> jnts = Joints(j1=50,j2=10,j3=0,j4=0)
>>> move_to_ang(MODE_TYPE.MOVL,position=pos)
"""
if joints is not None:
j1, j2, j3, j4 = joints.j1, joints.j2, joints.j3, joints.j4
elif j1 is None and j2 is None and j3 is None:
raise ValueError("Either a Position object or j1, j2, j3 coordinates must be provided")
j_pose = self.get_pose().joints
if j1 is None: j1 = j_pose.j1
if j2 is None: j2 = j_pose.j2
if j3 is None: j3 = j_pose.j3
if j4 is None: j4 = j_pose.j4
match mode:
case MOVE_TYPE.JUMP:
mode = MODE_PTP.JUMP_ANGLE
case MOVE_TYPE.MOVJ:
mode = MODE_PTP.MOVJ_ANGLE
case MOVE_TYPE.MOVL:
mode = MODE_PTP.MOVL_ANGLE
case _:
raise ValueError("Invalid movement type provided. Valid movements: MOVE_TYPE.JUMP,MOVE_TYPE.MOVJ,MOVE_TYPE.MOVL")
self.check_bounds(Joints(j1,j2,j3,j4))
return self._extract_cmd_index(self._set_ptp_cmd(j1, j2, j3, j4, mode, wait = wait))
# Relative movement functions
[docs]
def inc_xyz(self, mode: MOVE_TYPE, x=0, y=0, z=0, r=0, wait = True):
"""
Increment robotic arm end effector position by (x,y,z) in the Cartesian plane and increment
the end effector rotation servo angle by r.
Parameters
----------
mode : bot.MOVE_TYPE
Type of movement to be performed. Valid options { MOVE_TYPE.JUMP, MOVE_TYPE.MOVJ }.
x : float, optional
End effector x-coordinate change. Defaults to 0.
y : float, optional
End effector y-coordinate change. Defaults to 0.
z : float, optional
End effector z-coordinate change. Defaults to 0.
r : float, optional
End effector rotarion servo orientation change. Defaults to 0.
wait : bool, optional
If True, wait for the previous movement to complete before executing this new one. Defaults to True.
Returns
-------
Robot ptp movement instruction.
Raises
------
ValueError
If an invalid movement type is provided.
Examples
--------
>>> inc_xyz(MODE_TYPE.MOVJ,x=-10,y=0,z=10)
>>> inc_xyz(MODE_TYPE.MOVJ,y=-5)
"""
match mode:
case MOVE_TYPE.JUMP:
mode = MODE_PTP.JUMP_MOVL_XYZ
case MOVE_TYPE.MOVJ:
mode = MODE_PTP.MOVJ_XYZ_INC
case _:
raise ValueError("Invalid movement type provided. Valid movements: MOVE_TYPE.JUMP,MOVE_TYPE.MOVJ")
return self._extract_cmd_index(self._set_ptp_cmd(x, y, z, r, mode, wait = wait))
# TODO: check if the MOVL mode behaves as expected.
[docs]
def inc_ang(self, mode: MOVE_TYPE, j1=0, j2=0, j3=0, j4=0, wait = True):
"""
Increment robotic arm position by (j1,j2,j3) in the joints plane and increment
end effector angle by j4.
Parameters
----------
mode : MOVE_TYPE
Type of movement to be performed. Valid options { MOVE_TYPE.MOVEJ, MOVE_TYPE.MOVL }.
j1 : float, optional
J1 angle change in degrees. Defaults to 0.
j2 : float, optional
J2 angle change in degrees. Defaults to 0.
j3 : float, optional
J3 angle change in degrees. Defaults to 0.
j4 : float, optional
J4 angle change in degrees. Defaults to 0.
wait : bool, optional
If True, wait for the previous movement to complete before executing this new one. Defaults to True.
Returns
-------
Robot ptp movement instruction.
Raises
------
ValueError
If an invalid movement type is provided.
Examples
--------
>>> inc_ang(MODE_TYPE.MOVJ,x=-10,y=0,z=10)
>>> inc_ang(MODE_TYPE.MOVJ,y=-5)
"""
match mode:
case MOVE_TYPE.MOVJ:
mode = MODE_PTP.MOVJ_INC
case MOVE_TYPE.MOVL:
mode = MODE_PTP.MOVL_INC
case _:
raise ValueError("Invalid movement type provided. Valid movements: MOVE_TYPE.MOVJ,MOVE_TYPE.MOVL")
j_pose = self.get_pose().joints
self.check_bounds(Joints(j1+j_pose.j1,j2+j_pose.j2,j3+j_pose.j3,j4+j_pose.j4))
return self._extract_cmd_index(self._set_ptp_cmd(j1, j2, j3, j4, mode, wait = wait))