def get_end_effector_pose(self) -> Pose:
if self.simulator:
return self.forward_kinematics(self.robot_joints())
try:
pos = self._dobot.get_pose() # in mm
except DobotApiException as e:
raise DobotException("Failed to get pose.") from e
p = Pose()
p.position.x = pos.position.x / 1000.0
p.position.y = pos.position.y / 1000.0
p.position.z = pos.position.z / 1000.0
p.orientation = self.ROTATE_EEF * Orientation.from_rotation_vector(
z=math.radians(pos.position.r))
self._handle_pose_out(p)
return tr.make_pose_abs(self.pose, p)
class Dobot(metaclass=ABCMeta):
ROTATE_EEF = Orientation.from_rotation_vector(y=math.pi)
UNROTATE_EEF = ROTATE_EEF.inversed()
def __init__(self,
pose: Pose,
port: str = "/dev/dobot",
simulator: bool = False) -> None:
self.pose = pose
self.simulator = simulator
self._move_lock = NonBlockingLock()
if not self.simulator:
try:
self._dobot = DobotApi(port)
except DobotApiException as e:
raise DobotApiException(
"Could not connect to the robot.") from e
else:
self._joint_values: List[Joint] = [
] # has to be set to some initial value in derived classes
self._hand_teaching = False
def alarms_to_exception(self) -> None:
try:
alarms = self._dobot.get_alarms()
except DobotApiException as e:
raise DobotApiException("Failed to get alarms.") from e
if alarms:
raise DobotApiException(
f"Alarm(s): {','.join([alarm.name for alarm in alarms])}.")
def cleanup(self) -> None:
if not self.simulator:
self._dobot.close()
@property
def hand_teaching_mode(self) -> bool:
if self.simulator:
return self._hand_teaching
return self._dobot.get_hht_trig_output()
@hand_teaching_mode.setter
def hand_teaching_mode(self, value: bool) -> None:
if self.simulator:
self._hand_teaching = value
return
self._dobot.set_hht_trig_output(value)
def _inverse_kinematics(self, pose: Pose) -> List[Joint]:
raise Arcor2NotImplemented()
def inverse_kinematics(self, pose: Pose) -> List[Joint]:
raise Arcor2NotImplemented()
def forward_kinematics(self, joints: List[Joint]) -> Pose:
raise Arcor2NotImplemented()
def _handle_pose_out(self, pose: Pose) -> None:
"""This is called (only for a real robot) from `get_end_effector_pose`
so derived classes can do custom changes to the pose.
:param pose:
:return:
"""
pass
def _handle_pose_in(self, pose: Pose) -> None:
"""This is called (only for a real robot) from `move` so derived
classes can do custom changes to the pose.
:param pose:
:return:
"""
pass
def _check_orientation(self, pose: Pose) -> None:
unrotated = self.UNROTATE_EEF * pose.orientation
eps = 1e-6
if abs(unrotated.x) > eps or abs(unrotated.y) > eps:
raise DobotApiException("Impossible orientation.")
def get_end_effector_pose(self) -> Pose:
if self.simulator:
return self.forward_kinematics(self.robot_joints())
try:
pos = self._dobot.get_pose() # in mm
except DobotApiException as e:
raise DobotException("Failed to get pose.") from e
p = Pose()
p.position.x = pos.position.x / 1000.0
p.position.y = pos.position.y / 1000.0
p.position.z = pos.position.z / 1000.0
p.orientation = self.ROTATE_EEF * Orientation.from_rotation_vector(
z=math.radians(pos.position.r))
self._handle_pose_out(p)
return tr.make_pose_abs(self.pose, p)
def home(self) -> None:
"""Run the homing procedure."""
with self._move_lock:
if self.simulator:
time.sleep(2.0)
return
try:
self._dobot.clear_alarms()
self._dobot.wait_for_cmd(self._dobot.home())
except DobotApiException as e:
raise DobotException("Homing failed.") from e
self.alarms_to_exception()
def move(self,
pose: Pose,
move_type: MoveType,
velocity: float = 50.0,
acceleration: float = 50.0) -> None:
"""Moves the robot's end-effector to a specific pose.
:param pose: Target pose.
:move_type: Move type.
:param velocity: Speed of move (percent).
:param acceleration: Acceleration of move (percent).
:return:
"""
if not (0.0 <= velocity <= 100.0):
raise DobotException("Invalid velocity.")
if not (0.0 <= acceleration <= 100.0):
raise DobotException("Invalid acceleration.")
with self._move_lock:
rp = tr.make_pose_rel(self.pose, pose)
# prevent Dobot from moving when given an unreachable goal
try:
jv = self._inverse_kinematics(rp)
except Arcor2NotImplemented: # TODO remove this once M1 has IK
pass
if self.simulator:
self._joint_values = jv
time.sleep((100.0 - velocity) * 0.05)
return
self._handle_pose_in(rp)
try:
self._dobot.clear_alarms()
unrotated = self.UNROTATE_EEF * rp.orientation
rotation = math.degrees(
quaternion.as_rotation_vector(
unrotated.as_quaternion())[2])
self._dobot.speed(velocity, acceleration)
self._dobot.wait_for_cmd(
self._dobot.move_to(
rp.position.x * 1000.0,
rp.position.y * 1000.0,
rp.position.z * 1000.0,
rotation,
MOVE_TYPE_MAPPING[move_type],
))
except DobotApiException as e:
raise DobotException("Move failed.") from e
self.alarms_to_exception()
@abstractmethod
def suck(self) -> None:
pass
@abstractmethod
def release(self) -> None:
pass
@abstractmethod
def robot_joints(self) -> List[Joint]:
pass
