async def calibrate_robot_cb(req: srpc.r.CalibrateRobot.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
user_name = glob.USERS.user_name(ui)
ensure_scene_started()
if not req.args.camera_id:
for obj in glob.SCENE_OBJECT_INSTANCES.values():
if isinstance(obj, Camera):
req.args.camera_id = obj.id
break
else:
raise Arcor2Exception("No camera found.")
async with ctx_write_lock(req.args.camera_id, user_name, auto_unlock=False):
robot_inst = get_robot_instance(req.args.robot_id)
if not robot_inst.urdf_package_name:
raise Arcor2Exception("Robot with model required!")
camera_inst = get_instance(req.args.camera_id, Camera)
if camera_inst.color_camera_params is None:
raise Arcor2Exception("Calibrated camera required!")
await ensure_write_locked(req.args.robot_id, user_name)
asyncio.ensure_future(calibrate_robot(robot_inst, camera_inst, req.args.move_to_calibration_pose, user_name))
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 = 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")
if robot_inst.move_in_progress:
raise Arcor2Exception("Not possible while robot moves.")
params = []
if isinstance(robot_inst, MultiArmRobot):
params.append(req.args.arm_id)
elif req.args.arm_id:
raise Arcor2Exception(
f"The '{req.args.arm_id}' arm_id was set for single arm robot '{robot_inst.name}' "
f"of type '{robot_inst.__class__.__name__}'!"
)
hand_teaching_mode = await run_in_executor(robot_inst.get_hand_teaching_mode, *params)
if req.args.enable == hand_teaching_mode:
raise Arcor2Exception("That's the current state.")
await ensure_write_locked(req.args.robot_id, glob.USERS.user_name(ui))
if req.dry_run:
return
await run_in_executor(robot_inst.set_hand_teaching_mode, req.args.enable, *params)
evt = HandTeachingMode(HandTeachingMode.Data(req.args.robot_id, req.args.enable, req.args.arm_id))
asyncio.ensure_future(notif.broadcast_event(evt))
async def register_for_robot_event_cb(req: srpc.r.RegisterForRobotEvent.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
async with ctx_read_lock(req.args.robot_id, glob.USERS.user_name(ui)):
ensure_scene_started()
# check if robot exists
robot_inst = get_robot_instance(req.args.robot_id)
if req.args.what == req.args.RegisterEnum.JOINTS:
await register(
req, robot_inst, ui, ROBOT_JOINTS_TASKS, glob.ROBOT_JOINTS_REGISTERED_UIS, robot_joints_event
)
elif req.args.what == req.args.RegisterEnum.EEF_POSE:
if isinstance(robot_inst, MultiArmRobot):
for arm_id in await robot.get_arms(robot_inst):
if await robot.get_end_effectors(robot_inst, arm_id):
break
else:
raise Arcor2Exception("Robot does not have any end effector.")
elif not await robot.get_end_effectors(robot_inst, None):
raise Arcor2Exception("Robot does not have any end effector.")
await register(req, robot_inst, 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 get_robot_arms_cb(req: srpc.r.GetRobotArms.Request, ui: WsClient) -> srpc.r.GetRobotArms.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.GetRobotArms.Response(data=await robot.get_arms(get_robot_instance(req.args.robot_id)))
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 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 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(
get_robot_instance(req.args.robot_id), req.args.end_effector_id, req.args.arm_id
)
)
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 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 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 = 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,
req.args.linear,
user_name,
)
)
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 = 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.arm_id,
req.args.pose,
req.args.start_joints,
req.args.avoid_collisions,
)
resp = srpc.r.InverseKinematics.Response()
resp.data = joints
return resp
async def object_aiming_add_point_cb(
req: srpc.o.ObjectAimingAddPoint.Request,
ui: WsClient) -> srpc.o.ObjectAimingAddPoint.Response:
scene = glob.LOCK.scene_or_exception()
fo, user_name = await object_aiming_check(ui)
pt_idx = req.args.point_idx
scene_obj = scene.object(fo.obj_id)
obj_type = glob.OBJECT_TYPES[scene_obj.type].meta
assert obj_type.has_pose
assert obj_type.object_model
assert obj_type.object_model.mesh
focus_points = obj_type.object_model.mesh.focus_points
assert focus_points
if pt_idx < 0 or pt_idx > len(focus_points) - 1:
raise Arcor2Exception("Index out of range.")
robot_id, end_effector, arm_id = fo.robot.as_tuple()
robot_inst = get_robot_instance(robot_id)
r = srpc.o.ObjectAimingAddPoint.Response()
r.data = r.Data(finished_indexes=list(fo.poses.keys()))
if not req.dry_run:
fo.poses[pt_idx] = await get_end_effector_pose(robot_inst,
end_effector, arm_id)
r.data = r.Data(finished_indexes=list(fo.poses.keys()))
logger.info(
f"{user_name} just aimed index {pt_idx} for {scene_obj.name}. Done indexes: {r.data.finished_indexes}."
)
return r
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 = 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 check_feature(robot_inst, Robot.forward_kinematics.__name__)
await robot.check_robot_before_move(robot_inst)
await ensure_write_locked(req.args.robot_id, glob.USERS.user_name(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, req.args.arm_id)
target_joints_diff: float = 0.0
winning_idx: int = -1
poses: list[common.Pose] = [
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)
]
# select best (closest joint configuration) reachable pose
tasks = [
robot.ik(robot_inst, req.args.end_effector_id, req.args.arm_id, pose, current_joints, req.args.safe)
for pose in poses
]
# order of results from gather corresponds to order of tasks
for idx, res in enumerate(await asyncio.gather(*tasks, return_exceptions=True)):
if not isinstance(res, list):
continue
diff = 0.0
for f, b in zip(current_joints, res):
assert (
f.name == b.name
), f"current joints: {[j.name for j in current_joints]}, joints from ik {[j.name for j in res]}."
diff += (f.value - b.value) ** 2
if winning_idx < 0:
target_joints_diff = diff
winning_idx = idx
elif diff < target_joints_diff:
winning_idx = idx
target_joints_diff = diff
if winning_idx < 0:
raise Arcor2Exception("Could not find reachable pose.")
asyncio.ensure_future(
robot.move_to_pose(
robot_inst,
req.args.end_effector_id,
req.args.arm_id,
poses[winning_idx],
req.args.speed,
req.args.safe,
req.args.linear,
)
)
