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 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 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 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 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: 200: 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") pose = Pose.from_dict(request.json) mapping: Dict[str, Type[Dobot]] = { "magician": DobotMagician, "m1": DobotM1 } global _dobot _dobot = mapping[model](pose, port, _mock) return jsonify("ok")
def main() -> None: parser = argparse.ArgumentParser(description=SERVICE_NAME) parser.add_argument( "-d", "--debug", help="Set logging level to debug.", action="store_const", const=logging.DEBUG, default=logging.DEBUG if env.get_bool("ARCOR2_CALIBRATION_DEBUG") else logging.INFO, ) run_as_mock = env.get_bool("ARCOR2_CALIBRATION_MOCK") # argparse has not support for env vars so this is kind of workaround # TODO maybe it could be solved using a custom action like https://gist.github.com/orls/51525c86ee77a56ad396 if not run_as_mock: group = parser.add_mutually_exclusive_group(required=True) group.add_argument( "--config-file", "-c", type=argparse.FileType("r"), help="Config file name containing a valid YAML configuration.", ) sub_group = group.add_mutually_exclusive_group() sub_group.add_argument("-s", "--swagger", action="store_true", default=False) sub_group.add_argument( "-m", "--mock", action="store_true", default=False, help= "Run the service in a mock mode. The same can be done by setting ARCOR2_CALIBRATION_MOCK.", ) args = parser.parse_args() logger.setLevel(args.debug) if not (run_as_mock or args.swagger or args.mock): data = args.config_file.read() global MARKER_SIZE global MIN_DIST global MAX_DIST try: config = yaml.safe_load(data) MARKER_SIZE = float(config.get("marker_size", MARKER_SIZE)) MIN_DIST = float(config.get("min_dist", MIN_DIST)) MAX_DIST = float(config.get("max_dist", MAX_DIST)) calibration.BLUR_THRESHOLD = float( config.get("blur_threshold", calibration.BLUR_THRESHOLD)) for marker_id, marker in config["markers"].items(): MARKERS[int(marker_id)] = Pose.from_dict(marker["pose"]) logger.info( f"Loaded configuration id '{config['id']}' with {len(MARKERS)} marker(s) of size {MARKER_SIZE}." ) except (KeyError, ValueError, TypeError, ValidationError): logger.exception("Failed to load the configuration file.") sys.exit(1) if not (MAX_DIST > MIN_DIST): logger.error("'max_dist' have to be bigger than 'min_dist'.") sys.exit(1) global _mock _mock = run_as_mock or args.mock if _mock: logger.info("Starting as a mock!") run_app( app, SERVICE_NAME, arcor2_calibration.version(), port_from_url(CALIBRATION_URL), [Pose, CalibrateRobotArgs, MarkerCorners, EstimatedPose], getattr(args, "swagger", False), )
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 - in: query name: safe schema: type: boolean default: false requestBody: content: application/json: schema: $ref: Pose responses: 200: description: Ok 403: description: Not started 404: description: Can't find safe path. """ 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 = MoveType(request.args.get("moveType", "jump")) velocity = float(request.args.get("velocity", default=50.0)) acceleration = float(request.args.get("acceleration", default=50.0)) safe = request.args.get("safe") == "true" if safe: cp = _dobot.get_end_effector_pose() ip1 = copy.deepcopy(cp) ip2 = copy.deepcopy(pose) for _attempt in range(20): res = scene_service.line_check( LineCheck(ip1.position, ip2.position)) if res.safe: break if move_type == MoveType.LINEAR: raise FlaskException("There might be a collision.", error_code=400) ip1.position.z += 0.01 ip2.position.z += 0.01 else: return "Can't find safe path.", 404 logger.debug(f"Collision avoidance attempts: {_attempt}") if _attempt > 0: _dobot.move(ip1, move_type, velocity, acceleration) _dobot.move(ip2, move_type, velocity, acceleration) _dobot.move(pose, move_type, velocity, acceleration) return Response(status=204)
def main() -> None: parser = argparse.ArgumentParser(description=SERVICE_NAME) parser.add_argument("-s", "--swagger", action="store_true", default=False) parser.add_argument("-m", "--mock", action="store_true", default=False) parser.add_argument( "-d", "--debug", help="Set logging level to debug.", action="store_const", const=logging.DEBUG, default=logging.INFO, ) group = parser.add_mutually_exclusive_group() group.add_argument( "--config-file", "-c", type=argparse.FileType("r"), default=sys.stdin, help="Config file name containing a valid YAML configuration.", ) group.add_argument( "yaml-config", nargs="?", type=str, help="Input string containing a valid YAML configuration.") args = parser.parse_args() logger.setLevel(args.debug) if not (args.swagger or args.mock): try: data = args.config_file.read() except AttributeError: data = args.yaml_config global MARKER_SIZE try: config = yaml.safe_load(data) MARKER_SIZE = float(config["marker_size"]) for marker_id, marker in config["markers"].items(): MARKERS[int(marker_id)] = Pose.from_dict(marker["pose"]) logger.info( f"Loaded configuration id '{config['id']}' with {len(MARKERS)} marker(s) of size {MARKER_SIZE}." ) except (KeyError, ValueError, TypeError, ValidationError): logger.exception("Failed to load the configuration file.") sys.exit(1) global _mock _mock = args.mock if _mock: logger.info("Starting as a mock!") run_app( app, SERVICE_NAME, arcor2_calibration.version(), arcor2_calibration.version(), port_from_url(CALIBRATION_URL), [Pose, CalibrateRobotArgs, MarkerCorners], args.swagger, )