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 make_relative_ap_global(scene: Scene, project: Project, ap: ProjectActionPoint) -> None:
"""
Transforms (in place) relative AP into a global one.
:param scene:
:param project:
:param ap:
:return:
"""
if not ap.parent:
return
if ap.parent in scene.object_ids:
old_parent_pose = scene.object(ap.parent).pose
elif ap.parent in project.action_points_ids:
old_parent_pose = Pose(project.action_point(ap.parent).position, Orientation())
else:
raise Arcor2Exception("AP has unknown parent_id.")
ap.position = make_pose_abs(old_parent_pose, Pose(ap.position, Orientation())).position
for ori in ap.orientations:
ori.orientation = make_orientation_abs(old_parent_pose.orientation, ori.orientation)
if ap.parent in project.action_points_ids:
parent_ap = project.action_point(ap.parent)
if parent_ap.parent:
ap.parent = parent_ap.parent
make_relative_ap_global(scene, project, ap)
ap.parent = None
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 make_global_ap_relative(scene: Scene, project: Project, ap: ProjectActionPoint, parent_id: str) -> None:
"""
Transforms (in place) global AP into a relative one with given parent (can be object or another AP).
:param scene:
:param project:
:param ap:
:param parent_id:
:return:
"""
assert project.scene_id == scene.id
if parent_id in scene.object_ids:
new_parent_pose = scene.object(parent_id).pose
elif parent_id in project.action_points_ids:
parent_ap = project.action_point(parent_id)
if parent_ap.parent:
make_global_ap_relative(scene, project, ap, parent_ap.parent)
new_parent_pose = Pose(parent_ap.position, Orientation())
else:
raise Arcor2Exception("Unknown parent_id.")
ap.position = make_pose_rel(new_parent_pose, Pose(ap.position, Orientation())).position
for ori in ap.orientations:
ori.orientation = make_orientation_rel(new_parent_pose.orientation, ori.orientation)
ap.parent = parent_id
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_generic_with_pose(start_processes: None) -> None:
obj = GenericWithPose("id", "name", Pose(), Box("boxId", 0.1, 0.1, 0.1))
assert obj.id in scene_service.collision_ids()
obj.pose = Pose()
obj.enabled = False
assert obj.id not in scene_service.collision_ids()
obj.enabled = True
assert obj.id in scene_service.collision_ids()
obj.cleanup()
assert not scene_service.collision_ids()
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_generic_with_pose(start_processes: None) -> None:
obj = CollisionObject("id", "name", Pose(), Box("boxId", 0.1, 0.1, 0.1))
assert obj.id in scene_service.collision_ids()
obj.pose = Pose()
obj.enabled = False
assert obj.id not in scene_service.collision_ids()
obj.enabled = True
assert obj.id in scene_service.collision_ids()
scene_service.start()
assert obj.id in scene_service.collision_ids()
scene_service.stop() # after stop, all collisions are removed by the Scene service
assert not scene_service.collision_ids()
def main() -> None:
dm = DobotMagician("id", "name", Pose(), DobotSettings("/dev/ttyUSB0"))
joints = dm.robot_joints()
pose = dm.get_end_effector_pose("default")
print(pose)
print(joints)
print(quaternion.as_euler_angles(pose.orientation.as_quaternion()))
print("--- Forward Kinematics -----------------------------")
fk_pose = dm.forward_kinematics("", joints)
dx = pose.position.x - fk_pose.position.x
dy = pose.position.y - fk_pose.position.y
dz = pose.position.z - fk_pose.position.z
print("Position error: {:+.09f}".format(math.sqrt(dx**2 + dy**2 + dz**2)))
print("dx: {:+.06f}".format(dx))
print("dy: {:+.06f}".format(dy))
print("dz: {:+.06f}".format(dz))
print(fk_pose.orientation)
print("--- Inverse Kinematics -----------------------------")
ik_joints = dm.inverse_kinematics("", pose)
assert len(ik_joints) == len(joints)
for idx, (joint, ik_joint) in enumerate(zip(joints, ik_joints)):
print("j{}: {:+.06f}".format(idx + 1, joint.value - ik_joint.value))
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 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 main() -> None:
yumi = YuMi("", "", Pose(), YumiSettings("192.168.104.107"))
left_joints = yumi.robot_joints(arm_id="left")
right_joints = yumi.robot_joints(arm_id="right")
print(yumi.forward_kinematics("", left_joints, "left"))
print(yumi.forward_kinematics("", right_joints, "right"))
pose_l = yumi.get_end_effector_pose("", "left")
pose_r = yumi.get_end_effector_pose("", "right")
print(yumi.inverse_kinematics("", pose_l, None, False, arm_id="left"))
print(yumi.inverse_kinematics("", pose_r, None, False, arm_id="right"))
pose_l_2 = copy.deepcopy(pose_l)
pose_r_2 = copy.deepcopy(pose_r)
pose_l_2.position.z += 0.01
pose_r_2.position.z += 0.01
while True:
yumi.move_to_pose("", pose_l, 0.5, arm_id="left")
yumi.move_to_pose("", pose_l_2, 0.5, arm_id="left")
yumi.move_to_pose("", pose_r, 0.5, arm_id="right")
yumi.move_to_pose("", pose_r_2, 0.5, arm_id="right")
def make_pose_rel_to_parent(scene: Scene, project: Project, pose: Pose, parent_id: str) -> Pose:
"""
Transforms global Pose into Pose that is relative to a given parent (can be object or AP).
:param scene:
:param project:
:param pose:
:param parent_id:
:return:
"""
if parent_id in scene.object_ids:
parent_pose = scene.object(parent_id).pose
elif parent_id in project.action_points_ids:
parent_ap = project.action_point(parent_id)
if parent_ap.parent:
pose = make_pose_rel_to_parent(scene, project, pose, parent_ap.parent)
parent_pose = Pose(parent_ap.position, Orientation())
else:
raise Arcor2Exception("Unknown parent_id.")
return make_pose_rel(parent_pose, 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_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 parameter_value(
cls, type_defs: TypesDict, scene: CScene, project: CProject, action_id: str, parameter_id: str
) -> Pose:
try:
ap, ori = project.bare_ap_and_orientation(cls.orientation_id(project, action_id, parameter_id))
except Arcor2Exception as e:
raise ParameterPluginException("Failed to get scene/project data.") from e
return Pose(ap.position, ori.orientation)
def value(cls, type_defs: TypesDict, scene: Scene, project: Project,
action_id: str, parameter_id: str) -> Pose:
action = project.action(action_id)
param = action.parameter(parameter_id)
ori_id: str = cls.param_value(param)
ap, ori = project.ap_and_orientation(ori_id)
return Pose(ap.position, ori.orientation)
def __init__(self, obj_id: str, name: str, pose: Pose, settings: Optional[Settings] = None) -> None:
super().__init__(obj_id, name, pose, settings)
self._hand_teaching: dict[str, bool] = {self.Arms.left: False, self.Arms.right: False}
self._poses: dict[str, dict[str, Pose]] = {}
for arm, eefs in self.EEF.items():
self._poses[arm] = {}
for eef in eefs:
self._poses[arm][eef] = Pose()
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 make_pose_rel(parent: Pose, child: Pose) -> Pose:
"""
:param parent: e.g. scene object
:param child: e.g. action point
:return: relative pose
"""
p = Pose()
p.position = make_position_rel(parent.position, child.position).rotated(parent.orientation, True)
p.orientation = make_orientation_rel(parent.orientation, child.orientation)
return p
def make_pose_rel(parent: Pose, child: Pose) -> Pose:
"""
:param parent: e.g. scene object
:param child: e.g. action point
:return: relative pose
"""
return Pose(
(child.position - parent.position).rotated(parent.orientation, inverse=True),
Orientation.from_quaternion(parent.orientation.as_quaternion().inverse() * child.orientation.as_quaternion()),
)
def make_pose_abs(parent: Pose, child: Pose) -> Pose:
"""
:param parent: e.g. scene object
:param child: e.g. action point
:return: absolute pose
"""
return Pose(
(child.position.rotated(parent.orientation) + parent.position),
Orientation.from_quaternion(parent.orientation.as_quaternion() * child.orientation.as_quaternion()),
)
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 main() -> None:
kinect = KinectAzure("", "", Pose(), settings=UrlSettings("http://localhost:5016"))
# print(kinect.color_camera_params)
assert kinect.color_camera_params
ci_start = time.monotonic()
color_image = kinect.color_image()
print(f"Time to get color_image: {time.monotonic()-ci_start}")
calib_start = time.monotonic()
print(calib_client.estimate_camera_pose(kinect.color_camera_params, color_image))
print(f"Time to get calibration: {time.monotonic()-calib_start}")
def forward_kinematics(self, end_effector_id: str, joints: list[Joint], arm_id: Optional[str] = None) -> Pose:
"""Computes forward kinematics.
:param end_effector_id: Target end effector name
:param joints: Input joint values
:return: Pose of the given end effector
"""
if end_effector_id not in self.get_end_effectors_ids(arm_id):
raise Arcor2Exception("Unknown end effector.")
return Pose()
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_pose_abs(parent: Pose, child: Pose) -> Pose:
"""
:param parent: e.g. scene object
:param child: e.g. action point
:return: absolute pose
"""
p = Pose()
p.position = child.position.rotated(parent.orientation)
p.position = make_position_abs(parent.position, p.position)
p.orientation = make_orientation_abs(parent.orientation, child.orientation)
return p
def parameter_value(
cls, type_defs: TypesDict, scene: CScene, project: CProject, action_id: str, parameter_id: str
) -> List[Pose]:
ret: List[Pose] = []
ap, action = project.action_point_and_action(action_id)
parameter = action.parameter(parameter_id)
for orientation_id in cls._param_value_list(parameter):
ret.append(Pose(ap.position, project.orientation(orientation_id).orientation))
return ret
def execution_value(cls, type_defs: TypesDict, scene: Scene,
project: Project, action_id: str,
parameter_id: str) -> Pose:
action = project.action(action_id)
param = action.parameter(parameter_id)
ori_id: str = cls.param_value(param)
ap, _ = project.ap_and_orientation(ori_id)
copy_of_ap = copy.deepcopy(ap)
tr.make_relative_ap_global(scene, project, copy_of_ap)
return Pose(copy_of_ap.position, ap.orientation(ori_id).orientation)
def main() -> None:
# robots
aubo = Aubo(uid("rbt"), "Whatever", Pose(), RobotSettings("http://127.0.0.1:13000", "aubo"))
simatic = Aubo(uid("rbt"), "Whatever", Pose(), RobotSettings("http://127.0.0.1:13001", "simatic"))
# objects with pose, with 'System' and 'Configurations' controllers
barcode = Barcode(uid("srv"), "Whatever", Pose(), settings=Settings("http://127.0.0.1:14000", "simulator"))
search = Search(uid("srv"), "Whatever", Pose(), settings=Settings("http://127.0.0.1:12000", "simulator"))
ict = Ict(uid("srv"), "Whatever", Pose(), settings=Settings("http://127.0.0.1:19000", "simulator"))
# objects without pose, without 'System' and 'Configurations' controllers
interaction = Interaction(uid("srv"), "Whatever", SimpleSettings("http://127.0.0.1:17000"))
statistic = Statistic(uid("srv"), "Whatever", SimpleSettings("http://127.0.0.1:16000"))
# Add @action decorator to all actions of each object using patch_object_actions.
# It has to be called exactly once for each type!
# Certain functions of Execution unit (as e.g. pausing script execution) would not work without this step.
# Note: in a generated script, this is done within the Resources context manager.
# Side effect: a lot of JSON data will be printed out to the console when running the script manually.
patch_object_actions(Aubo)
patch_object_actions(Barcode)
patch_object_actions(Search)
patch_object_actions(Ict)
patch_object_actions(Interaction)
patch_object_actions(Statistic)
scene_service.delete_all_collisions()
scene_service.upsert_collision(Box("box_id", 0.1, 0.1, 0.1), Pose())
scene_service.start() # this is normally done by auto-generated Resources class
aubo.move("suction", Pose(), MoveTypeEnum.SIMPLE, safe=False)
simatic.set_joints(ProjectRobotJoints("", "", [Joint("x", 0), Joint("y", 0)]), MoveTypeEnum.SIMPLE)
barcode.scan()
search.set_search_parameters(SearchEngineParameters(search_log_level=SearchLogLevel(LogLevel.DEBUG)))
search.grab_image()
interaction.add_notification("Test", NotificationLevelEnum.INFO)
try:
statistic.get_groups()
except rest.RestHttpException as e:
# service returned error code
print(f"The error code is {e.error_code}.")
except rest.RestException as e:
# request totally failed (timeout, connection error, etc)
print(str(e))
if ict.ready():
test = ict.test("OK")
print(test)
scene_service.stop() # this is normally done by auto-generated Resources class