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