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_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(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 add_logic_to_loop(tree: Module, scene: Scene, project: Project) -> None:
loop = main_loop_body(tree)
try:
actions_cache, first_action_id, last_action_id = get_actions_cache(
project)
except ProjectException as e:
raise SourceException(e)
if first_action_id is None:
raise SourceException("'start' action not found.")
if last_action_id is None:
raise SourceException("'end' action not found.")
next_action_id = first_action_id
while True:
act = actions_cache[next_action_id]
if len(loop) == 1 and isinstance(loop[0], Pass):
# pass is not necessary now
loop.clear()
ac_obj, ac_type = act.type.split('/')
# for scene objects, convert ID to name
try:
ac_obj = scene.object(ac_obj).name
except Arcor2Exception:
pass
append_method_call(loop, fix_object_name(ac_obj), ac_type,
[get_name_attr("res", clean(act.name))], [])
if act.id == last_action_id:
break
next_action_id = act.outputs[0].default