def make_position_abs(parent: Position, child: Position) -> Position:
p = Position()
p.x = child.x + parent.x
p.y = child.y + parent.y
p.z = child.z + parent.z
return p
def test_make_pose_rel_and_abs_again() -> None:
parent = Pose(Position(), Orientation(0, 0, 1, 0))
child_to_be = Pose(Position(1, 0, 0))
child = make_pose_rel(parent, child_to_be)
assert child == Pose(Position(-1, 0, 0), Orientation(0, 0, -1, 0))
assert make_pose_abs(parent, child) == child_to_be
def test_obj_relative_ap_global() -> None:
scene = Scene("s1")
# object rotated 90° clock-wise
so1 = SceneObject(
"so1", "WhatEver",
Pose(Position(1, 0, 0), Orientation(0, 0, -0.707, 0.707)))
scene.objects.append(so1)
cached_scene = CachedScene(scene)
project = Project("p1", scene.id)
ap1 = ActionPoint("ap1", Position(1, 0, 0), parent=so1.id)
project.action_points.append(ap1)
ap2 = ActionPoint("ap2", Position(1, 0, 0), parent=ap1.id)
no1 = NamedOrientation("o1", Orientation())
ap2.orientations.append(no1)
project.action_points.append(ap2)
cached_project = CachedProject(project)
make_relative_ap_global(cached_scene, cached_project, ap2)
check_ap(ap2)
assert ap2.position == Position(1, -2, 0)
assert so1.pose
assert no1.orientation == so1.pose.orientation
make_global_ap_relative(cached_scene, cached_project, ap2, ap1.id)
assert ap2.position == Position(1, 0, 0)
assert no1.orientation == Orientation()
check_ap(ap2)
def test_make_pose_abs_3() -> None:
parent = Pose(Position(1, 1, 0), Orientation(0, 0, 0.707, 0.707))
child = Pose(Position(-1, 1, 0), Orientation())
assert make_pose_abs(parent,
child) == Pose(Position(0, 0, 0),
Orientation(0, 0, 0.707, 0.707))
def test_make_relative_ap_global_and_relative_again():
scene = Scene("s1", "s1")
scene.objects.append(
SceneObject("so1", "so1", "WhatEver",
Pose(Position(3, 0, 0), Orientation())))
project = Project("p1", "p1", "s1")
project.action_points.append(
ProjectActionPoint("ap1", "ap1", Position(-1, 0, 0), parent="so1"))
project.action_points.append(
ProjectActionPoint("ap2", "ap2", Position(-1, 0, 0), parent="ap1"))
ap3 = ProjectActionPoint("ap3", "ap3", Position(-1, 0, 0), parent="ap2")
project.action_points.append(ap3)
make_relative_ap_global(scene, project, ap3)
assert ap3.parent is None
assert ap3.position.x == .0
make_global_ap_relative(scene, project, ap3, "ap2")
assert ap3.parent == "ap2"
assert ap3.position.x == -1
ap3.parent = "something_unknown"
with pytest.raises(Arcor2Exception):
make_relative_ap_global(scene, project, ap3)
def make_position_rel(parent: Position, child: Position) -> Position:
p = Position()
p.x = child.x - parent.x
p.y = child.y - parent.y
p.z = child.z - parent.z
return p
def test_get_value() -> None:
p = Pose(Position(1, 2, 3), Orientation(1, 0, 0, 0))
scene = Scene("s1")
obj = SceneObject("test_name", TestObject.__name__)
scene.objects.append(obj)
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position(1, 0, 0))
project.action_points.append(ap1)
ap2 = ActionPoint("ap2", Position(), parent=ap1.id)
project.action_points.append(ap2)
ori1 = NamedOrientation("ori1", p.orientation)
ap2.orientations.append(ori1)
invalid_param_name = "invalid_param"
ac1 = Action(
"ac1",
f"{obj.id}/{TestObject.action.__name__}",
parameters=[
ActionParameter(param_name, PosePlugin.type_name(),
json.dumps(ori1.id)),
ActionParameter(invalid_param_name, PosePlugin.type_name(),
json.dumps("non_sense")),
],
)
ap1.actions.append(ac1)
cscene = CachedScene(scene)
cproject = CachedProject(project)
with pytest.raises(Arcor2Exception):
PosePlugin.parameter_value(type_defs, cscene, cproject, ac1.id,
"non_sense")
with pytest.raises(Arcor2Exception):
PosePlugin.parameter_value(type_defs, cscene, cproject, "non_sense",
param_name)
with pytest.raises(ParameterPluginException):
PosePlugin.parameter_value(type_defs, cscene, cproject, ac1.id,
invalid_param_name)
value = PosePlugin.parameter_value(type_defs, cscene, cproject, ac1.id,
param_name)
exe_value = PosePlugin.parameter_execution_value(type_defs, cscene,
cproject, ac1.id,
param_name)
assert value == value
assert value != exe_value
def test_blind_branch() -> None:
scene = Scene("s1")
obj = SceneObject("test_name", Test.__name__)
scene.objects.append(obj)
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position())
project.action_points.append(ap1)
ac1 = Action("ac1", f"{obj.id}/test", flows=[Flow(outputs=["bool_res"])])
ap1.actions.append(ac1)
ac2 = Action("ac2", f"{obj.id}/test", flows=[Flow()])
ap1.actions.append(ac2)
ac3 = Action("ac3", f"{obj.id}/test", flows=[Flow()])
ap1.actions.append(ac3)
ac4 = Action("ac4", f"{obj.id}/test", flows=[Flow()])
ap1.actions.append(ac4)
project.logic.append(LogicItem(LogicItem.START, ac1.id))
project.logic.append(LogicItem(ac1.id, ac2.id, ProjectLogicIf(f"{ac1.id}/default/0", json.dumps(True))))
project.logic.append(LogicItem(ac1.id, ac3.id, ProjectLogicIf(f"{ac1.id}/default/0", json.dumps(False))))
project.logic.append(LogicItem(ac2.id, ac4.id))
project.logic.append(LogicItem(ac4.id, LogicItem.END))
with pytest.raises(SourceException, match=f"Action {ac3.name} has no outputs."):
program_src({Test.__name__: Test}, CachedProject(project), CachedScene(scene))
def test_constant() -> None:
scene = Scene("s1")
obj = SceneObject("test_name", Test.__name__)
scene.objects.append(obj)
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position())
project.action_points.append(ap1)
const_value = 1234
const = ProjectParameter("int_const", "integer", json.dumps(const_value))
project.parameters.append(const)
ac1 = Action(
"ac1",
f"{obj.id}/test_par",
flows=[Flow()],
parameters=[
ActionParameter("param", ActionParameter.TypeEnum.CONSTANT,
json.dumps(const.id))
],
)
ap1.actions.append(ac1)
project.logic.append(LogicItem(LogicItem.START, ac1.id))
project.logic.append(LogicItem(ac1.id, LogicItem.END))
src = program_src({Test.__name__: Test}, CachedProject(project),
CachedScene(scene))
assert f"{const.name} = {const_value}" in src
assert f"test_name.{Test.test_par.__name__}({const.name}, an='ac1')" in src
def test_global_aps_cls() -> None:
proj = Project("test", "scene_id")
pos = Position(1, 2, 3)
ap1 = ActionPoint("ap1", pos)
ap1_o1 = NamedOrientation("o1", Orientation(0.707, 0, 0, 0.707))
ap1.orientations.append(ap1_o1)
ap1_j1 = ProjectRobotJoints("j1", "robot", [Joint("whatever", 1234)])
ap1.robot_joints.append(ap1_j1)
proj.action_points.append(ap1)
os.environ["ARCOR2_PROJECT_PATH"] = "/tmp"
import arcor2.resources # noqa
my_name = "my_module"
my_spec = importlib.util.spec_from_loader(my_name, loader=None)
my_module = importlib.util.module_from_spec(my_spec)
cproj = CachedProject(proj)
src = global_action_points_class(cproj)
exec(src, my_module.__dict__)
sys.modules["my_module"] = my_module
aps = my_module.ActionPoints(SimResources(cproj)) # type: ignore
assert aps.ap1.position == pos
assert aps.ap1.position is not pos
assert aps.ap1.poses.o1 == Pose(ap1.position, ap1_o1.orientation)
assert aps.ap1.poses.o1.orientation is not ap1_o1.orientation
assert aps.ap1.joints.j1 == ap1_j1
assert aps.ap1.joints.j1 is not ap1_j1
def test_make_relative_ap_global_and_relative_again() -> None:
scene = Scene("s1")
so1 = SceneObject("so1", "WhatEver", Pose(Position(3, 0, 0),
Orientation()))
scene.objects.append(so1)
cached_scene = CachedScene(scene)
project = Project("p1", scene.id)
ap1 = ActionPoint("ap1", Position(-1, 0, 0), parent=so1.id)
project.action_points.append(ap1)
ap2 = ActionPoint("ap2", Position(-1, 0, 0), parent=ap1.id)
project.action_points.append(ap2)
ap3 = ActionPoint(
"ap3",
Position(-1, 0, 0),
parent=ap2.id,
orientations=[NamedOrientation("bla", random_orientation())])
project.action_points.append(ap3)
cached_project = CachedProject(project)
assert ap3.parent
ap3_parent = get_parent_pose(cached_scene, cached_project, ap3.parent)
assert Pose(ap2.position, Orientation()) == ap3_parent.pose
assert ap3_parent.parent_id == ap1.id
make_relative_ap_global(cached_scene, cached_project, ap3)
check_ap(ap3)
assert ap3.parent is None
assert ap3.position.x == 0.0 # type: ignore
make_global_ap_relative(cached_scene, cached_project, ap3, ap2.id)
check_ap(ap3)
assert ap3.parent == ap2.id
assert ap3.position.x == -1
ap3.parent = "something_unknown"
with pytest.raises(Arcor2Exception):
make_relative_ap_global(cached_scene, cached_project, ap3)
def project() -> UpdateableCachedProject:
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position())
project.action_points.append(ap1)
ap1.actions.append(ac1)
ap1.actions.append(ac2)
ap1.actions.append(ac3)
ap1.actions.append(ac4)
return UpdateableCachedProject(project)
def test_patch_object_actions(monkeypatch, capsys) -> None:
class MyObject(Generic):
def action(self, pose: Pose, *, an: Optional[str] = None) -> None:
pass
action.__action__ = ActionMetadata() # type: ignore
# @action tries to read from stdin
sio = io.StringIO()
sio.fileno = lambda: 0 # type: ignore # fake whatever fileno
monkeypatch.setattr("sys.stdin", sio)
obj_id = "123"
pose = Pose(Position(0, 0, 0), Orientation(1, 0, 0, 0))
setattr(pose, AP_ID_ATTR,
"pose") # set pose id (simulate pose declaration in scene json)
my_obj = MyObject(obj_id, "")
my_obj.action(pose)
assert action._executed_action is None
out_before, _ = capsys.readouterr()
assert not out_before
patch_object_actions(MyObject)
setattr(
MyObject, ACTION_NAME_ID_MAPPING_ATTR,
{"name": "id"}) # this simulates what patch_with_action_mapping does
my_obj.action(pose, an="name")
assert action._executed_action is None
out_after, _ = capsys.readouterr()
arr = out_after.strip().split("\n")
assert len(arr) == 2
before_evt = ActionStateBefore.from_json(arr[0])
after_evt = ActionStateAfter.from_json(arr[1])
assert before_evt.data.action_id == "id"
assert after_evt.data.action_id == "id"
assert before_evt.data.action_point_ids is not None
assert "pose" in before_evt.data.action_point_ids
with pytest.raises(Arcor2Exception):
my_obj.action(pose)
assert action._executed_action is None
with pytest.raises(Arcor2Exception):
my_obj.action(pose, an="unknown_action_name")
assert action._executed_action is None
def estimate_camera_pose(camera_matrix: List[List[float]],
dist_matrix: List[float], image: Image.Image,
marker_size: float) -> Dict[int, Pose]:
camera_matrix_arr, dist_matrix_arr, gray, corners, ids = detect_corners(
camera_matrix, dist_matrix, image, refine=True)
ret: Dict[int, Pose] = {}
if np.all(ids is None):
return ret
# TODO do not perform estimation for un-configured markers
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, marker_size,
camera_matrix_arr,
dist_matrix_arr)
rvec = rvec.reshape(len(ids), 3)
tvec = tvec.reshape(len(ids), 3)
if __debug__:
backtorgb = cv2.cvtColor(gray, cv2.COLOR_GRAY2RGB)
aruco.drawDetectedMarkers(backtorgb,
corners) # Draw A square around the markers
for idx in range(len(ids)):
aruco.drawAxis(backtorgb, camera_matrix_arr, dist_matrix_arr,
rvec[idx], tvec[idx], 0.15)
cv2.imwrite("marker.jpg", backtorgb)
for idx, mid in enumerate(ids):
# convert pose of the marker wrt camera to pose of camera wrt marker
# based on https://stackoverflow.com/a/51515560/3142796
marker_rot_matrix, _ = cv2.Rodrigues(rvec[idx])
assert np.allclose(np.linalg.inv(marker_rot_matrix),
marker_rot_matrix.transpose())
assert math.isclose(np.linalg.det(marker_rot_matrix), 1)
camera_rot_matrix = marker_rot_matrix.transpose()
camera_trans_vector = np.matmul(-camera_rot_matrix,
tvec[idx].reshape(3, 1)).flatten()
o = Orientation.from_quaternion(
quaternion.from_rotation_matrix(camera_rot_matrix))
ret[mid[0]] = Pose(
Position(camera_trans_vector[0], camera_trans_vector[1],
camera_trans_vector[2]), o)
return ret
def _make_global_ap_relative(parent_id: str) -> None:
parent = get_parent_pose(scene, project, parent_id)
if parent.parent_id:
_make_global_ap_relative(parent.parent_id)
ap.position = make_pose_rel(parent.pose, Pose(ap.position, Orientation())).position
for ori in project.ap_orientations(ap.id):
ori.orientation = make_pose_rel(parent.pose, Pose(Position(), ori.orientation)).orientation
if parent.pose.orientation != Orientation():
_update_childs(parent, ap.id)
def test_get_value() -> None:
scene = Scene("s1", "s1")
obj = SceneObject("test_name", TestObject.__name__)
prj = ProjectRobotJoints("name", obj.id, [Joint("name", 0.333)])
scene.objects.append(obj)
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position(1, 0, 0))
ap1.robot_joints.append(prj)
project.action_points.append(ap1)
invalid_param_name = "invalid_param"
act = Action(
"ac1",
f"{obj.id}/{TestObject.action.__name__}",
parameters=[
ActionParameter(param_name, JointsPlugin.type_name(),
json.dumps(prj.id)),
ActionParameter(invalid_param_name, JointsPlugin.type_name(),
json.dumps("non_sense")),
],
)
ap1.actions.append(act)
cscene = CachedScene(scene)
cproject = CachedProject(project)
with pytest.raises(Arcor2Exception):
JointsPlugin.parameter_value(type_defs, cscene, cproject, act.id,
"non_sense")
with pytest.raises(Arcor2Exception):
JointsPlugin.parameter_value(type_defs, cscene, cproject, "non_sense",
param_name)
with pytest.raises(ParameterPluginException):
JointsPlugin.parameter_value(type_defs, cscene, cproject, act.id,
invalid_param_name)
value = JointsPlugin.parameter_value(type_defs, cscene, cproject, act.id,
param_name)
exe_value = JointsPlugin.parameter_execution_value(type_defs, cscene,
cproject, act.id,
param_name)
assert value == value
assert value == exe_value
def test_get_value() -> None:
img = Image.new("RGB", (320, 240))
scene = Scene("s1", "s1")
obj = SceneObject("test_name", TestObject.__name__)
scene.objects.append(obj)
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position(1, 0, 0))
project.action_points.append(ap1)
invalid_param_name = "invalid_param"
ac1 = Action(
"ac1",
f"{obj.id}/{TestObject.action.__name__}",
parameters=[
ActionParameter(param_name, ImagePlugin.type_name(),
ImagePlugin.value_to_json(img)),
ActionParameter(invalid_param_name, ImagePlugin.type_name(),
json.dumps("non_sense")),
],
)
ap1.actions.append(ac1)
cscene = CachedScene(scene)
cproject = CachedProject(project)
with pytest.raises(Arcor2Exception):
ImagePlugin.parameter_value(type_defs, cscene, cproject, ac1.id,
"non_sense")
with pytest.raises(Arcor2Exception):
ImagePlugin.parameter_value(type_defs, cscene, cproject, "non_sense",
param_name)
with pytest.raises(ParameterPluginException):
ImagePlugin.parameter_value(type_defs, cscene, cproject, ac1.id,
invalid_param_name)
value = ImagePlugin.parameter_value(type_defs, cscene, cproject, ac1.id,
param_name)
exe_value = ImagePlugin.parameter_execution_value(type_defs, cscene,
cproject, ac1.id,
param_name)
assert value == value
assert value == exe_value
def message_to_pose(message: str) -> Pose:
tokens = message.split()
if len(tokens) != 7:
raise YumiException("Invalid format for pose! Got:\n{0}".format(message))
pose_vals = [float(token) for token in tokens]
q = pose_vals[3:]
t = pose_vals[:3]
try:
return Pose(
Position(t[0], t[1], t[2]) * MM_TO_METERS, Orientation.from_quaternion(quaternion.from_float_array(q))
)
except (IndexError, ValueError):
raise YumiException("Invalid pose.")
def _make_relative_ap_global(_ap: BareActionPoint) -> None:
if not _ap.parent:
return
parent = get_parent_pose(scene, project, _ap.parent)
if parent.pose.orientation != Orientation():
_update_childs(parent, _ap.id)
_ap.position = make_pose_abs(parent.pose, Pose(_ap.position, Orientation())).position
for ori in project.ap_orientations(_ap.id):
ori.orientation = make_pose_abs(parent.pose, Pose(Position(), ori.orientation)).orientation
_ap.parent = parent.parent_id
_make_relative_ap_global(_ap)
def test_get_value(self, val: str) -> None:
scene = Scene("s1")
obj = SceneObject("test_name", TestObject.__name__)
scene.objects.append(obj)
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position())
project.action_points.append(ap1)
invalid_param_name = "invalid_param"
ac1 = Action(
"ac1",
f"{obj.id}/{TestObject.action.__name__}",
parameters=[
ActionParameter(param_name, StringPlugin.type_name(),
StringPlugin.value_to_json(val)),
ActionParameter(invalid_param_name, StringPlugin.type_name(),
json.dumps(666)),
],
)
ap1.actions.append(ac1)
cscene = CachedScene(scene)
cproject = CachedProject(project)
with pytest.raises(Arcor2Exception):
StringPlugin.parameter_value({}, cscene, cproject, ac1.id,
"non_sense")
with pytest.raises(Arcor2Exception):
StringPlugin.parameter_value({}, cscene, cproject, "non_sense",
param_name)
with pytest.raises(ParameterPluginException):
StringPlugin.parameter_value({}, cscene, cproject, ac1.id,
invalid_param_name)
value = StringPlugin.parameter_value({}, cscene, cproject, ac1.id,
param_name)
exe_value = StringPlugin.parameter_execution_value({}, cscene,
cproject, ac1.id,
param_name)
assert value == val
assert value == exe_value
def test_patch_object_actions_without_mapping(monkeypatch, capsys) -> None:
class MyObject(Generic):
def action(self, pose: Pose, *, an: Optional[str] = None) -> None:
pass
action.__action__ = ActionMetadata() # type: ignore
# @action tries to read from stdin
sio = io.StringIO()
sio.fileno = lambda: 0 # type: ignore # fake whatever fileno
monkeypatch.setattr("sys.stdin", sio)
obj_id = "123"
pose = Pose(Position(0, 0, 0), Orientation(1, 0, 0, 0))
setattr(pose, AP_ID_ATTR,
"pose") # set pose id (simulate pose declaration in scene json)
my_obj = MyObject(obj_id, "")
patch_object_actions(MyObject) # no mapping given
my_obj.action(pose) # this should be ok
assert action._executed_action is None
out_after, _ = capsys.readouterr()
assert out_after
arr = out_after.strip().split("\n")
assert len(arr) == 1
before_evt = ActionStateBefore.from_json(arr[0])
assert before_evt.data.action_id is None
assert before_evt.data.parameters
assert Pose.from_json(before_evt.data.parameters[0]) == pose
assert before_evt.data.action_point_ids is not None
assert "pose" in before_evt.data.action_point_ids
# the 'an' should be just ignored
# we are testing here that second execution of action is fine
my_obj.action(pose, an="whatever")
assert action._executed_action is None
out_after2, _ = capsys.readouterr()
assert out_after2 == out_after
def test_prev_result() -> None:
scene = Scene("s1")
obj = SceneObject("test_name", Test.__name__)
scene.objects.append(obj)
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position())
project.action_points.append(ap1)
ac1 = Action("ac1",
f"{obj.id}/{Test.get_int.__name__}",
flows=[Flow(outputs=["res"])])
ap1.actions.append(ac1)
ac2 = Action(
"ac2",
f"{obj.id}/{Test.test_par.__name__}",
flows=[Flow()],
parameters=[
ActionParameter("param", ActionParameter.TypeEnum.LINK,
json.dumps(f"{ac1.id}/default/0"))
],
)
ap1.actions.append(ac2)
project.logic.append(LogicItem(LogicItem.START, ac1.id))
project.logic.append(LogicItem(ac1.id, ac2.id))
project.logic.append(LogicItem(ac2.id, LogicItem.END))
src = program_src({Test.__name__: Test}, CachedProject(project),
CachedScene(scene))
assert f"res = test_name.{Test.get_int.__name__}(an='{ac1.name}')" in src
assert f"test_name.{Test.test_par.__name__}(res, an='{ac2.name}')" in src
# test wrong order of logic
project.logic.clear()
project.logic.append(LogicItem(LogicItem.START, ac2.id))
project.logic.append(LogicItem(ac2.id, ac1.id))
project.logic.append(LogicItem(ac1.id, LogicItem.END))
with pytest.raises(SourceException):
program_src({Test.__name__: Test}, CachedProject(project),
CachedScene(scene))
def main() -> None:
dm = DobotMagician(
"id", "name", Pose(Position(1, -1, -3),
Orientation(0, 0, 0.707, 0.707)),
DobotSettings("/dev/ttyUSB0"))
orig_pose = dm.get_end_effector_pose("default")
val = float(sys.argv[1])
updated_pose = copy.deepcopy(orig_pose)
updated_pose.position.x += val
dm.move_to_pose("", updated_pose, 1.0)
new_pose = dm.get_end_effector_pose("default")
print(new_pose.position.x - orig_pose.position.x - val)
def forward_kinematics(self, joints: List[Joint]) -> Pose:
"""Computes forward kinematics.
Outputs absolute pose.
Inspired by DobotKinematics.py from open-dobot project.
:param end_effector_id: Target end effector name
:param joints: Input joint values
:return: Pose of the given end effector
"""
self.validate_joints(joints)
j1 = joints[0].value
j2 = joints[1].value
j3 = joints[2].value
sj = j2 + j3
radius = (
self.link_2_length * math.cos(j2 - math.pi / 2) + self.link_3_length * math.cos(sj) + self.link_4_length
)
x = radius * math.cos(j1)
y = radius * math.sin(j1)
z = (
self.link_2_length * math.cos(j2)
+ self.link_3_length * math.cos(sj + math.pi / 2)
- self.end_effector_length
)
pose = Pose(
Position(x, y, z),
Orientation.from_quaternion(quaternion.from_euler_angles(0, math.pi, joints[-1].value + j1)),
)
if __debug__:
self._check_orientation(pose)
return tr.make_pose_abs(self.pose, pose)
def make_relative_ap_global(scene: CScene, project: CProject,
ap: BareActionPoint) -> None:
"""Transforms (in place) relative AP into a global one.
:param scene:
:param project:
:param ap:
:return:
"""
if not ap.parent:
return
parent = get_parent_pose(scene, project, ap.parent)
ap.position = make_pose_abs(parent.pose, Pose(ap.position,
Orientation())).position
for ori in project.ap_orientations(ap.id):
ori.orientation = make_pose_abs(parent.pose,
Pose(Position(),
ori.orientation)).orientation
ap.parent = parent.parent_id
make_relative_ap_global(scene, project, ap)
import pytest
from arcor2.data.common import Orientation, Pose, Position
from arcor2_fit_demo.object_types.abstract_dobot import DobotException, DobotSettings
from arcor2_fit_demo.object_types.dobot_magician import DobotMagician
URL = os.getenv("ARCOR2_DOBOT_URL", "http://*****:*****@pytest.mark.skip(reason="Temporarily disabled (need to run the service).")
@pytest.mark.parametrize(
"pose",
[
Pose(
Position(0.24027805415562356, -0.033064804793467024,
-0.10107283020019531),
Orientation(-0.06832257459695434, 0.9976632827765306,
-4.18355111448332e-18, 6.108925729395595e-17),
),
Pose(
Position(0.2570099061465494, -0.03512534022111209,
0.021290023803710936),
Orientation(-0.0678616466931505, 0.9976947413453146,
-4.155327420381767e-18, 6.10911835757343e-17),
),
Pose(
Position(0.21265726282945766, -0.04578391051491453,
0.10375743103027343),
Orientation(-0.10583019614908788, 0.9943842162781173,
-6.480230548355851e-18, 6.088847237938229e-17),
),
def test_update_object_pose(start_processes: None, ars: ARServer) -> None:
upload_def(Box, BoxModel("Box", 0.1, 0.1, 0.1))
upload_def(DummyMultiArmRobot)
test = "test"
event(ars, events.c.ShowMainScreen)
assert ars.call_rpc(
rpc.s.NewScene.Request(get_id(), rpc.s.NewScene.Request.Args(test)),
rpc.s.NewScene.Response).result
assert len(event(ars, events.o.ChangedObjectTypes).data) == 2
event(ars, events.s.OpenScene)
event(ars, events.s.SceneState)
assert ars.call_rpc(
rpc.s.AddObjectToScene.Request(
get_id(),
rpc.s.AddObjectToScene.Request.Args("obj", Box.__name__, Pose())),
rpc.s.AddObjectToScene.Response,
).result
scene_obj = event(ars, events.s.SceneObjectChanged).data
assert ars.call_rpc(
rpc.s.AddObjectToScene.Request(
get_id(),
rpc.s.AddObjectToScene.Request.Args("robot",
DummyMultiArmRobot.__name__,
Pose())),
rpc.s.AddObjectToScene.Response,
).result
scene_robot = event(ars, events.s.SceneObjectChanged).data
new_pose = Pose(Position(1))
lock_object(ars, scene_obj.id)
assert ars.call_rpc(
rpc.s.UpdateObjectPose.Request(
get_id(),
rpc.s.UpdateObjectPose.Request.Args(scene_obj.id, new_pose)),
rpc.s.UpdateObjectPose.Response,
).result
assert event(ars, events.s.SceneObjectChanged).data.pose == new_pose
unlock_object(ars, scene_obj.id)
lock_object(ars, scene_robot.id)
assert ars.call_rpc(
rpc.s.UpdateObjectPose.Request(
get_id(),
rpc.s.UpdateObjectPose.Request.Args(scene_robot.id, new_pose)),
rpc.s.UpdateObjectPose.Response,
).result
assert event(ars, events.s.SceneObjectChanged).data.pose == new_pose
unlock_object(ars, scene_robot.id)
assert ars.call_rpc(
rpc.s.StartScene.Request(get_id()),
rpc.s.StartScene.Response,
).result
assert event(ars, events.s.SceneState
).data.state == events.s.SceneState.Data.StateEnum.Starting
assert event(ars, events.s.SceneState
).data.state == events.s.SceneState.Data.StateEnum.Started
lock_object(ars, scene_obj.id)
# with started scene, it should still be possible to manipulate the object
assert ars.call_rpc(
rpc.s.UpdateObjectPose.Request(
get_id(),
rpc.s.UpdateObjectPose.Request.Args(scene_obj.id, Pose())),
rpc.s.UpdateObjectPose.Response,
).result
assert event(ars, events.s.SceneObjectChanged).data.pose == Pose()
unlock_object(ars, scene_obj.id)
lock_object(ars, scene_robot.id)
# ...but it should not be possible to manipulate the robot
assert not ars.call_rpc(
rpc.s.UpdateObjectPose.Request(
get_id(),
rpc.s.UpdateObjectPose.Request.Args(scene_robot.id, Pose())),
rpc.s.UpdateObjectPose.Response,
).result
unlock_object(ars, scene_robot.id)
def test_branched_output_2() -> None:
scene = Scene("s1")
obj = SceneObject("test_name", Test.__name__)
scene.objects.append(obj)
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position())
project.action_points.append(ap1)
ac1 = Action("ac1", f"{obj.id}/test", flows=[Flow(outputs=["bool_res"])])
ap1.actions.append(ac1)
ac2 = Action("ac2", f"{obj.id}/test", flows=[Flow()])
ap1.actions.append(ac2)
ac3 = Action("ac3", f"{obj.id}/test", flows=[Flow()])
ap1.actions.append(ac3)
ac4 = Action("ac4", f"{obj.id}/test", flows=[Flow(outputs=["bool2_res"])])
ap1.actions.append(ac4)
ac5 = Action("ac5", f"{obj.id}/test", flows=[Flow()])
ap1.actions.append(ac5)
ac6 = Action("ac6", f"{obj.id}/test", flows=[Flow()])
ap1.actions.append(ac6)
project.logic.append(LogicItem(LogicItem.START, ac1.id))
project.logic.append(
LogicItem(ac1.id, ac2.id,
ProjectLogicIf(f"{ac1.id}/default/0", json.dumps(True))))
project.logic.append(
LogicItem(ac1.id, ac4.id,
ProjectLogicIf(f"{ac1.id}/default/0", json.dumps(False))))
project.logic.append(LogicItem(ac2.id, ac3.id))
project.logic.append(LogicItem(ac3.id, ac6.id))
project.logic.append(
LogicItem(ac4.id, ac5.id,
ProjectLogicIf(f"{ac4.id}/default/0", json.dumps(True))))
project.logic.append(LogicItem(ac5.id, ac6.id))
project.logic.append(LogicItem(ac6.id, LogicItem.END))
project.logic.append(
LogicItem(ac4.id, LogicItem.END,
ProjectLogicIf(f"{ac4.id}/default/0", json.dumps(False))))
src = program_src({Test.__name__: Test}, CachedProject(project),
CachedScene(scene))
parse(src)
"""
bool_res = test_name.test(res.ac1)
if (bool_res == False):
bool2_res = test_name.test(res.ac4)
if (bool2_res == False):
continue
elif (bool2_res == True):
test_name.test(res.ac5)
elif (bool_res == True):
test_name.test(res.ac2)
test_name.test(res.ac3)
test_name.test(res.ac6)
"""
spl = src.splitlines()
# it has to be robust against changed order of blocks
ac1_idx = subs_index(spl, "bool_res = test_name.test(an='ac1')")
if_bool_res_false_idx = subs_index(spl, "if (bool_res == False):")
assert if_bool_res_false_idx > ac1_idx
assert cntsp(spl[ac1_idx]) == cntsp(spl[if_bool_res_false_idx])
bool2_res_idx = subs_index(spl, "bool2_res = test_name.test(an='ac4')")
assert bool2_res_idx > if_bool_res_false_idx
assert cntsp(spl[if_bool_res_false_idx]) == cntsp(spl[bool2_res_idx]) - TAB
if_bool_2_res_false_idx = subs_index(spl, "if (bool2_res == False):")
assert cntsp(spl[if_bool_2_res_false_idx]) == cntsp(spl[bool2_res_idx])
assert if_bool_2_res_false_idx > bool2_res_idx
assert "continue" in spl[if_bool_2_res_false_idx + 1]
assert cntsp(
spl[bool2_res_idx]) == cntsp(spl[if_bool_2_res_false_idx + 1]) - TAB
if_bool_2_res_true_idx = subs_index(spl, "if (bool2_res == True):")
assert if_bool_2_res_true_idx > bool2_res_idx
assert "test_name.test(an='ac5')" in spl[if_bool_2_res_true_idx + 1]
assert cntsp(spl[if_bool_2_res_true_idx]) == cntsp(
spl[if_bool_2_res_true_idx + 1]) - TAB
if_bool_res_true_idx = subs_index(spl, "if (bool_res == True):")
assert if_bool_res_true_idx > ac1_idx
assert cntsp(spl[ac1_idx]) == cntsp(spl[if_bool_res_true_idx])
assert "test_name.test(an='ac2')" in spl[if_bool_res_true_idx + 1]
assert cntsp(spl[if_bool_res_true_idx]) == cntsp(
spl[if_bool_res_true_idx + 1]) - TAB
assert "test_name.test(an='ac3')" in spl[if_bool_res_true_idx + 2]
assert cntsp(spl[if_bool_res_true_idx]) == cntsp(
spl[if_bool_res_true_idx + 2]) - TAB
ac6_idx = subs_index(spl, "test_name.test(an='ac6')")
assert cntsp(spl[ac1_idx]) == cntsp(spl[ac6_idx])
assert ac6_idx > if_bool_2_res_false_idx
assert ac6_idx > if_bool_2_res_true_idx
def get_calibration() -> RespT:
"""Get calibration (camera pose wrt. marker)
---
put:
description: Returns camera pose with respect to the origin.
tags:
- Camera
parameters:
- in: query
name: inverse
schema:
type: boolean
description: When set, the method returns pose of the origin wrt. the camera.
- in: query
name: fx
schema:
type: number
format: float
required: true
description: unique ID
- in: query
name: fy
schema:
type: number
format: float
required: true
description: unique ID
- in: query
name: cx
schema:
type: number
format: float
required: true
description: unique ID
- in: query
name: cy
schema:
type: number
format: float
required: true
description: unique ID
- in: query
name: distCoefs
schema:
type: array
items:
type: number
format: float
required: true
description: unique ID
requestBody:
content:
multipart/form-data:
schema:
type: object
required:
- image
properties:
# 'image' will be the field name in this multipart request
image:
type: string
format: binary
responses:
200:
description: Ok
content:
application/json:
schema:
$ref: EstimatedPose
404:
description: No marker found.
content:
application/json:
schema:
type: string
"""
file = request.files["image"]
camera_matrix = camera_matrix_from_request()
image = Image.open(file.stream)
dist_matrix = dist_matrix_from_request()
if _mock:
time.sleep(0.5)
quality = random.uniform(0, 1)
pose = Pose(
Position(random.uniform(-0.5, 0.5), random.uniform(-0.5, 0.5),
random.uniform(0.2, 1)))
else:
poses = estimate_camera_pose(camera_matrix, dist_matrix, image,
MARKER_SIZE)
if not poses:
return jsonify("No marker detected."), 404
quality_dict: dict[int, float] = {k: 0.0 for k, v in MARKERS.items()}
known_markers: list[tuple[Pose, float]] = []
# apply configured marker offset from origin to the detected poses
for marker_id in poses.keys():
try:
cpose = MARKERS[marker_id]
except KeyError:
logger.debug(f"Detected un-configured marker id {marker_id}.")
continue
mpose = poses[marker_id]
dist = math.sqrt(mpose.position.x**2 + mpose.position.y**2 +
mpose.position.z**2)
# the closer the theta is to pi, the higher quality we get
theta = quaternion.as_spherical_coords(
mpose.orientation.as_quaternion())[0]
ori_marker_quality = normalize(abs(theta), MIN_THETA, MAX_THETA)
# the closer the marker is, the higher quality we get
dist_marker_quality = 1.0 - normalize(dist, MIN_DIST, MAX_DIST)
marker_quality = (ori_marker_quality + dist_marker_quality) / 2
quality_dict[marker_id] = marker_quality
known_markers.append((tr.make_pose_abs(cpose,
mpose), marker_quality))
logger.debug(f"Known marker : {marker_id}")
logger.debug(f"...original pose : {poses[marker_id]}")
logger.debug(f"...transformed pose: {poses[marker_id]}")
logger.debug(f"...dist quality : {dist_marker_quality:.3f}")
logger.debug(f"...ori quality : {ori_marker_quality:.3f}")
logger.debug(f"...overall quality : {marker_quality:.3f}")
if not known_markers:
return jsonify("No known marker detected."), 404
weights = [marker[1] for marker in known_markers]
wsum = sum(weights)
if wsum <= 0:
logger.warning(f"Got invalid weights, probably bad input data.\n"
f"Camera matrix: {camera_matrix}\n"
f"Dist matrix: {dist_matrix}")
return jsonify("Invalid input data."), 500
# combine all detections
pose = Pose()
for mpose, weight in known_markers:
pose.position += mpose.position * weight
pose.position *= 1.0 / wsum
quaternions = np.array([
quaternion.as_float_array(km[0].orientation.as_quaternion())
for km in known_markers
])
pose.orientation.set_from_quaternion(
quaternion.from_float_array(
weighted_average_quaternions(quaternions, np.array(weights))))
quality = np.mean(list(quality_dict.values()))
inverse = request.args.get("inverse", default="false") == "true"
if inverse:
logger.debug("Inverting the output pose.")
pose = pose.inversed()
return jsonify(EstimatedPose(pose, quality)), 200
def test_object_parameters(start_processes: None, ars: ARServer,
scene: Scene) -> None:
assert ars.call_rpc(rpc.s.OpenScene.Request(uid(), IdArgs(scene.id)),
rpc.s.OpenScene.Response).result
event(ars, events.s.OpenScene)
assert ars.call_rpc(
rpc.s.AddObjectToScene.Request(
uid(),
rpc.s.AddObjectToScene.Request.Args("ows",
ObjectWithActions.__name__)),
rpc.s.AddObjectToScene.Response,
).result
obj = event(ars, events.s.SceneObjectChanged).data
assert obj is not None
assert ars.call_rpc(
rpc.p.NewProject.Request(
uid(), rpc.p.NewProject.Request.Args(scene.id, "Project name")),
rpc.p.NewProject.Response,
).result
event(ars, events.p.OpenProject)
assert ars.call_rpc(
rpc.p.AddActionPoint.Request(
uid(), rpc.p.AddActionPoint.Request.Args("ap", Position())),
rpc.p.AddActionPoint.Response,
).result
ap = event(ars, events.p.ActionPointChanged).data
assert ap is not None
assert ars.call_rpc(
rpc.p.AddAction.Request(
uid(),
rpc.p.AddAction.Request.Args(
ap.id,
"a1",
f"{obj.id}/{ObjectWithActions.bool_action.__name__}",
[],
[Flow(outputs=["bool_result"])],
),
),
rpc.p.AddAction.Response,
).result
a1 = event(ars, events.p.ActionChanged).data
assert a1 is not None
assert ars.call_rpc(
rpc.p.AddAction.Request(
uid(),
rpc.p.AddAction.Request.Args(
ap.id,
"a2",
f"{obj.id}/{ObjectWithActions.str_action.__name__}",
[],
[Flow()],
),
),
rpc.p.AddAction.Response,
).result
a2 = event(ars, events.p.ActionChanged).data
assert a2 is not None
add_logic_item(ars, LogicItem.START, a1.id)
add_logic_item(
ars, a1.id, a2.id,
ProjectLogicIf(f"{a1.id}/{FlowTypes.DEFAULT}/{0}", json.dumps(True)))
add_logic_item(ars, a2.id, LogicItem.END)
add_logic_item(
ars, a1.id, LogicItem.END,
ProjectLogicIf(f"{a1.id}/{FlowTypes.DEFAULT}/{0}", json.dumps(False)))
# TODO try to add some invalid connections here?
save_project(ars)
