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
def test_make_pose_abs_2() -> None:
parent = Pose(Position(-1, 2, -3), Orientation(0, 0.707, -0.707, 0))
child = Pose()
assert make_pose_abs(parent, child) == parent
def test_make_pose_rel_and_abs_again_2() -> None:
parent = Pose(Position(-1, 1, -1), Orientation(0, -0.707, 0.707, 0))
child_to_be = Pose(Position(10, -10, 3))
child = make_pose_rel(parent, child_to_be)
assert make_pose_abs(parent, child) == child_to_be
def test_make_pose_rel() -> None:
parent = Pose(Position(1, 2, 3), Orientation(0, 0, 1, 0))
child_to_be = copy.deepcopy(parent)
assert make_pose_rel(parent, child_to_be) == Pose()
def test_make_pose_abs() -> None:
parent = Pose(Position(1, 2, 3), Orientation(0, 0, 1, 0))
child = Pose()
assert make_pose_abs(parent, child) == parent
def test_valid_orientation() -> None:
o1 = Orientation(0, 0, 0, 1)
o2 = Orientation()
o2.set_from_quaternion(o1.as_quaternion())
assert o1 == o2
def test_pose_inv() -> None:
p = Pose(Position(1, 1, 1), Orientation(0.707, 0, 0, 0.707))
pi = p.inversed()
assert pi == Pose(Position(-1, -1, 1), Orientation(-0.707, 0, 0, 0.707))
assert p == pi.inversed()
def test_make_orientation_rel():
parent = Orientation(0.707, 0, 0.707, 0)
child = Orientation(0.707, 0, 0.707, 0)
assert make_orientation_rel(parent, child) == Orientation(0, 0, 0, 1)
def test_make_orientation_abs_3():
parent = Orientation(0, 1, 0, 0)
child = Orientation(0, 0, 0, 1)
assert make_orientation_abs(parent, child) == parent
def test_project_ap_rpcs(start_processes: None, ars: ARServer) -> None:
upload_def(Box, BoxModel(Box.__name__, 1, 2, 3))
event(ars, events.c.ShowMainScreen)
assert ars.call_rpc(
rpc.s.NewScene.Request(get_id(), rpc.s.NewScene.Request.Args("Test scene")), rpc.s.NewScene.Response
).result
assert len(event(ars, events.o.ChangedObjectTypes).data) == 1
scene_data = event(ars, events.s.OpenScene).data
assert scene_data
scene = scene_data.scene
event(ars, events.s.SceneState)
assert ars.call_rpc(
rpc.s.AddObjectToScene.Request(get_id(), rpc.s.AddObjectToScene.Request.Args("box", Box.__name__, Pose())),
rpc.s.AddObjectToScene.Response,
).result
obj = event(ars, events.s.SceneObjectChanged).data
assert obj
# ------------------------------------------------------------------------------------------------------------------
assert ars.call_rpc(
rpc.p.NewProject.Request(get_id(), rpc.p.NewProject.Request.Args(scene.id, "Project name")),
rpc.p.NewProject.Response,
).result
event(ars, events.s.SceneSaved)
event(ars, events.p.OpenProject)
event(ars, events.s.SceneState)
assert ars.call_rpc(
rpc.p.AddActionPoint.Request(get_id(), rpc.p.AddActionPoint.Request.Args("parent_ap", Position())),
rpc.p.AddActionPoint.Response,
).result
parent_ap_evt = event(ars, events.p.ActionPointChanged)
assert ars.call_rpc(
rpc.p.AddActionPoint.Request(
get_id(), rpc.p.AddActionPoint.Request.Args("child_ap", Position(-1), parent_ap_evt.data.id)
),
rpc.p.AddActionPoint.Response,
).result
child_ap_evt = event(ars, events.p.ActionPointChanged)
assert child_ap_evt.data.parent == parent_ap_evt.data.id
lock_object(ars, child_ap_evt.data.id)
assert ars.call_rpc(
rpc.p.AddActionPointOrientation.Request(
get_id(), rpc.p.AddActionPointOrientation.Request.Args(child_ap_evt.data.id, Orientation())
),
rpc.p.AddActionPointOrientation.Response,
).result
ori = event(ars, events.p.OrientationChanged)
assert ars.call_rpc(
rpc.p.AddAction.Request(
get_id(),
rpc.p.AddAction.Request.Args(
child_ap_evt.data.id,
"act_name",
f"{obj.id}/{Box.update_pose.__name__}",
[ActionParameter("new_pose", PosePlugin.type_name(), json.dumps(ori.data.id))],
[Flow()],
),
),
rpc.p.AddAction.Response,
).result
event(ars, events.p.ActionChanged)
unlock_object(ars, child_ap_evt.data.id)
ars.event_mapping[ActionChanged.__name__] = ActionChanged
assert ars.call_rpc(
rpc.p.CopyActionPoint.Request(get_id(), rpc.p.CopyActionPoint.Request.Args(parent_ap_evt.data.id)),
rpc.p.CopyActionPoint.Response,
).result
new_parent_ap = event(ars, events.p.ActionPointChanged)
assert not new_parent_ap.data.parent
new_child_ap = event(ars, events.p.ActionPointChanged)
assert new_child_ap.data.parent == new_parent_ap.data.id
new_ori = event(ars, events.p.OrientationChanged)
assert new_ori.parent_id == new_child_ap.data.id
# with events.p.ActionChanged it would return only BareAction (without parameters)
new_action = event(ars, ActionChanged)
ars.event_mapping[ActionChanged.__name__] = events.p.ActionChanged
assert new_action.parent_id == new_child_ap.data.id
# Pose parameter (orientation id) should be updated now
assert len(new_action.data.parameters) == 1
assert json.loads(new_action.data.parameters[0].value) == new_ori.data.id
