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 goto_pose(self, pose: Pose, linear: bool = True, relative: bool = False) -> None:
"""Commands the YuMi to goto the given pose.
Parameters
----------
pose : RigidTransform
linear : bool, optional
If True, will use MoveL in RAPID to ensure linear path.
Otherwise use MoveJ in RAPID, which does not ensure linear path.
Defaults to True
relative : bool, optional
If True, will use goto_pose_relative by computing the delta pose from current pose to target pose.
Defaults to False
"""
if relative:
cur_pose = self.get_pose()
delta_pose = Pose.from_tr_matrix(cur_pose.inversed().as_tr_matrix() * pose.as_tr_matrix())
rot = np.rad2deg(quaternion.as_euler_angles(delta_pose.orientation.as_quaternion()))
self.goto_pose_delta(delta_pose.position, rot)
else:
body = self._get_pose_body(pose)
if linear:
cmd = CmdCodes.goto_pose_linear
else:
cmd = CmdCodes.goto_pose
req = self._construct_req(cmd, body)
self._request(req, timeout=self._motion_timeout)
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_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 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_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 _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_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 put_ik() -> RespT:
"""Get the current state.
---
put:
description: Get the current state.
tags:
- Robot
requestBody:
content:
application/json:
schema:
$ref: Pose
responses:
200:
description: Ok
content:
application/json:
schema:
type: array
items:
$ref: Joint
403:
description: Not started
"""
assert _dobot is not None
if not isinstance(request.json, dict):
raise FlaskException("Body should be a JSON dict containing Pose.",
error_code=400)
pose = Pose.from_dict(request.json)
return jsonify(_dobot.inverse_kinematics(pose))
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 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 put_eef_pose() -> RespT:
"""Set the EEF pose.
---
put:
description: Set the EEF pose.
tags:
- Robot
parameters:
- in: query
name: moveType
schema:
type: string
enum:
- JUMP
- LINEAR
- JOINTS
required: true
description: Move type
- name: velocity
in: query
schema:
type: number
format: float
minimum: 0
maximum: 100
- name: acceleration
in: query
schema:
type: number
format: float
minimum: 0
maximum: 100
requestBody:
content:
application/json:
schema:
$ref: Pose
responses:
200:
description: Ok
content:
application/json:
schema:
$ref: Pose
403:
description: Not started
"""
assert _dobot is not None
if not isinstance(request.json, dict):
raise FlaskException("Body should be a JSON dict containing Pose.", error_code=400)
pose = Pose.from_dict(request.json)
move_type: str = request.args.get("moveType", "jump")
velocity = float(request.args.get("velocity", default=50.0))
acceleration = float(request.args.get("acceleration", default=50.0))
_dobot.move(pose, MoveType(move_type), velocity, acceleration)
return jsonify("ok")
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 put_ik() -> RespT:
"""Get the current state.
---
put:
description: Get the current state.
tags:
- Robot
requestBody:
content:
application/json:
schema:
$ref: Pose
responses:
200:
description: Ok
content:
application/json:
schema:
type: array
items:
$ref: Joint
403:
description: Not started
"""
assert _dobot is not None
pose = Pose.from_dict(request.json)
return jsonify(_dobot.inverse_kinematics(pose))
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 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 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 put_start() -> RespT:
"""Start the robot.
---
put:
description: Start the robot.
tags:
- State
parameters:
- in: query
name: port
schema:
type: string
default: /dev/dobot
description: Dobot port
- in: query
name: model
schema:
type: string
enum:
- magician
- m1
required: true
description: Dobot model
requestBody:
content:
application/json:
schema:
$ref: Pose
responses:
204:
description: Ok
403:
description: Already started
"""
if started():
return "Already started.", 403
model: str = request.args.get("model", default="magician")
port: str = request.args.get("port", default="/dev/dobot")
if not isinstance(request.json, dict):
raise FlaskException("Body should be a JSON dict containing Pose.",
error_code=400)
pose = Pose.from_dict(request.json)
mapping: dict[str, type[Dobot]] = {
"magician": DobotMagician,
"m1": DobotM1
}
global _dobot
_dobot = mapping[model](pose, port, _mock)
return Response(status=204)
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 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 __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 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 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
"""
return Pose(
(child.position - parent.position).rotated(parent.orientation, inverse=True),
Orientation.from_quaternion(parent.orientation.as_quaternion().inverse() * 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
