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, ) )