async def stop_robot_cb(req: srpc.r.StopRobot.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
# Stop robot cannot use lock, because robot is locked when action is called. Stop will also release lock.
ensure_scene_started()
robot_inst = get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.stop.__name__)
await robot.stop(robot_inst)
async def move_to_joints_cb(req: srpc.r.MoveToJoints.Request,
ui: WsClient) -> None:
ensure_scene_started()
await check_feature(req.args.robot_id, Robot.move_to_joints.__name__)
await robot.check_robot_before_move(req.args.robot_id)
asyncio.ensure_future(
robot.move_to_joints(req.args.robot_id, req.args.joints,
req.args.speed, req.args.safe))
async def get_suctions_cb(req: srpc.r.GetSuctions.Request, ui: WsClient) -> srpc.r.GetSuctions.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock(req.args.robot_id, glob.USERS.user_name(ui)):
ensure_scene_started()
return srpc.r.GetSuctions.Response(
data=await robot.get_suctions(get_robot_instance(req.args.robot_id), req.args.arm_id)
)
async def set_eef_perpendicular_to_world_cb(req: srpc.r.SetEefPerpendicularToWorld.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
ensure_scene_started()
robot_inst = await osa.get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.move_to_pose.__name__)
await check_feature(robot_inst, Robot.inverse_kinematics.__name__)
await robot.check_robot_before_move(robot_inst)
await ensure_locked(req.args.robot_id, ui)
if req.dry_run: # attempt to find suitable joints can take some time so it is not done for dry_run
return
tp, current_joints = await robot.get_pose_and_joints(robot_inst, req.args.end_effector_id)
target_joints: Optional[List[common.Joint]] = None
target_joints_diff: float = 0.0
# select best (closest joint configuration) reachable pose
tasks = [
robot.ik(robot_inst, req.args.end_effector_id, pose, current_joints, req.args.safe)
for pose in [
common.Pose(
tp.position,
common.Orientation.from_rotation_vector(y=math.pi) * common.Orientation.from_rotation_vector(z=z_rot),
)
for z_rot in np.linspace(-math.pi, math.pi, 360)
]
]
for res in await asyncio.gather(*tasks, return_exceptions=True):
if not isinstance(res, list):
continue
if not target_joints:
target_joints = res
for f, b in zip(current_joints, target_joints):
assert f.name == b.name
target_joints_diff += (f.value - b.value) ** 2
else:
diff = 0.0
for f, b in zip(current_joints, res):
assert f.name == b.name
diff += (f.value - b.value) ** 2
if diff < target_joints_diff:
target_joints = res
target_joints_diff = diff
if not target_joints:
raise Arcor2Exception("Could not find reachable pose.")
asyncio.ensure_future(robot.move_to_joints(robot_inst, target_joints, req.args.speed, req.args.safe))
async def camera_color_image_cb(req: CameraColorImage.Request,
ui: WsClient) -> CameraColorImage.Response:
assert glob.SCENE
ensure_scene_started()
camera = get_camera_instance(req.args.id)
resp = CameraColorImage.Response()
resp.data = image_to_str(camera.color_image())
return resp
async def object_aiming_start_cb(req: srpc.o.ObjectAimingStart.Request,
ui: WsClient) -> None:
"""Starts the aiming process for a selected object (with mesh) and robot.
Only possible when the scene is started/online.
UI have to acquire write locks for object and robot in advance.
:param req:
:param ui:
:return:
"""
scene = glob.LOCK.scene_or_exception()
if glob.LOCK.project:
raise Arcor2Exception("Project has to be closed first.")
ensure_scene_started()
user_name = glob.USERS.user_name(ui)
if user_name in _objects_being_aimed:
raise Arcor2Exception("Aiming already started.")
obj_id = req.args.object_id
scene_obj = scene.object(obj_id)
obj_type = glob.OBJECT_TYPES[scene_obj.type].meta
if not obj_type.has_pose:
raise Arcor2Exception("Only available for objects with pose.")
if not obj_type.object_model or obj_type.object_model.type != Model3dType.MESH:
raise Arcor2Exception("Only available for objects with mesh model.")
assert obj_type.object_model.mesh
focus_points = obj_type.object_model.mesh.focus_points
if not focus_points:
raise Arcor2Exception("focusPoints not defined for the mesh.")
await ensure_write_locked(req.args.object_id, user_name)
await ensure_write_locked(req.args.robot.robot_id, user_name)
await check_eef_arm(get_robot_instance(req.args.robot.robot_id),
req.args.robot.arm_id, req.args.robot.end_effector)
if req.dry_run:
return
_objects_being_aimed[user_name] = AimedObject(req.args.object_id,
req.args.robot)
logger.info(
f"{user_name} just started aiming of {scene_obj.name} using {scene.object(req.args.robot.robot_id).name}."
)
async def step_robot_eef_cb(req: srpc.r.StepRobotEef.Request, ui: WsClient) -> None:
scene = glob.LOCK.scene_or_exception()
ensure_scene_started()
robot_inst = get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.move_to_pose.__name__)
await robot.check_robot_before_move(robot_inst)
tp = await robot.get_end_effector_pose(robot_inst, req.args.end_effector_id, req.args.arm_id)
if req.args.mode == req.args.mode.ROBOT:
tp = tr.make_pose_rel(robot_inst.pose, tp)
elif req.args.mode == req.args.mode.RELATIVE:
assert req.args.pose
tp = tr.make_pose_rel(req.args.pose, tp)
elif req.args.mode == req.args.mode.USER:
assert req.args.pose
raise Arcor2Exception("Not supported yet.")
if req.args.what == req.args.what.POSITION:
if req.args.axis == req.args.axis.X:
tp.position.x += req.args.step
elif req.args.axis == req.args.axis.Y:
tp.position.y += req.args.step
elif req.args.axis == req.args.axis.Z:
tp.position.z += req.args.step
elif req.args.what == req.args.what.ORIENTATION:
if req.args.axis == req.args.axis.X:
tp.orientation *= common.Orientation.from_rotation_vector(x=req.args.step)
elif req.args.axis == req.args.axis.Y:
tp.orientation *= common.Orientation.from_rotation_vector(y=req.args.step)
elif req.args.axis == req.args.axis.Z:
tp.orientation *= common.Orientation.from_rotation_vector(z=req.args.step)
if req.args.mode == req.args.mode.ROBOT:
tp = tr.make_pose_abs(robot_inst.pose, tp)
elif req.args.mode == req.args.mode.RELATIVE:
assert req.args.pose
tp = tr.make_pose_abs(req.args.pose, tp)
await robot.check_reachability(scene, robot_inst, req.args.end_effector_id, req.args.arm_id, tp, req.args.safe)
await ensure_write_locked(req.args.robot_id, glob.USERS.user_name(ui))
if req.dry_run:
return
asyncio.ensure_future(
robot.move_to_pose(
robot_inst, req.args.end_effector_id, req.args.arm_id, tp, req.args.speed, req.args.safe, req.args.linear
)
)
async def fk_cb(req: srpc.r.ForwardKinematics.Request,
ui: WsClient) -> srpc.r.ForwardKinematics.Response:
ensure_scene_started()
await check_feature(req.args.robot_id, Robot.forward_kinematics.__name__)
pose = await robot.fk(req.args.robot_id, req.args.end_effector_id,
req.args.joints)
resp = srpc.r.ForwardKinematics.Response()
resp.data = pose
return resp
async def camera_color_parameters_cb(
req: CameraColorParameters.Request,
ui: WsClient) -> CameraColorParameters.Response:
assert glob.SCENE
ensure_scene_started()
camera = get_camera_instance(req.args.id)
resp = CameraColorParameters.Response()
resp.data = camera.color_camera_params
return resp
async def ik_cb(req: srpc.r.InverseKinematics.Request,
ui: WsClient) -> srpc.r.InverseKinematics.Response:
ensure_scene_started()
await check_feature(req.args.robot_id, Robot.inverse_kinematics.__name__)
joints = await robot.ik(req.args.robot_id, req.args.end_effector_id,
req.args.pose, req.args.start_joints,
req.args.avoid_collisions)
resp = srpc.r.InverseKinematics.Response()
resp.data = joints
return resp
async def camera_color_image_cb(req: CameraColorImage.Request,
ui: WsClient) -> CameraColorImage.Response:
glob.LOCK.scene_or_exception()
await ensure_locked(req.args.id, ui)
ensure_scene_started()
camera = get_camera_instance(req.args.id)
resp = CameraColorImage.Response()
resp.data = image_to_str(camera.color_image())
return resp
async def get_end_effector_pose_cb(
req: srpc.r.GetEndEffectorPose.Request,
ui: WsClient) -> srpc.r.GetEndEffectorPose.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock(req.args.robot_id, glob.USERS.user_name(ui)):
ensure_scene_started()
return srpc.r.GetEndEffectorPose.Response(
data=await robot.get_end_effector_pose(
await osa.get_robot_instance(req.args.robot_id),
req.args.end_effector_id, req.args.arm_id))
async def calibrate_camera_cb(req: CalibrateCamera.Request,
ui: WsClient) -> None:
assert glob.SCENE
ensure_scene_started()
camera = get_camera_instance(req.args.id)
# TODO check camera features / meta if it supports getting color image
if not camera.color_camera_params:
raise Arcor2Exception("Camera parameters not available.")
asyncio.ensure_future(calibrate_camera(camera))
async def camera_color_parameters_cb(
req: CameraColorParameters.Request,
ui: WsClient) -> CameraColorParameters.Response:
glob.LOCK.scene_or_exception()
await ensure_locked(req.args.id, ui)
ensure_scene_started()
camera = get_camera_instance(req.args.id)
resp = CameraColorParameters.Response()
resp.data = camera.color_camera_params
return resp
async def move_to_action_point_cb(req: srpc.r.MoveToActionPoint.Request, ui: WsClient) -> None:
scene = glob.LOCK.scene_or_exception()
project = glob.LOCK.project_or_exception()
async with ctx_write_lock(req.args.robot_id, glob.USERS.user_name(ui)):
ensure_scene_started()
robot_inst = get_robot_instance(req.args.robot_id)
await robot.check_robot_before_move(robot_inst)
if (req.args.orientation_id is None) == (req.args.joints_id is None):
raise Arcor2Exception("Set orientation or joints. Not both.")
if req.args.orientation_id:
await check_feature(robot_inst, Robot.move_to_pose.__name__)
if req.args.end_effector_id is None:
raise Arcor2Exception("eef id has to be set.")
pose = tr.abs_pose_from_ap_orientation(scene, project, req.args.orientation_id)
# TODO check if the target pose is reachable (dry_run)
asyncio.ensure_future(
robot.move_to_ap_orientation(
robot_inst,
req.args.end_effector_id,
req.args.arm_id,
pose,
req.args.speed,
req.args.orientation_id,
req.args.safe,
req.args.linear,
)
)
elif req.args.joints_id:
await check_feature(robot_inst, Robot.move_to_joints.__name__)
# TODO add arm_id to ProjectRobotJoints and use it as a parameter for move_to_ap_joints
joints = project.joints(req.args.joints_id)
# TODO check if the joints are within limits and reachable (dry_run)
asyncio.ensure_future(
robot.move_to_ap_joints(
robot_inst, joints.joints, req.args.speed, req.args.joints_id, req.args.safe, req.args.arm_id
)
)
async def fk_cb(req: srpc.r.ForwardKinematics.Request, ui: WsClient) -> srpc.r.ForwardKinematics.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock([req.args.robot_id, req.args.end_effector_id], glob.USERS.user_name(ui)):
ensure_scene_started()
robot_inst = get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.forward_kinematics.__name__)
pose = await robot.fk(robot_inst, req.args.end_effector_id, req.args.arm_id, req.args.joints)
resp = srpc.r.ForwardKinematics.Response()
resp.data = pose
return resp
async def focus_object_start_cb(req: srpc.o.FocusObjectStart.Request,
ui: WsClient) -> None:
# TODO this event should take long time, notify UI that robot is locked
async with ctx_read_lock([req.args.object_id, req.args.robot.robot_id],
glob.USERS.user_name(ui),
auto_unlock=False):
scene = glob.LOCK.scene_or_exception()
if glob.LOCK.project:
raise Arcor2Exception("Project has to be closed first.")
ensure_scene_started()
obj_id = req.args.object_id
if obj_id in FOCUS_OBJECT_ROBOT:
raise Arcor2Exception("Focusing already started.")
if obj_id not in glob.SCENE_OBJECT_INSTANCES:
raise Arcor2Exception("Unknown object.")
inst = await osa.get_robot_instance(req.args.robot.robot_id)
await check_eef_arm(inst, req.args.robot.arm_id,
req.args.robot.end_effector)
robot_type = glob.OBJECT_TYPES[inst.__class__.__name__]
assert robot_type.robot_meta
obj_type = glob.OBJECT_TYPES[scene.object(obj_id).type].meta
if not obj_type.has_pose:
raise Arcor2Exception("Only available for objects with pose.")
if not obj_type.object_model or obj_type.object_model.type != Model3dType.MESH:
raise Arcor2Exception(
"Only available for objects with mesh model.")
assert obj_type.object_model.mesh
focus_points = obj_type.object_model.mesh.focus_points
if not focus_points:
raise Arcor2Exception("focusPoints not defined for the mesh.")
FOCUS_OBJECT_ROBOT[req.args.object_id] = req.args.robot
FOCUS_OBJECT[obj_id] = {}
glob.logger.info(f"Start of focusing for {obj_id}.")
return None
async def move_to_joints_cb(req: srpc.r.MoveToJoints.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
user_name = glob.USERS.user_name(ui)
async with ctx_write_lock(req.args.robot_id, user_name, auto_unlock=False):
ensure_scene_started()
robot_inst = get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.move_to_joints.__name__)
await robot.check_robot_before_move(robot_inst)
asyncio.ensure_future(
robot.move_to_joints(robot_inst, req.args.joints, req.args.speed, req.args.safe, user_name, req.args.arm_id)
)
async def ik_cb(req: srpc.r.InverseKinematics.Request, ui: WsClient) -> srpc.r.InverseKinematics.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock([req.args.robot_id, req.args.end_effector_id], glob.USERS.user_name(ui)):
ensure_scene_started()
robot_inst = await osa.get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.inverse_kinematics.__name__)
joints = await robot.ik(
robot_inst, req.args.end_effector_id, req.args.pose, req.args.start_joints, req.args.avoid_collisions
)
resp = srpc.r.InverseKinematics.Response()
resp.data = joints
return resp
async def move_to_action_point_cb(req: srpc.r.MoveToActionPoint.Request,
ui: WsClient) -> None:
ensure_scene_started()
await robot.check_robot_before_move(req.args.robot_id)
assert glob.SCENE
assert glob.PROJECT
if (req.args.orientation_id is None) == (req.args.joints_id is None):
raise Arcor2Exception("Set orientation or joints. Not both.")
if req.args.orientation_id:
await check_feature(req.args.robot_id, Robot.move_to_pose.__name__)
if req.args.end_effector_id is None:
raise Arcor2Exception("eef id has to be set.")
pose = tr.abs_pose_from_ap_orientation(glob.SCENE, glob.PROJECT,
req.args.orientation_id)
# TODO check if the target pose is reachable (dry_run)
asyncio.ensure_future(
robot.move_to_ap_orientation(
req.args.robot_id,
req.args.end_effector_id,
pose,
req.args.speed,
req.args.orientation_id,
req.args.safe,
))
elif req.args.joints_id:
await check_feature(req.args.robot_id, Robot.move_to_joints.__name__)
joints = glob.PROJECT.joints(req.args.joints_id)
# TODO check if the joints are within limits and reachable (dry_run)
asyncio.ensure_future(
robot.move_to_ap_joints(req.args.robot_id, joints.joints,
req.args.speed, req.args.joints_id,
req.args.safe))
async def update_action_point_joints_using_robot_cb(
req: srpc.p.UpdateActionPointJointsUsingRobot.Request,
ui: WsClient) -> None:
ensure_scene_started()
assert glob.SCENE
assert glob.PROJECT
robot_joints = glob.PROJECT.joints(req.args.joints_id)
robot_joints.joints = await get_robot_joints(robot_joints.robot_id)
robot_joints.is_valid = True
glob.PROJECT.update_modified()
evt = sevts.p.JointsChanged(robot_joints)
evt.change_type = Event.Type.UPDATE
asyncio.ensure_future(notif.broadcast_event(evt))
return None
async def move_to_pose_cb(req: srpc.r.MoveToPose.Request,
ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
user_name = glob.USERS.user_name(ui)
async with ctx_write_lock(req.args.robot_id, user_name, auto_unlock=False):
ensure_scene_started()
robot_inst = await osa.get_robot_instance(req.args.robot_id)
await robot.check_eef_arm(robot_inst, req.args.arm_id,
req.args.end_effector_id)
await check_feature(robot_inst, Robot.move_to_pose.__name__)
await robot.check_robot_before_move(robot_inst)
if (req.args.position is None) != (req.args.orientation is None):
target_pose = await robot.get_end_effector_pose(
robot_inst, req.args.end_effector_id, req.args.arm_id)
if req.args.position:
target_pose.position = req.args.position
elif req.args.orientation:
target_pose.orientation = req.args.orientation
elif req.args.position is not None and req.args.orientation is not None:
target_pose = common.Pose(req.args.position, req.args.orientation)
else:
raise Arcor2Exception("Position or orientation should be given.")
# TODO check if the target pose is reachable (dry_run)
asyncio.ensure_future(
robot.move_to_pose(
robot_inst,
req.args.end_effector_id,
req.args.arm_id,
target_pose,
req.args.speed,
req.args.safe,
user_name,
))
async def action_param_values_cb(
req: srpc.o.ActionParamValues.Request,
ui: WsClient) -> srpc.o.ActionParamValues.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock(req.args.id, glob.USERS.user_name(ui)):
ensure_scene_started()
inst = get_instance(req.args.id)
parent_params = {}
for pp in req.args.parent_params:
parent_params[pp.id] = pp.value
try:
method_name, required_parent_params = inst.DYNAMIC_PARAMS[
req.args.param_id]
except KeyError:
raise Arcor2Exception(
"Unknown parameter or values not constrained.")
if parent_params.keys() != required_parent_params:
raise Arcor2Exception("Not all required parent params were given.")
# TODO validate method parameters vs parent_params (check types)?
resp = srpc.o.ActionParamValues.Response()
try:
method = getattr(inst, method_name)
except AttributeError:
glob.logger.error(
f"Unable to get values for parameter {req.args.param_id}, "
f"object/service {inst.id} has no method named {method_name}.")
raise Arcor2Exception("System error.")
# TODO update hlp.run_in_executor to support kwargs
resp.data = await asyncio.get_event_loop().run_in_executor(
None, functools.partial(method, **parent_params))
return resp
async def update_action_point_using_robot_cb(
req: srpc.p.UpdateActionPointUsingRobot.Request, ui: WsClient) -> None:
ensure_scene_started()
assert glob.SCENE
assert glob.PROJECT
ap = glob.PROJECT.bare_action_point(req.args.action_point_id)
new_pose = await get_end_effector_pose(req.args.robot.robot_id,
req.args.robot.end_effector)
if ap.parent:
new_pose = tr.make_pose_rel_to_parent(glob.SCENE, glob.PROJECT,
new_pose, ap.parent)
await update_ap_position(
glob.PROJECT.bare_action_point(req.args.action_point_id),
new_pose.position)
return None
async def focus_object_start_cb(req: srpc.o.FocusObjectStart.Request,
ui: WsClient) -> None:
ensure_scene_started()
obj_id = req.args.object_id
if obj_id in FOCUS_OBJECT_ROBOT:
raise Arcor2Exception("Focusing already started.")
if obj_id not in glob.SCENE_OBJECT_INSTANCES:
raise Arcor2Exception("Unknown object.")
inst = await osa.get_robot_instance(req.args.robot.robot_id,
req.args.robot.end_effector)
robot_type = glob.OBJECT_TYPES[inst.__class__.__name__]
assert robot_type.robot_meta
obj_type = glob.OBJECT_TYPES[osa.get_obj_type_name(obj_id)].meta
if not obj_type.has_pose:
raise Arcor2Exception("Only available for objects with pose.")
if not obj_type.object_model or obj_type.object_model.type != Model3dType.MESH:
raise Arcor2Exception("Only available for objects with mesh model.")
assert obj_type.object_model.mesh
focus_points = obj_type.object_model.mesh.focus_points
if not focus_points:
raise Arcor2Exception("focusPoints not defined for the mesh.")
FOCUS_OBJECT_ROBOT[req.args.object_id] = req.args.robot
FOCUS_OBJECT[obj_id] = {}
glob.logger.info(f"Start of focusing for {obj_id}.")
return None
async def hand_teaching_mode_cb(req: srpc.r.HandTeachingMode.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
ensure_scene_started()
robot_inst = await osa.get_robot_instance(req.args.robot_id)
# in this case, method name does not correspond to feature name
await check_feature(robot_inst, "hand_teaching")
hand_teaching_mode = await run_in_executor(robot_inst.get_hand_teaching_mode)
if req.args.enable == hand_teaching_mode:
raise Arcor2Exception("That's the current state.")
await ensure_locked(req.args.robot_id, ui)
if req.dry_run:
return
await run_in_executor(robot_inst.set_hand_teaching_mode, req.args.enable)
evt = HandTeachingMode(HandTeachingMode.Data(req.args.robot_id, req.args.enable))
asyncio.ensure_future(notif.broadcast_event(evt))
async def register_for_robot_event_cb(
req: srpc.r.RegisterForRobotEvent.Request, ui: WsClient) -> None:
ensure_scene_started()
# check if robot exists
await osa.get_robot_instance(req.args.robot_id)
if req.args.what == req.args.RegisterEnum.JOINTS:
await register(req, ui, ROBOT_JOINTS_TASKS,
glob.ROBOT_JOINTS_REGISTERED_UIS, robot_joints_event)
elif req.args.what == req.args.RegisterEnum.EEF_POSE:
if not (await robot.get_end_effectors(req.args.robot_id)):
raise Arcor2Exception("Robot does not have any end effector.")
await register(req, ui, EEF_POSE_TASKS, glob.ROBOT_EEF_REGISTERED_UIS,
robot_eef_pose_event)
else:
raise Arcor2Exception(
f"Option '{req.args.what.value}' not implemented.")
return None
async def hand_teaching_mode_cb(req: srpc.r.HandTeachingMode.Request,
ui: WsClient) -> None:
ensure_scene_started()
robot_inst = await osa.get_robot_instance(req.args.robot_id)
otd = osa.get_obj_type_data(req.args.robot_id)
assert otd.robot_meta is not None
if not otd.robot_meta.features.hand_teaching:
raise Arcor2Exception("Robot does not support hand teaching.")
hand_teaching_mode = await run_in_executor(
robot_inst.get_hand_teaching_mode)
if req.args.enable == hand_teaching_mode:
raise Arcor2Exception("That's the current state.")
if req.dry_run:
return
await run_in_executor(robot_inst.set_hand_teaching_mode, req.args.enable)
evt = HandTeachingMode(
HandTeachingMode.Data(req.args.robot_id, req.args.enable))
asyncio.ensure_future(notif.broadcast_event(evt))
async def add_action_point_orientation_using_robot_cb(
req: srpc.p.AddActionPointOrientationUsingRobot.Request,
ui: WsClient) -> None:
"""Adds orientation and joints to the action point.
:param req:
:return:
"""
ensure_scene_started()
assert glob.SCENE
assert glob.PROJECT
ap = glob.PROJECT.bare_action_point(req.args.action_point_id)
hlp.is_valid_identifier(req.args.name)
unique_name(req.args.name, glob.PROJECT.ap_orientation_names(ap.id))
if req.dry_run:
return None
new_pose = await get_end_effector_pose(req.args.robot.robot_id,
req.args.robot.end_effector)
if ap.parent:
new_pose = tr.make_pose_rel_to_parent(glob.SCENE, glob.PROJECT,
new_pose, ap.parent)
orientation = common.NamedOrientation(req.args.name, new_pose.orientation)
glob.PROJECT.upsert_orientation(ap.id, orientation)
evt = sevts.p.OrientationChanged(orientation)
evt.change_type = Event.Type.ADD
evt.parent_id = ap.id
asyncio.ensure_future(notif.broadcast_event(evt))
return None
async def update_object_pose_using_robot_cb(
req: srpc.o.UpdateObjectPoseUsingRobot.Request, ui: WsClient) -> None:
"""Updates object's pose using a pose of the robot's end effector.
:param req:
:return:
"""
assert glob.SCENE
ensure_scene_started()
if req.args.id == req.args.robot.robot_id:
raise Arcor2Exception("Robot cannot update its own pose.")
scene_object = glob.SCENE.object(req.args.id)
obj_type = glob.OBJECT_TYPES[scene_object.type]
if not obj_type.meta.has_pose:
raise Arcor2Exception("Object without pose.")
object_model = obj_type.meta.object_model
if object_model:
collision_model = object_model.model()
if isinstance(collision_model, object_type.Mesh
) and req.args.pivot != req.args.PivotEnum.MIDDLE:
raise Arcor2Exception(
"Only middle pivot point is supported for objects with mesh collision model."
)
elif req.args.pivot != req.args.PivotEnum.MIDDLE:
raise Arcor2Exception(
"Only middle pivot point is supported for objects without collision model."
)
new_pose = await get_end_effector_pose(req.args.robot.robot_id,
req.args.robot.end_effector)
position_delta = common.Position()
if object_model:
collision_model = object_model.model()
if isinstance(collision_model, object_type.Box):
if req.args.pivot == req.args.PivotEnum.TOP:
position_delta.z -= collision_model.size_z / 2
elif req.args.pivot == req.args.PivotEnum.BOTTOM:
position_delta.z += collision_model.size_z / 2
elif isinstance(collision_model, object_type.Cylinder):
if req.args.pivot == req.args.PivotEnum.TOP:
position_delta.z -= collision_model.height / 2
elif req.args.pivot == req.args.PivotEnum.BOTTOM:
position_delta.z += collision_model.height / 2
elif isinstance(collision_model, object_type.Sphere):
if req.args.pivot == req.args.PivotEnum.TOP:
position_delta.z -= collision_model.radius / 2
elif req.args.pivot == req.args.PivotEnum.BOTTOM:
position_delta.z += collision_model.radius / 2
position_delta = position_delta.rotated(new_pose.orientation)
assert scene_object.pose
scene_object.pose.position = new_pose.position - position_delta
scene_object.pose.orientation.set_from_quaternion(
new_pose.orientation.as_quaternion() *
quaternion.quaternion(0, 1, 0, 0))
asyncio.ensure_future(update_scene_object_pose(scene_object))
return None
