From 1c61bdda8060e7422b7a33a674a0309baa4f27e7 Mon Sep 17 00:00:00 2001 From: Adnan Esmail Date: Fri, 24 Jul 2026 13:39:24 -0700 Subject: [PATCH 1/5] build: provision libtrossen_arm and make build_deps.sh idempotent - build_deps.sh gains a pinned libtrossen_arm section (v1.10.0, prebuilt static lib + headers): the iNerve controller rejects a driver whose major.minor doesn't match its firmware, so the SDK ref is pinned and verified like the other native deps. - Per-dep skip: an existing install artifact at the pinned ref means the dep is done, so re-runs are idempotent instead of rebuilding Pinocchio from scratch every time. - CMake learns the trossen prefix; pyproject adds trossen-arm==1.10.0 (Python-side discovery/zeroing/IP provisioning, pinned to the same SDK release) and pyserial>=3.5 (USB-serial bus-servo sessions). No consumer of the new deps yet; the build is unchanged until the driver stacks land. Co-Authored-By: Claude Fable 5 --- CMakeLists.txt | 6 ++++ pyproject.toml | 8 +++++ scripts/build_deps.sh | 72 +++++++++++++++++++++++++++++++++---------- 3 files changed, 69 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 00ac8ab..83225ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,8 @@ set(OPENPI_CONTROL_PINOCCHIO_PREFIX "${OPENPI_CONTROL_DEPS_DIR}/pinocchio/instal "Pinocchio installation prefix used by openpi-control") set(OPENPI_CONTROL_CPPZMQ_PREFIX "${OPENPI_CONTROL_DEPS_DIR}/cppzmq/install" CACHE PATH "cppzmq installation prefix used by openpi-control") +set(OPENPI_CONTROL_TROSSEN_PREFIX "${OPENPI_CONTROL_DEPS_DIR}/trossen_arm/install" CACHE PATH + "libtrossen_arm installation prefix used by openpi-control") if(EXISTS "${OPENPI_CONTROL_PINOCCHIO_PREFIX}/lib/pkgconfig/pinocchio.pc") list(PREPEND CMAKE_PREFIX_PATH "${OPENPI_CONTROL_PINOCCHIO_PREFIX}") @@ -37,6 +39,10 @@ if(EXISTS "${OPENPI_CONTROL_CPPZMQ_PREFIX}/include/zmq.hpp") set(OPENPI_CONTROL_CPPZMQ_INCLUDE_DIR "${OPENPI_CONTROL_CPPZMQ_PREFIX}/include") endif() +if(EXISTS "${OPENPI_CONTROL_TROSSEN_PREFIX}/lib/libtrossen_arm.a") + list(PREPEND CMAKE_PREFIX_PATH "${OPENPI_CONTROL_TROSSEN_PREFIX}") +endif() + # Sanitizer plumbing for the test targets. Valid values: "" (off), "address,undefined", # "thread". Applied globally so the exercised pi_control sources are instrumented too. set(OPENPI_CONTROL_SANITIZER "" CACHE STRING "Comma-separated -fsanitize= list for test builds") diff --git a/pyproject.toml b/pyproject.toml index 8be847b..7cf7cef 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -14,6 +14,14 @@ dependencies = [ # Direct CAN access for the servo zeroing registry (openpi_control.servos). "python-can>=4.3", "pyzmq>=25", + # Trossen iNerve Ethernet controllers: discovery, EEPROM zeroing, and IP + # provisioning (openpi_control.servos.trossen_eth). Pinned to the same + # release as the C++ libtrossen_arm in scripts/build_deps.sh so the driver + # <-> controller firmware compatibility stays aligned. + "trossen-arm==1.10.0", + # USB-serial bus-servo maintenance sessions (openpi_control.servos.buses) + # for the FeeTech/Dynamixel serial families (e.g. SO-ARM101). + "pyserial>=3.5", ] [project.optional-dependencies] diff --git a/scripts/build_deps.sh b/scripts/build_deps.sh index d3d3efb..048a5bf 100755 --- a/scripts/build_deps.sh +++ b/scripts/build_deps.sh @@ -26,6 +26,12 @@ pinocchio_ref="${PINOCCHIO_GIT_REF:-v3.4.0}" cppzmq_source="${deps_root}/cppzmq" cppzmq_prefix="${cppzmq_source}/install" cppzmq_ref="${CPPZMQ_GIT_REF:-v4.9.0}" +# Trossen iNerve controller SDK (prebuilt static lib + headers). Pinned so the +# driver <-> controller firmware compatibility stays reproducible: the +# controller rejects a driver whose major.minor doesn't match its firmware. +trossen_arm_source="${deps_root}/trossen_arm" +trossen_arm_prefix="${trossen_arm_source}/install" +trossen_arm_ref="${TROSSEN_ARM_GIT_REF:-v1.10.0}" build_jobs="${BUILD_JOBS:-2}" # A cached clone built from a different pinned ref must not be reused @@ -45,6 +51,7 @@ verify_cached_ref() { verify_cached_ref "${pinocchio_source}" "${pinocchio_ref}" verify_cached_ref "${cppzmq_source}" "${cppzmq_ref}" +verify_cached_ref "${trossen_arm_source}" "${trossen_arm_ref}" if ! [[ "${build_jobs}" =~ ^[1-9][0-9]*$ ]]; then echo "BUILD_JOBS must be a positive integer; got ${build_jobs@Q}" >&2 @@ -62,26 +69,50 @@ install -d "${cppzmq_prefix}/include" install -m 0644 "${cppzmq_source}/zmq.hpp" "${cppzmq_prefix}/include/zmq.hpp" install -m 0644 "${cppzmq_source}/zmq_addon.hpp" "${cppzmq_prefix}/include/zmq_addon.hpp" -if [[ ! -d "${pinocchio_source}/.git" ]]; then - git clone --branch "${pinocchio_ref}" --depth 1 \ - https://github.com/stack-of-tasks/pinocchio.git "${pinocchio_source}" -fi +# Per-dep skip: verify_cached_ref above already guarantees that any cached +# clone is at the pinned ref (a ref bump against an old cache fails fast), so +# an existing install artifact means this exact pinned version is installed. +if [[ -f "${pinocchio_prefix}/lib/pkgconfig/pinocchio.pc" ]]; then + echo "Pinocchio ${pinocchio_ref} already installed at ${pinocchio_prefix}; skipping." +else + if [[ ! -d "${pinocchio_source}/.git" ]]; then + git clone --branch "${pinocchio_ref}" --depth 1 \ + https://github.com/stack-of-tasks/pinocchio.git "${pinocchio_source}" + fi -if [[ ! -f "${pinocchio_source}/cmake/base.cmake" ]]; then - git -C "${pinocchio_source}" submodule update --init --depth 1 cmake \ - || git -C "${pinocchio_source}" submodule update --init cmake + if [[ ! -f "${pinocchio_source}/cmake/base.cmake" ]]; then + git -C "${pinocchio_source}" submodule update --init --depth 1 cmake \ + || git -C "${pinocchio_source}" submodule update --init cmake + fi + + cmake -S "${pinocchio_source}" -B "${pinocchio_source}/build" \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX="${pinocchio_prefix}" \ + -DBUILD_BENCHMARK=OFF \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_PYTHON_INTERFACE=OFF \ + -DBUILD_TESTING=OFF \ + -DENABLE_TEMPLATE_INSTANTIATION=OFF + cmake --build "${pinocchio_source}/build" --parallel "${build_jobs}" + cmake --install "${pinocchio_source}/build" fi -cmake -S "${pinocchio_source}" -B "${pinocchio_source}/build" \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX="${pinocchio_prefix}" \ - -DBUILD_BENCHMARK=OFF \ - -DBUILD_EXAMPLES=OFF \ - -DBUILD_PYTHON_INTERFACE=OFF \ - -DBUILD_TESTING=OFF \ - -DENABLE_TEMPLATE_INSTANTIATION=OFF -cmake --build "${pinocchio_source}/build" --parallel "${build_jobs}" -cmake --install "${pinocchio_source}/build" +if [[ -f "${trossen_arm_prefix}/lib/libtrossen_arm.a" ]]; then + echo "libtrossen_arm ${trossen_arm_ref} already installed at ${trossen_arm_prefix}; skipping." +else + if [[ ! -d "${trossen_arm_source}/.git" ]]; then + git clone --branch "${trossen_arm_ref}" --depth 1 \ + https://github.com/TrossenRobotics/trossen_arm.git "${trossen_arm_source}" + fi + + # The repo ships a prebuilt static library + headers per OS/arch; "building" + # is just a CMake install into the prefix. + cmake -S "${trossen_arm_source}" -B "${trossen_arm_source}/build" \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX="${trossen_arm_prefix}" + cmake --build "${trossen_arm_source}/build" --parallel "${build_jobs}" + cmake --install "${trossen_arm_source}/build" +fi if [[ ! -f "${pinocchio_prefix}/lib/pkgconfig/pinocchio.pc" ]]; then echo "Pinocchio installation is missing pinocchio.pc: ${pinocchio_prefix}" >&2 @@ -91,6 +122,10 @@ if [[ ! -f "${cppzmq_prefix}/include/zmq.hpp" ]]; then echo "cppzmq installation is missing zmq.hpp: ${cppzmq_prefix}" >&2 exit 1 fi +if [[ ! -f "${trossen_arm_prefix}/lib/libtrossen_arm.a" ]]; then + echo "libtrossen_arm installation is missing libtrossen_arm.a: ${trossen_arm_prefix}" >&2 + exit 1 +fi cat < Date: Fri, 24 Jul 2026 13:40:42 -0700 Subject: [PATCH 2/5] config: Ethernet and serial arm connection types Two new connection forms alongside SocketCAN: - EthernetConnection: a whole-arm Ethernet controller (e.g. Trossen iNerve) at a fixed IPv4 address. - SerialConnection: a USB-serial bus-servo chain (e.g. SO-ARM101 FeeTech) at a /dev tty path. connection_for_interface() dispatches the interface part of a runtime bus spec onto the right type -- the three forms never overlap (IPv4 literal -> Ethernet, /dev path -> serial, otherwise SocketCAN) -- so runtime configs can drive all bus families through one entry point. ArmConfig.catalog_baudrate() surfaces the model catalog's bus baud rate (required for serial buses). Model-catalog entries for the new arms land with their driver stacks. Co-Authored-By: Claude Fable 5 --- src/openpi_control/__init__.py | 6 ++++ src/openpi_control/config.py | 61 +++++++++++++++++++++++++++++++++- tests/test_config.py | 23 ++++++++++++- 3 files changed, 88 insertions(+), 2 deletions(-) diff --git a/src/openpi_control/__init__.py b/src/openpi_control/__init__.py index 0cdec74..2038f9e 100644 --- a/src/openpi_control/__init__.py +++ b/src/openpi_control/__init__.py @@ -5,10 +5,13 @@ FOLLOWER_CONTROL_FREQUENCY_HZ, LEADER_CONTROL_FREQUENCY_HZ, ArmConfig, + EthernetConnection, InputLayout, ResolvedArmAssets, SafetyLimits, + SerialConnection, SocketCanConnection, + connection_for_interface, resolve_model_assets, ) from .exceptions import ( @@ -52,6 +55,7 @@ "ConfigurationError", "ConnectionUnavailableError", "EffectorState", + "EthernetConnection", "FollowerArm", "HardwareFaultError", "InputLayout", @@ -64,9 +68,11 @@ "RoleError", "SafetyLimits", "ResolvedArmAssets", + "SerialConnection", "SocketCanConnection", "StaleStateError", "StateTimeoutError", "TeleopPair", + "connection_for_interface", "resolve_model_assets", ] diff --git a/src/openpi_control/config.py b/src/openpi_control/config.py index fc19ac5..f224a20 100644 --- a/src/openpi_control/config.py +++ b/src/openpi_control/config.py @@ -2,6 +2,7 @@ from __future__ import annotations +import ipaddress import json import re from dataclasses import dataclass @@ -37,6 +38,54 @@ def __post_init__(self) -> None: raise ConfigurationError("SocketCAN interface must be a simple interface name") +@dataclass(frozen=True, slots=True) +class EthernetConnection: + """A whole-arm Ethernet controller (e.g. Trossen iNerve) at a fixed IPv4 address.""" + + ip: str + + def __post_init__(self) -> None: + try: + ipaddress.IPv4Address(self.ip) + except ValueError as err: + raise ConfigurationError( + f"invalid controller IPv4 address {self.ip!r} for Ethernet connection" + ) from err + + +@dataclass(frozen=True, slots=True) +class SerialConnection: + """A USB-serial bus-servo chain (e.g. SO-ARM101 FeeTech) at a tty device path.""" + + device: str + + def __post_init__(self) -> None: + if not self.device.startswith("/dev/"): + raise ConfigurationError( + f"serial connection device must be a /dev path, got {self.device!r}" + ) + + +ArmConnection = SocketCanConnection | EthernetConnection | SerialConnection + + +def connection_for_interface(interface: str) -> ArmConnection: + """Build the arm connection for one bus interface string from a runtime config. + + The runtime config stores the interface part of a bus spec ("can:", + "eth:", or "serial:"). The three forms never overlap: an IPv4 + literal selects a whole-arm Ethernet controller, a /dev path selects a + USB-serial bus, anything else is a SocketCAN interface name. + """ + if interface.startswith("/dev/"): + return SerialConnection(interface) + try: + ipaddress.IPv4Address(interface) + except ValueError: + return SocketCanConnection(interface) + return EthernetConnection(interface) + + @dataclass(frozen=True, slots=True) class SafetyLimits: max_bilateral_gain: float = 0.3 @@ -137,7 +186,7 @@ class ArmConfig: name: str model: str - connection: SocketCanConnection + connection: ArmConnection instance_config: Path | None = None effector_model: str | None = None effector_instance_config: Path | None = None @@ -236,3 +285,13 @@ def is_read_only(self) -> bool: """True when the model declares read_only (leader-only, no actuation, e.g. ARX_ENC).""" data = json.loads(self.resolve_assets().model_config.read_text()) return bool(data.get("read_only", False)) + + def catalog_baudrate(self) -> int: + """Bus baud rate declared by the model catalog (required for serial buses).""" + data = json.loads(self.resolve_assets().model_config.read_text()) + baudrate = data.get("catalog", {}).get("baudrate") + if not isinstance(baudrate, int) or baudrate <= 0: + raise ConfigurationError( + f"model {self.model!r} catalog does not declare a positive integer baudrate" + ) + return baudrate diff --git a/tests/test_config.py b/tests/test_config.py index c4425c2..8b95e20 100644 --- a/tests/test_config.py +++ b/tests/test_config.py @@ -3,7 +3,15 @@ import pytest -from openpi_control import ArmConfig, ConfigurationError, InputLayout, SocketCanConnection +from openpi_control import ( + ArmConfig, + ConfigurationError, + EthernetConnection, + InputLayout, + SerialConnection, + SocketCanConnection, + connection_for_interface, +) from openpi_control.config import SUPPORTED_MODELS from openpi_control.protocol import topics_for @@ -23,6 +31,19 @@ def test_physical_model_catalog_is_complete(model: str) -> None: assert json.loads(assets.model_config.read_text())["algo_type"] in ("Pinocchio", "KDL") +def test_connection_for_interface_dispatches_on_the_interface_form() -> None: + assert connection_for_interface("can_left") == SocketCanConnection("can_left") + assert connection_for_interface("192.168.1.11") == EthernetConnection("192.168.1.11") + assert connection_for_interface("/dev/serial/by-id/usb-test-if00") == SerialConnection( + "/dev/serial/by-id/usb-test-if00" + ) + + +def test_serial_connection_requires_a_dev_path() -> None: + with pytest.raises(ConfigurationError, match="/dev path"): + SerialConnection("ttyUSB0") + + def test_logical_identity_is_separate_from_instance_config(tmp_path: Path) -> None: canonical = ( ArmConfig("source", "Yam", SocketCanConnection("test")).resolve_assets().instance_config From e0f755ec4822d0429cb9db84f23987c210147352 Mon Sep 17 00:00:00 2001 From: Adnan Esmail Date: Fri, 24 Jul 2026 13:41:57 -0700 Subject: [PATCH 3/5] servos: ethernet and serial bus sessions; FeeTech and Trossen zeroing drivers - buses.py grows ethernet (Trossen iNerve reachability/session) and USB-serial (pyserial) session types beside SocketCAN, with the same check/open/receive surface the zeroing tools drive. - New per-family drivers: ft_serial (FeeTech SMS/STS protocol -- ping/scan/register read/write/checksum, EEPROM-lock-wrapped calibrate-middle set_zero, baudrate get/set) and trossen_eth (controller discovery, reachability probe, whole-arm EEPROM zeroing, IP provisioning and host-subnet helpers). - The registry gains a WHOLE_ARM_ZERO contract: controller-managed arms zero in one whole-arm operation instead of per-servo; the existing CAN/serial families declare False. Co-Authored-By: Claude Fable 5 --- src/openpi_control/servos/__init__.py | 19 +- src/openpi_control/servos/buses.py | 82 +++- src/openpi_control/servos/dm_can.py | 1 + src/openpi_control/servos/dxl_serial.py | 1 + src/openpi_control/servos/encos_can.py | 1 + src/openpi_control/servos/ft_serial.py | 231 ++++++++++ src/openpi_control/servos/trossen_eth.py | 528 +++++++++++++++++++++++ tests/test_servos.py | 368 +++++++++++++++- 8 files changed, 1199 insertions(+), 32 deletions(-) create mode 100644 src/openpi_control/servos/ft_serial.py create mode 100644 src/openpi_control/servos/trossen_eth.py diff --git a/src/openpi_control/servos/__init__.py b/src/openpi_control/servos/__init__.py index 98c3e0d..15ee8e8 100644 --- a/src/openpi_control/servos/__init__.py +++ b/src/openpi_control/servos/__init__.py @@ -12,10 +12,15 @@ string in the model JSON and in this table. Driver module API (uniform across families): - PORT_TYPE: str "can" or "serial" — which bus session the - driver needs (see ``buses.open_bus``) - set_zero(bus, servo_id) set the current position as the firmware zero; - returns None on success or an error string + PORT_TYPE: str "can", "serial", or "ethernet" — which bus + session the driver needs (see ``buses.open_bus``) + WHOLE_ARM_ZERO: bool False: the family zeroes one servo at a time and + defines ``set_zero(bus, servo_id)``. True: the + family (whole-arm Ethernet controllers, e.g. + Trossen iNerve) zeroes every joint in one + controller transaction and defines + ``set_zero_whole_arm(bus)`` instead. + set_zero* functions return None on success or an error string Adding a servo family: create ``_.py`` with that API (port the routine from robot-test ``pi_control/servos``), then register every canonical @@ -27,7 +32,7 @@ import types -from openpi_control.servos import dm_can, dxl_serial, encos_can +from openpi_control.servos import dm_can, dxl_serial, encos_can, ft_serial, trossen_eth # Servo model string -> driver module, or None for read-only encoders whose # zero is fixed in hardware (reported as skipped by the zeroing tool, never @@ -46,6 +51,10 @@ "Dynamixel XH430-W210": dxl_serial, "Dynamixel XC330-T288": dxl_serial, "Dynamixel XH430-W350": dxl_serial, + # FeeTech SMS/STS serial (SO-ARM100/101; also covers Hiwonder HX-30HM/HX-10HM). + "FeeTech STS3215": ft_serial, + # Whole-arm Ethernet controller joints (zeroed via one EEPROM write). + "Trossen WXAI Joint": trossen_eth, # Read-only encoders: no motor, zero reference fixed in hardware. "ARX Remote Encoder": None, "CAN Passive Encoder": None, diff --git a/src/openpi_control/servos/buses.py b/src/openpi_control/servos/buses.py index aecc5ce..c141367 100644 --- a/src/openpi_control/servos/buses.py +++ b/src/openpi_control/servos/buses.py @@ -1,31 +1,43 @@ """Bus sessions for servo maintenance tools, keyed by the driver's PORT_TYPE. -Servo drivers declare which transport they need ("can" or "serial"); this -module owns opening/validating that transport so tools never hard-code a bus -type. CAN is implemented (SocketCAN via python-can, with the settle timing -verified in robot-test). Serial is a declared seam: the checks work, but -opening a session raises until the first serial servo family (Dynamixel / -FeeTech) is actually integrated — port the session handling from robot-test -``pi_control/servos/dxl_serial.py`` (plain pyserial at the family's baud rate) -at that point. +Servo drivers declare which transport they need ("can", "serial", or +"ethernet"); this module owns opening/validating that transport so tools +never hard-code a bus type. CAN is implemented (SocketCAN via python-can, +with the settle timing verified in robot-test). Ethernet opens a Trossen +controller session (the "bus" is the vendor driver object). Serial opens a +plain pyserial session at the caller-provided baud rate (the model catalog's +``baudrate``); the protocol on top belongs to the servo driver module +(``ft_serial`` for FeeTech, ``dxl_serial`` once Dynamixel is integrated). """ from __future__ import annotations import contextlib +import ipaddress import pathlib import time from collections.abc import Iterator import can +import serial as pyserial +import trossen_arm + +from openpi_control.servos import trossen_eth PORT_TYPE_CAN = "can" PORT_TYPE_SERIAL = "serial" +PORT_TYPE_ETHERNET = trossen_eth.PORT_TYPE + +_SUPPORTED_PORT_TYPES = f"{PORT_TYPE_CAN}, {PORT_TYPE_SERIAL}, {PORT_TYPE_ETHERNET}" # robot-test waits 1 s after opening a CAN bus before the first frame; # adapters (especially SLCAN) drop the first frames when hit too early. _CAN_OPEN_SETTLE_S = 1.0 +# Serial per-read deadline: dominated by one USB latency window (up to 16 ms +# FTDI/CDC), so 100 ms leaves ample margin (robot-test ft_serial timing). +_SERIAL_READ_TIMEOUT_S = 0.1 + def check_interface(port_type: str, interface: str) -> str | None: """Return None when ``interface`` exists for ``port_type``, an error message otherwise.""" @@ -43,28 +55,56 @@ def check_interface(port_type: str, interface: str) -> str | None: f"serial device {interface!r} does not exist. Plug in the adapter and " "check the names with run/devices.sh (ls /dev/serial/by-id)." ) - raise SystemExit( - f"unknown port type {port_type!r}; supported: {PORT_TYPE_CAN}, {PORT_TYPE_SERIAL}" - ) + if port_type == PORT_TYPE_ETHERNET: + try: + ipaddress.IPv4Address(interface) + except ValueError: + return ( + f"Ethernet controller address {interface!r} is not a valid IPv4 address; " + "check the bus entry in the config TOML (eth:)." + ) + if trossen_eth.reachable(interface): + return None + return ( + f"Ethernet controller at {interface} did not answer a discovery probe. Check the " + "cable and power, and that the host has an address on the controller's subnet." + ) + raise SystemExit(f"unknown port type {port_type!r}; supported: {_SUPPORTED_PORT_TYPES}") @contextlib.contextmanager -def open_bus(port_type: str, interface: str) -> Iterator[can.BusABC]: - """Open a settled bus session on ``interface`` for drivers of ``port_type``.""" +def open_bus( + port_type: str, interface: str, *, baudrate: int | None = None +) -> Iterator[can.BusABC | trossen_arm.TrossenArmDriver | pyserial.Serial]: + """Open a settled bus session on ``interface`` for drivers of ``port_type``. + + Serial sessions require ``baudrate`` (the arm model catalog's ``baudrate`` + field) — the transport is protocol-neutral, so the caller must state the + bus speed explicitly. + """ if port_type == PORT_TYPE_CAN: with can.interface.Bus(channel=interface, interface="socketcan") as bus: time.sleep(_CAN_OPEN_SETTLE_S) yield bus return + if port_type == PORT_TYPE_ETHERNET: + # The "bus" is the vendor driver session; ``interface`` is the + # controller's IPv4 address. + with trossen_eth.open_session(interface) as driver: + yield driver + return if port_type == PORT_TYPE_SERIAL: - raise NotImplementedError( - "serial bus sessions are not integrated yet: port the pyserial session " - "handling from robot-test pi_control/servos/dxl_serial.py when the first " - "serial servo family is brought up" - ) - raise SystemExit( - f"unknown port type {port_type!r}; supported: {PORT_TYPE_CAN}, {PORT_TYPE_SERIAL}" - ) + if baudrate is None or baudrate <= 0: + raise SystemExit( + "serial bus sessions need an explicit positive baudrate " + "(the arm model catalog's 'baudrate' field)" + ) + with pyserial.Serial( + port=interface, baudrate=baudrate, timeout=_SERIAL_READ_TIMEOUT_S + ) as bus: + yield bus + return + raise SystemExit(f"unknown port type {port_type!r}; supported: {_SUPPORTED_PORT_TYPES}") def recv_from(bus: can.BusABC, expected_ids: tuple[int, ...], timeout_s: float) -> bool: diff --git a/src/openpi_control/servos/dm_can.py b/src/openpi_control/servos/dm_can.py index 703adbe..584ccf0 100644 --- a/src/openpi_control/servos/dm_can.py +++ b/src/openpi_control/servos/dm_can.py @@ -15,6 +15,7 @@ from openpi_control.servos import buses PORT_TYPE = buses.PORT_TYPE_CAN +WHOLE_ARM_ZERO = False _ZERO_PAYLOAD = bytes([0xFF] * 7 + [0xFE]) _RESPONSE_TIMEOUT_S = 0.05 diff --git a/src/openpi_control/servos/dxl_serial.py b/src/openpi_control/servos/dxl_serial.py index 0386cfe..0f6edaa 100644 --- a/src/openpi_control/servos/dxl_serial.py +++ b/src/openpi_control/servos/dxl_serial.py @@ -15,6 +15,7 @@ from openpi_control.servos import buses PORT_TYPE = buses.PORT_TYPE_SERIAL +WHOLE_ARM_ZERO = False def set_zero(bus: object, servo_id: int) -> str | None: diff --git a/src/openpi_control/servos/encos_can.py b/src/openpi_control/servos/encos_can.py index 41f1365..0ba89a7 100644 --- a/src/openpi_control/servos/encos_can.py +++ b/src/openpi_control/servos/encos_can.py @@ -17,6 +17,7 @@ from openpi_control.servos import buses PORT_TYPE = buses.PORT_TYPE_CAN +WHOLE_ARM_ZERO = False _CAN_ID_BROADCAST = 0x7FF _CMD_SET_ZERO = 0x03 diff --git a/src/openpi_control/servos/ft_serial.py b/src/openpi_control/servos/ft_serial.py new file mode 100644 index 0000000..d8f02f2 --- /dev/null +++ b/src/openpi_control/servos/ft_serial.py @@ -0,0 +1,231 @@ +"""FeeTech SMS/STS serial servo driver — packet layer, ping/scan, and zeroing. + +Ported from robot-test ``pi_control/servos/ft_serial.py``, reduced to what the +zeroing tool and the device wizard need for the STS3215 class (SO-ARM100/101; +the Hiwonder HX-30HM / HX-10HM in SO-ARM101 kits share the exact register +layout and protocol, so no separate servo model exists). + +The C++ mirror of this protocol lives in native/pi_control/src/pi_driver_ft.cpp. +""" + +from __future__ import annotations + +import logging +import struct + +import serial + +from openpi_control.servos import buses + +PORT_TYPE = buses.PORT_TYPE_SERIAL +WHOLE_ARM_ZERO = False + +# FeeTech instruction set (subset used here). +INST_PING = 0x01 +INST_READ = 0x02 +INST_WRITE = 0x03 + +ID_BROADCAST = 0xFE + +# SMS/STS control table (subset). +ADDR_BAUD_RATE = 6 +ADDR_TORQUE_ENABLE = 40 +ADDR_LOCK_FLAG = 55 +ADDR_PRESENT_POSITION = 56 + +# Writing 128 to Torque Enable calibrates the current position to the encoder +# center (2048) on SMS/STS servos — the standard STS3215 zeroing method (same +# operation as the FeeTech FD software "Calibrate middle" button). +TORQUE_ENABLE_CALIBRATE_MIDDLE = 128 + +# Baud Rate register (addr 6) value mapping for the STS3215 class. +BAUDRATE_REG_VALUES = { + 1000000: 0, + 500000: 1, + 250000: 2, + 128000: 3, + 115200: 4, + 76800: 5, + 57600: 6, + 38400: 7, +} + + +def check_sum(packet: bytes) -> int: + """FeeTech checksum: ~(sum of bytes after the FF FF header) & 0xFF.""" + return (~(sum(packet[2:]) & 0xFF)) & 0xFF + + +def build_packet(servo_id: int, instruction: int, parameters: bytes) -> bytes: + """Build an instruction packet: FF FF ID LEN INST [params] CHK.""" + packet = bytearray(b"\xff\xff") + packet += struct.pack(" bool: + """Write a register (INST_WRITE) and validate the status packet.""" + value_bytes = value.to_bytes(length, byteorder="little", signed=value < 0) + packet = build_packet(servo_id, INST_WRITE, struct.pack(" bytes | None: + """Read a register window (INST_READ); returns the parameter bytes or None.""" + packet = build_packet(servo_id, INST_READ, struct.pack(" bool: + """Ping one servo (INST_PING) and validate the status packet.""" + packet = build_packet(servo_id, INST_PING, b"") + bus.reset_input_buffer() + bus.write(packet) + bus.flush() + + response = bus.read(6) + if len(response) < 6: + return False + if response[:2] != b"\xff\xff" or response[2] != servo_id or response[3] != 0x02: + return False + return response[-1] == check_sum(response[:-1]) + + +def scan(bus: serial.Serial, servo_ids: tuple[int, ...]) -> list[int]: + """Ping each id in turn; returns the ids that answered.""" + return [servo_id for servo_id in servo_ids if ping(bus, servo_id)] + + +def torque_enable(bus: serial.Serial, servo_id: int, enable: bool | int) -> bool: + """Enable/disable torque output (also carries the calibrate-middle value 128).""" + return ft_write(bus, servo_id, ADDR_TORQUE_ENABLE, int(enable), 1) + + +def set_eeprom_lock(bus: serial.Serial, servo_id: int, locked: bool) -> bool: + """Set the EEPROM write Lock Flag (STS3215 ships write-locked).""" + return ft_write(bus, servo_id, ADDR_LOCK_FLAG, 1 if locked else 0, 1) + + +def get_baudrate(bus: serial.Serial, servo_id: int) -> int | None: + """Read the Baud Rate register and return it in bps, or None on failure.""" + response = ft_read(bus, servo_id, ADDR_BAUD_RATE, 1) + if response is None or len(response) < 1: + logging.getLogger(__name__).error("ID %d: failed to read Baud Rate register", servo_id) + return None + for bps, reg_value in BAUDRATE_REG_VALUES.items(): + if reg_value == response[0]: + return bps + logging.getLogger(__name__).error( + "ID %d: unknown Baud Rate register value %d", servo_id, response[0] + ) + return None + + +def set_baudrate(bus: serial.Serial, servo_id: int, new_baudrate: int) -> bool: + """Change the servo's UART baudrate and follow it on the host side. + + Steps: unlock EEPROM, write the Baud Rate register (takes effect + immediately, no reboot instruction exists in the FeeTech protocol), switch + the host serial port to the new baud, re-lock EEPROM, verify with a ping. + On failure the host baud is restored. + """ + if new_baudrate not in BAUDRATE_REG_VALUES: + logging.getLogger(__name__).error( + "ID %d: baudrate %d is not supported (allowed: %s)", + servo_id, + new_baudrate, + sorted(BAUDRATE_REG_VALUES), + ) + return False + + old_baudrate = int(bus.baudrate) + if not set_eeprom_lock(bus, servo_id, locked=False): + logging.getLogger(__name__).error( + "ID %d: failed to unlock EEPROM for baudrate change", servo_id + ) + return False + + if not ft_write(bus, servo_id, ADDR_BAUD_RATE, BAUDRATE_REG_VALUES[new_baudrate], 1): + logging.getLogger(__name__).error("ID %d: failed to write Baud Rate register", servo_id) + set_eeprom_lock(bus, servo_id, locked=True) + return False + + # The servo switches immediately after the write; follow it on the host side. + bus.baudrate = int(new_baudrate) + bus.reset_input_buffer() + + if not set_eeprom_lock(bus, servo_id, locked=True): + logging.getLogger(__name__).error( + "ID %d: failed to re-lock EEPROM after baudrate change", servo_id + ) + + if not ping(bus, servo_id): + logging.getLogger(__name__).error( + "ID %d: no answer at new baudrate %d; restoring host baud %d", + servo_id, + new_baudrate, + old_baudrate, + ) + bus.baudrate = old_baudrate + return False + return True + + +def set_zero(bus: serial.Serial, servo_id: int) -> str | None: + """Calibrate the current position to the encoder center (STS3215 zeroing). + + Torque Enable = 128 sets the present position to step 2048; the write is + wrapped with the EEPROM Lock Flag unlock/re-lock pair the STS3215 firmware + requires. Returns None on success or an error detail string. + """ + if not set_eeprom_lock(bus, servo_id, locked=False): + return "failed to unlock EEPROM for calibrate-middle" + calibrated = torque_enable(bus, servo_id, TORQUE_ENABLE_CALIBRATE_MIDDLE) + if not set_eeprom_lock(bus, servo_id, locked=True): + return "failed to re-lock EEPROM after calibrate-middle" + if not calibrated: + return "calibrate-middle command was not acknowledged" + return None diff --git a/src/openpi_control/servos/trossen_eth.py b/src/openpi_control/servos/trossen_eth.py new file mode 100644 index 0000000..cbd3e98 --- /dev/null +++ b/src/openpi_control/servos/trossen_eth.py @@ -0,0 +1,528 @@ +"""Trossen iNerve Ethernet controller helpers: discovery, zeroing, and IP provisioning. + +Central home for every Python-side interaction with the ``trossen_arm`` SDK +(same pinned release as the C++ ``libtrossen_arm`` linked by the native node, +so driver <-> controller firmware compatibility stays aligned): + +- ``discover()`` read-only /24 TCP sweep for controllers +- ``reachable()`` single-host reachability gate (native connect) +- ``open_session()`` configured driver session (idled + cleaned up) +- ``set_zero_whole_arm()`` persist the current pose as zero (one EEPROM write) +- ``read_ip_settings()`` / ``provision_manual_ip()`` IP provisioning +- ``local_subnet_prefixes()`` / ``candidate_ips()`` / ``ip_in_use()`` + host-side helpers for the provisioning flow +- ``wired_up_interfaces()`` / ``free_host_ip()`` / ``add_persistent_address()`` + / ``add_temporary_address()`` / ``remove_temporary_address()`` + host NIC setup for the factory controller subnet +""" + +from __future__ import annotations + +import contextlib +import ipaddress +import logging +import pathlib +import shutil +import subprocess +import threading +import time +from collections.abc import Iterator +from concurrent.futures import ThreadPoolExecutor +from dataclasses import dataclass + +import trossen_arm + +PORT_TYPE = "ethernet" + +# All joint offsets are persisted in ONE EEPROM write; per-servo zero commands +# would wear the controller flash once per joint (see set_zero_whole_arm). +WHOLE_ARM_ZERO = True + +# Trossen model string accepted in model configs (must match the C++ +# pi_trossen_shim.cpp TROSSEN_MODEL_WXAI_V0 constant). +TROSSEN_MODEL_WXAI_V0 = "wxai_v0" + +# Vendor factory default: every controller ships at this address, so two +# factory-fresh arms on one LAN collide until they are provisioned. +FACTORY_DEFAULT_IP = "192.168.1.2" + +# Default provisioning target per arm slot: the host octet on the controller's +# /24 subnet. When a base octet is already in use on the LAN the walk +# continues at base + k * PROVISION_HOST_OCTET_STEP (see candidate_ips), so +# every slot keeps a disjoint, predictable candidate set (e.g. follower_left: +# .11 / .111 / .211). +PROVISION_SLOT_HOST_OCTETS = { + "follower_left": 11, + "follower_right": 12, + "leader_left": 21, + "leader_right": 22, +} +PROVISION_HOST_OCTET_STEP = 100 + +# Host-side address on the controller subnet (the wizard can add it via +# NetworkManager when missing). The walk starts high to stay clear of typical +# LAN allocations and skips every arm provisioning candidate octet. +HOST_OCTET_START = 200 +_ARM_CANDIDATE_OCTETS = frozenset( + base + step + for base in PROVISION_SLOT_HOST_OCTETS.values() + for step in range(0, 255, PROVISION_HOST_OCTET_STEP) + if base + step < 255 +) + +# Duplicate-address probes for a provisioning candidate. ping catches any live +# IP stack; arping -D (duplicate address detection) additionally catches hosts +# that drop ICMP. One second is enough for a LAN device to defend its address. +_PING_TIMEOUT_S = 1 +_ARPING_TIMEOUT_S = 2 + +# Discovery probes every host of a /24 subnet with a short per-host TCP +# timeout. The vendor discover() probes serially, so the host range is split +# into chunks probed on parallel threads (the SDK releases the GIL during the +# socket wait). Worker count is capped at 16: measured on hardware in +# robot-test, 32 concurrent probes flood the link and the controller's reply +# gets dropped past the per-host timeout (missed discovery). +DISCOVERY_TIMEOUT_S = 0.05 +DISCOVERY_CHUNK_HOSTS = 16 +DISCOVERY_MAX_WORKERS = 16 + +# TCP handshake budget for an actual configure(): generous enough for a +# booting controller, small enough to fail fast on a wrong IP. +CONFIGURE_TIMEOUT_S = 10.0 + +# Single-host reachability gate: a controller that is momentarily busy (the +# config port is single-client) can miss one probe, so a few attempts with a +# longer per-host timeout are used. +REACHABILITY_TIMEOUT_S = 0.5 +REACHABILITY_ATTEMPTS = 3 +REACHABILITY_RETRY_DELAY_S = 1.0 + +# iNerve controllers use a PJRC Ethernet MAC. This lets the wizard distinguish +# a controller whose network stack answers but whose Trossen SDK service does +# not from a controller that is absent from the network entirely. +CONTROLLER_MAC_PREFIXES = ("04:e9:e5",) +_PING_REACHABILITY_TIMEOUT_S = 1 +POWER_CYCLE_OFF_S = 10 +CONTROLLER_BOOT_WAIT_S = 20 + +# After a provisioning reboot the controller needs time to come back before +# the re-discovery verification can succeed. +PROVISION_REBOOT_WAIT_S = 10.0 + +# Guards discover(): concurrent sweeps double the worker count, flood the +# link, and can make the controller's single-client config port stop +# answering real connects for a while. +_DISCOVER_SWEEP_LOCK = threading.Lock() + + +@dataclass(frozen=True) +class DiscoveredController: + """One controller that answered a discovery probe.""" + + address: str + model: str + firmware_version: str + error_state: str + + +@dataclass(frozen=True) +class IpSettings: + """Controller EEPROM network settings (read via a configure session).""" + + manual_ip: str + ip_method: str + firmware_version: str + + +def discover(subnets: list[str]) -> list[DiscoveredController]: + """Scan IPv4 /24 ``subnets`` (prefixes like ``"192.168.1"``) for controllers. + + Read-only: probes TCP connects only, never changes host or controller + network state. + """ + with _DISCOVER_SWEEP_LOCK: + return _discover_locked(subnets) + + +def reachable(ip: str) -> bool: + """True when the controller at ``ip`` answers a discovery probe.""" + subnet, host = _split_host(ip) + for attempt in range(REACHABILITY_ATTEMPTS): + if attempt > 0: + time.sleep(REACHABILITY_RETRY_DELAY_S) + with _DISCOVER_SWEEP_LOCK: + found = trossen_arm.TrossenArmDriver.discover( + subnet=subnet, ip_start=host, ip_end=host, timeout=REACHABILITY_TIMEOUT_S + ) + if found: + return True + return False + + +def ping_reachable_controller_ips(subnets: list[str]) -> list[str]: + """Return controller-like hosts that answer ping on the scanned subnets. + + A discovery sweep populates the local ARP/neighbor table even when the + controller's TCP service is unavailable. The known controller MAC prefix + filters out unrelated LAN hosts before the explicit ping check. + """ + result = subprocess.run( + ["ip", "neigh", "show"], + capture_output=True, + text=True, + check=False, + ) + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + logging.getLogger(__name__).warning("Could not inspect the IP neighbor table: %s", detail) + return [] + + candidates: list[str] = [] + for line in result.stdout.splitlines(): + parts = line.split() + if len(parts) < 5 or "lladdr" not in parts: + continue + ip = parts[0] + try: + address = ipaddress.IPv4Address(ip) + except ipaddress.AddressValueError: + continue + prefix = ".".join(str(address).split(".")[:3]) + mac = parts[parts.index("lladdr") + 1].lower() + if prefix not in subnets or not mac.startswith(CONTROLLER_MAC_PREFIXES): + continue + ping = subprocess.run( + ["ping", "-c", "1", "-W", str(_PING_REACHABILITY_TIMEOUT_S), str(address)], + capture_output=True, + text=True, + check=False, + ) + if ping.returncode == 0: + candidates.append(str(address)) + return sorted(set(candidates), key=ipaddress.IPv4Address) + + +@contextlib.contextmanager +def open_session(ip: str, *, clear_error: bool = False) -> Iterator[trossen_arm.TrossenArmDriver]: + """Configured driver session for maintenance work; idled and cleaned up on exit. + + The follower end-effector mass set only matters for the controller's own + gravity compensation while idle, which is correct for both roles at + standstill (the maintenance path never drives the arm). + """ + driver = trossen_arm.TrossenArmDriver() + driver.configure( + trossen_arm.Model.wxai_v0, + trossen_arm.StandardEndEffector.wxai_v0_follower, + str(ip), + clear_error, + CONFIGURE_TIMEOUT_S, + ) + try: + yield driver + finally: + try: + driver.set_all_modes(trossen_arm.Mode.idle) + finally: + driver.cleanup() + + +def set_zero_whole_arm(driver: trossen_arm.TrossenArmDriver) -> str | None: + """Persist the current pose as zero via position_offset (one EEPROM write). + + Trossen semantics (docs "position_offset"): position_motor = position + + position_offset, i.e. the offset is subtracted from motor feedback. To + make the current pose read 0: new_offset = old_offset + current_position. + + Returns None on success or an error message (arm_zero driver contract). + """ + positions = driver.get_all_positions() + characteristics = driver.get_joint_characteristics() + if len(characteristics) != len(positions): + return ( + f"joint characteristics size mismatch: {len(characteristics)} characteristics " + f"vs {len(positions)} positions" + ) + for characteristic, position in zip(characteristics, positions, strict=True): + characteristic.position_offset = float(characteristic.position_offset) + float(position) + driver.set_joint_characteristics(characteristics) + logging.getLogger(__name__).info( + "Trossen zero set at current pose (new position_offsets: %s)", + [round(float(c.position_offset), 4) for c in characteristics], + ) + return None + + +def read_ip_settings(ip: str) -> IpSettings: + """Read the controller's EEPROM network settings (no changes).""" + with open_session(ip) as driver: + return IpSettings( + manual_ip=str(driver.get_manual_ip()), + ip_method=str(driver.get_ip_method()), + firmware_version=str(driver.get_controller_version()), + ) + + +def provision_manual_ip(current_ip: str, new_ip: str) -> None: + """Persist ``new_ip`` as the controller's manual IP and reboot it. + + The controller writes the address to EEPROM and only applies it after the + reboot; the caller must re-discover the controller at ``new_ip`` to verify + (see PROVISION_REBOOT_WAIT_S). Raises on any SDK failure. + """ + ipaddress.IPv4Address(new_ip) # Fast-fail before touching the controller. + driver = trossen_arm.TrossenArmDriver() + driver.configure( + trossen_arm.Model.wxai_v0, + trossen_arm.StandardEndEffector.wxai_v0_follower, + str(current_ip), + False, + CONFIGURE_TIMEOUT_S, + ) + driver.set_manual_ip(str(new_ip)) + driver.set_ip_method(trossen_arm.IPMethod.manual) + # cleanup(True) reboots the controller so the EEPROM settings take effect. + driver.cleanup(True) + logging.getLogger(__name__).info( + "Trossen controller IP provisioned: %s -> %s (EEPROM, controller rebooting)", + current_ip, + new_ip, + ) + + +def local_subnet_prefixes() -> list[str]: + """The /24 prefixes (``"a.b.c"``) of all global IPv4 addresses of this host. + + Read-only (``ip -4 addr show``); loopback and link-local are excluded. + These are the subnets a discovery sweep can actually reach. + """ + result = subprocess.run( + ["ip", "-4", "-o", "addr", "show", "scope", "global"], + capture_output=True, + text=True, + check=True, + ) + prefixes: list[str] = [] + for line in result.stdout.splitlines(): + parts = line.split() + if "inet" not in parts: + continue + address = parts[parts.index("inet") + 1].split("/")[0] + prefix = ".".join(address.split(".")[:3]) + if prefix.startswith(("127.", "169.254.")) or prefix in prefixes: + continue + prefixes.append(prefix) + return prefixes + + +def candidate_ips(subnet: str, slot: str) -> list[str]: + """Provisioning candidate addresses for ``slot`` on the ``"a.b.c"`` subnet. + + The walk starts at the slot's reserved host octet and steps by + PROVISION_HOST_OCTET_STEP, keeping each slot's candidates disjoint from + every other slot's. When all candidates are in use the caller must ask + for a manual address. + """ + base = PROVISION_SLOT_HOST_OCTETS[slot] + return [f"{subnet}.{octet}" for octet in range(base, 255, PROVISION_HOST_OCTET_STEP)] + + +def ip_in_use(ip: str, interface: str | None = None) -> bool: + """Best-effort duplicate-address check for a provisioning candidate. + + True when anything on the LAN answers a ping or an ``arping -D`` + duplicate-address probe. arping needs the outgoing ``interface`` and is + skipped when unavailable (not installed, or no raw-socket privilege) — + ping alone still catches every normal IP stack. + """ + ping = subprocess.run( + ["ping", "-c", "1", "-W", str(_PING_TIMEOUT_S), str(ip)], + capture_output=True, + text=True, + check=False, + ) + if ping.returncode == 0: + return True + if interface is None or shutil.which("arping") is None: + return False + # arping -D exits 0 when the address is free, non-zero when a reply came + # back (duplicate) — but also on errors (e.g. missing raw-socket + # privilege), so only a reply actually printed counts as "in use". + arping = subprocess.run( + ["arping", "-D", "-c", "2", "-w", str(_ARPING_TIMEOUT_S), "-I", str(interface), str(ip)], + capture_output=True, + text=True, + check=False, + ) + return arping.returncode != 0 and "reply from" in arping.stdout.lower() + + +def wired_up_interfaces(sys_class_net: pathlib.Path = pathlib.Path("/sys/class/net")) -> list[str]: + """Wired Ethernet interfaces whose link is up, sorted by name. + + Wireless interfaces are excluded (adding addresses there can disturb the + Wi-Fi LAN), as are loopback and non-Ethernet devices. + """ + interfaces: list[str] = [] + if not sys_class_net.is_dir(): + return interfaces + for iface in sys_class_net.iterdir(): + if (iface / "wireless").exists(): + continue + try: + # ARPHRD_ETHER == 1; excludes loopback (772), CAN (280), ... + if (iface / "type").read_text().strip() != "1": + continue + if (iface / "operstate").read_text().strip() != "up": + continue + except OSError: + continue # interface disappeared mid-scan + interfaces.append(iface.name) + return sorted(interfaces) + + +def free_host_ip(subnet: str, interface: str) -> str | None: + """A conflict-free host address on the ``"a.b.c"`` /24 for this machine. + + Walks from HOST_OCTET_START upward, skipping every arm provisioning + candidate octet and every address something on the LAN already answers + for (ping + arping duplicate detection via ip_in_use). None when the + whole range is in use. + """ + for octet in range(HOST_OCTET_START, 255): + if octet in _ARM_CANDIDATE_OCTETS: + continue + candidate = f"{subnet}.{octet}" + if not ip_in_use(candidate, interface): + return candidate + return None + + +def add_temporary_address(interface: str, cidr: str) -> None: + """Add ``cidr`` to ``interface`` until reboot/removal (``sudo ip addr add``). + + Used to probe which NIC reaches the arm controllers before anything is + persisted; pair with remove_temporary_address. Raises RuntimeError on + failure. + """ + result = subprocess.run( + ["sudo", "ip", "addr", "add", str(cidr), "dev", str(interface)], + capture_output=True, + text=True, + check=False, + ) + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + raise RuntimeError(f"adding temporary {cidr} to {interface} failed: {detail}") + + +def remove_temporary_address(interface: str, cidr: str) -> None: + """Remove an address added by add_temporary_address (best effort, logged).""" + result = subprocess.run( + ["sudo", "ip", "addr", "del", str(cidr), "dev", str(interface)], + capture_output=True, + text=True, + check=False, + ) + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + logging.getLogger(__name__).warning( + "Removing temporary %s from %s failed: %s", cidr, interface, detail + ) + + +def add_persistent_address(interface: str, cidr: str) -> None: + """Add ``cidr`` (e.g. "192.168.1.200/24") to ``interface`` via NetworkManager. + + The address is ADDED next to the connection's existing addresses (DHCP + stays untouched) and persists across reboots. Raises RuntimeError when the + interface has no active NetworkManager connection or nmcli fails — the + caller falls back to printing manual setup instructions. + """ + if shutil.which("nmcli") is None: + raise RuntimeError( + "nmcli not found — configure the address manually " + "(see packages/openpi-runtime/docs/trossen/README.md)" + ) + show = subprocess.run( + ["nmcli", "-g", "GENERAL.CONNECTION", "device", "show", str(interface)], + capture_output=True, + text=True, + check=False, + ) + connection = show.stdout.strip() + if show.returncode != 0 or not connection or connection == "--": + raise RuntimeError( + f"interface {interface} has no active NetworkManager connection — " + "configure the address manually (see packages/openpi-runtime/docs/trossen/README.md)" + ) + for command in ( + ["sudo", "nmcli", "connection", "modify", connection, "+ipv4.addresses", str(cidr)], + ["sudo", "nmcli", "connection", "up", connection], + ): + result = subprocess.run(command, capture_output=True, text=True, check=False) + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + raise RuntimeError(f"{' '.join(command)} failed: {detail}") + logging.getLogger(__name__).info( + "Added %s to NetworkManager connection %r (interface %s)", cidr, connection, interface + ) + + +def interface_for_subnet(subnet: str) -> str | None: + """The host interface owning an address on the ``"a.b.c"`` /24, if any.""" + result = subprocess.run( + ["ip", "-4", "-o", "addr", "show", "scope", "global"], + capture_output=True, + text=True, + check=True, + ) + for line in result.stdout.splitlines(): + parts = line.split() + if "inet" not in parts: + continue + address = parts[parts.index("inet") + 1].split("/")[0] + if ".".join(address.split(".")[:3]) == subnet: + return parts[1] + return None + + +def _discover_locked(subnets: list[str]) -> list[DiscoveredController]: + def _probe_chunk(subnet: str, ip_start: int, ip_end: int) -> list[DiscoveredController]: + return [ + DiscoveredController( + address=str(found.ip), + model=str(trossen_arm.MODEL_NAME[found.model]), + firmware_version=str(found.firmware_version), + error_state=str(trossen_arm.ERROR_INFORMATION[found.error_state]), + ) + for found in trossen_arm.TrossenArmDriver.discover( + subnet=subnet, + ip_start=ip_start, + ip_end=ip_end, + timeout=DISCOVERY_TIMEOUT_S, + ) + ] + + chunks = [ + (subnet, start, min(start + DISCOVERY_CHUNK_HOSTS - 1, 254)) + for subnet in subnets + for start in range(1, 255, DISCOVERY_CHUNK_HOSTS) + ] + results: list[DiscoveredController] = [] + if chunks: + with ThreadPoolExecutor(max_workers=DISCOVERY_MAX_WORKERS) as pool: + for chunk_result in pool.map(lambda args: _probe_chunk(*args), chunks): + results.extend(chunk_result) + if results: + logging.getLogger(__name__).info( + "Discovered %d Trossen controller(s): %s", len(results), [r.address for r in results] + ) + return results + + +def _split_host(ip: str) -> tuple[str, int]: + """Split ``"a.b.c.d"`` into the ``"a.b.c"`` prefix and the host octet.""" + ipaddress.IPv4Address(ip) + octets = str(ip).split(".") + return ".".join(octets[:3]), int(octets[3]) diff --git a/tests/test_servos.py b/tests/test_servos.py index e140d83..864ce1c 100644 --- a/tests/test_servos.py +++ b/tests/test_servos.py @@ -8,7 +8,7 @@ import pytest from openpi_control import servos -from openpi_control.servos import buses, dm_can, dxl_serial, encos_can +from openpi_control.servos import buses, dm_can, dxl_serial, encos_can, ft_serial, trossen_eth _ENCOS_BROADCAST_ID = 0x7FF @@ -42,6 +42,8 @@ def test_registry_resolves_every_model_json_servo() -> None: assert servos.zero_driver("DM J4340") is dm_can assert servos.zero_driver("Encos EC-A4310-P2-36") is encos_can assert servos.zero_driver("Dynamixel XM430-W210") is dxl_serial + assert servos.zero_driver("FeeTech STS3215") is ft_serial + assert servos.zero_driver("Trossen WXAI Joint") is trossen_eth assert servos.zero_driver("ARX Remote Encoder") is None assert servos.zero_driver("CAN Passive Encoder") is None @@ -51,10 +53,19 @@ def test_registry_rejects_unknown_model() -> None: servos.zero_driver("Unknown Servo 9000") -def test_every_driver_declares_a_known_port_type() -> None: +def test_every_driver_declares_a_known_port_type_and_zero_entrypoint() -> None: for driver in servos.SERVO_ZERO_DRIVERS.values(): - if driver is not None: - assert driver.PORT_TYPE in (buses.PORT_TYPE_CAN, buses.PORT_TYPE_SERIAL) + if driver is None: + continue + assert driver.PORT_TYPE in ( + buses.PORT_TYPE_CAN, + buses.PORT_TYPE_SERIAL, + buses.PORT_TYPE_ETHERNET, + ) + if driver.WHOLE_ARM_ZERO: + assert callable(driver.set_zero_whole_arm) + else: + assert callable(driver.set_zero) def test_dm_zero_acknowledged() -> None: @@ -106,12 +117,153 @@ def test_dxl_zero_fails_fast_until_integrated() -> None: dxl_serial.set_zero(object(), 1) -def test_serial_bus_session_fails_fast_until_integrated() -> None: - with pytest.raises(NotImplementedError, match="serial bus sessions"): +def test_serial_bus_session_requires_a_baudrate() -> None: + with pytest.raises(SystemExit, match="explicit positive baudrate"): with buses.open_bus(buses.PORT_TYPE_SERIAL, "/dev/ttyUSB_test"): pass +def test_serial_bus_session_opens_pyserial_with_catalog_baudrate( + monkeypatch: pytest.MonkeyPatch, +) -> None: + opened: dict[str, object] = {} + + class _FakePortSession: + def __init__(self, *, port: str, baudrate: int, timeout: float) -> None: + opened.update(port=port, baudrate=baudrate, timeout=timeout) + + def __enter__(self) -> _FakePortSession: + return self + + def __exit__(self, *_: object) -> None: + pass + + monkeypatch.setattr(buses.pyserial, "Serial", _FakePortSession) + with buses.open_bus(buses.PORT_TYPE_SERIAL, "/dev/ttyUSB_test", baudrate=1000000) as bus: + assert isinstance(bus, _FakePortSession) + assert opened["port"] == "/dev/ttyUSB_test" + assert opened["baudrate"] == 1000000 + assert opened["timeout"] == pytest.approx(0.1) + + +# --- FeeTech SMS/STS (ft_serial) ------------------------------------------ + + +class _FakeSerial: + """Minimal pyserial double: records writes, replays scripted response bytes.""" + + def __init__(self, responses: list[bytes]) -> None: + self.written: list[bytes] = [] + self.baudrate = 1000000 + self._responses = responses + self._buffer = b"" + + def write(self, data: bytes) -> int: + self.written.append(bytes(data)) + return len(data) + + def flush(self) -> None: + pass + + def reset_input_buffer(self) -> None: + self._buffer = b"" + + def read(self, size: int) -> bytes: + if not self._buffer and self._responses: + self._buffer = self._responses.pop(0) + result, self._buffer = self._buffer[:size], self._buffer[size:] + return result + + +def _ft_status(servo_id: int, *, error: int = 0, params: bytes = b"") -> bytes: + """A FeeTech status packet: FF FF ID LEN ERR [params] CHK.""" + packet = bytearray(b"\xff\xff") + bytes([servo_id, 2 + len(params), error]) + params + packet.append(ft_serial.check_sum(packet)) + return bytes(packet) + + +def test_ft_build_packet_checksum_matches_protocol() -> None: + # PING to servo 1: FF FF 01 02 01 FB (complement checksum over ID..params). + packet = ft_serial.build_packet(1, ft_serial.INST_PING, b"") + assert packet == bytes([0xFF, 0xFF, 0x01, 0x02, 0x01, 0xFB]) + + +def test_ft_ping_and_scan_report_answering_ids() -> None: + # Servo 2 stays silent (empty response); 1 and 3 answer valid status packets. + bus = _FakeSerial([_ft_status(1), b"", _ft_status(3)]) + assert ft_serial.scan(bus, (1, 2, 3)) == [1, 3] + + +def test_ft_read_returns_register_bytes() -> None: + bus = _FakeSerial([_ft_status(1, params=bytes([0x04]))]) + assert ft_serial.ft_read(bus, 1, ft_serial.ADDR_BAUD_RATE, 1) == bytes([0x04]) + # The read request names the address and length. + assert bus.written[0][5] == ft_serial.ADDR_BAUD_RATE + assert bus.written[0][6] == 1 + + +def test_ft_read_rejects_corrupted_checksum() -> None: + good = _ft_status(1, params=bytes([0x04])) + corrupted = good[:-1] + bytes([good[-1] ^ 0xFF]) + bus = _FakeSerial([corrupted]) + assert ft_serial.ft_read(bus, 1, ft_serial.ADDR_BAUD_RATE, 1) is None + + +def test_ft_write_reports_servo_error_bits() -> None: + bus = _FakeSerial([_ft_status(1, error=0x20)]) + assert ft_serial.ft_write(bus, 1, ft_serial.ADDR_TORQUE_ENABLE, 1, 1) is False + + +def test_ft_set_zero_wraps_calibrate_middle_in_eeprom_unlock_lock() -> None: + bus = _FakeSerial([_ft_status(2), _ft_status(2), _ft_status(2)]) + assert ft_serial.set_zero(bus, 2) is None + # Packet layout: FF FF ID LEN INST ADDR VALUE CHK. + addresses = [packet[5] for packet in bus.written] + values = [packet[6] for packet in bus.written] + assert addresses == [ + ft_serial.ADDR_LOCK_FLAG, + ft_serial.ADDR_TORQUE_ENABLE, + ft_serial.ADDR_LOCK_FLAG, + ] + assert values == [0, ft_serial.TORQUE_ENABLE_CALIBRATE_MIDDLE, 1] + + +def test_ft_set_zero_reports_unlock_failure() -> None: + bus = _FakeSerial([]) # the unlock write times out + error = ft_serial.set_zero(bus, 2) + assert error is not None and "unlock" in error + + +def test_ft_baudrate_register_values_are_reversible() -> None: + mapping = ft_serial.BAUDRATE_REG_VALUES + assert mapping[1000000] == 0 + assert len(set(mapping.values())) == len(mapping) + + +def test_ft_get_baudrate_reads_the_register() -> None: + bus = _FakeSerial([_ft_status(1, params=bytes([ft_serial.BAUDRATE_REG_VALUES[115200]]))]) + assert ft_serial.get_baudrate(bus, 1) == 115200 + + +def test_ft_set_baudrate_switches_the_host_side_too() -> None: + bus = _FakeSerial([_ft_status(1), _ft_status(1), _ft_status(1), _ft_status(1)]) + assert ft_serial.set_baudrate(bus, 1, 500000) is True + assert bus.baudrate == 500000 + + +def test_ft_set_baudrate_restores_host_baud_when_servo_goes_silent() -> None: + # unlock ack + write ack, then silence: no lock ack, no ping answer. + bus = _FakeSerial([_ft_status(1), _ft_status(1)]) + assert ft_serial.set_baudrate(bus, 1, 500000) is False + assert bus.baudrate == 1000000 + + +def test_ft_set_baudrate_rejects_unsupported_rates() -> None: + bus = _FakeSerial([]) + assert ft_serial.set_baudrate(bus, 1, 12345) is False + assert bus.written == [] + + def test_check_interface_serial_uses_device_path(tmp_path: pathlib.Path) -> None: device = tmp_path / "ttyUSB_test" assert buses.check_interface(buses.PORT_TYPE_SERIAL, str(device)) is not None @@ -122,3 +274,207 @@ def test_check_interface_serial_uses_device_path(tmp_path: pathlib.Path) -> None def test_check_interface_can_reports_missing_interface() -> None: error = buses.check_interface(buses.PORT_TYPE_CAN, "can_does_not_exist") assert error is not None and "does not exist" in error + + +class _FakeJointCharacteristic: + def __init__(self, position_offset: float) -> None: + self.position_offset = position_offset + + +class _FakeTrossenDriver: + """trossen_arm.TrossenArmDriver double for the whole-arm zeroing routine.""" + + def __init__(self, positions: list[float], offsets: list[float]) -> None: + self._positions = positions + self._characteristics = [_FakeJointCharacteristic(offset) for offset in offsets] + self.written: list[_FakeJointCharacteristic] | None = None + + def get_all_positions(self) -> list[float]: + return self._positions + + def get_joint_characteristics(self) -> list[_FakeJointCharacteristic]: + return self._characteristics + + def set_joint_characteristics(self, characteristics: list[_FakeJointCharacteristic]) -> None: + self.written = characteristics + + +def test_trossen_whole_arm_zero_accumulates_position_offsets() -> None: + # Trossen semantics: new_offset = old_offset + current_position makes the + # current pose read zero (position_motor = position + position_offset). + driver = _FakeTrossenDriver(positions=[0.1, -0.2, 0.0], offsets=[1.0, 2.0, -0.5]) + assert trossen_eth.set_zero_whole_arm(driver) is None + assert driver.written is not None + assert [c.position_offset for c in driver.written] == pytest.approx([1.1, 1.8, -0.5]) + + +def test_trossen_whole_arm_zero_reports_size_mismatch() -> None: + driver = _FakeTrossenDriver(positions=[0.1, 0.2], offsets=[0.0]) + error = trossen_eth.set_zero_whole_arm(driver) + assert error is not None and "size mismatch" in error + assert driver.written is None + + +def test_trossen_declares_whole_arm_ethernet_family() -> None: + assert trossen_eth.PORT_TYPE == buses.PORT_TYPE_ETHERNET + assert trossen_eth.WHOLE_ARM_ZERO is True + + +def test_check_interface_ethernet_rejects_invalid_ip() -> None: + error = buses.check_interface(buses.PORT_TYPE_ETHERNET, "not-an-ip") + assert error is not None and "IPv4" in error + + +def test_check_interface_ethernet_probes_reachability(monkeypatch: pytest.MonkeyPatch) -> None: + probed: list[str] = [] + monkeypatch.setattr(trossen_eth, "reachable", lambda ip: probed.append(ip) or True) + assert buses.check_interface(buses.PORT_TYPE_ETHERNET, "192.168.1.11") is None + assert probed == ["192.168.1.11"] + monkeypatch.setattr(trossen_eth, "reachable", lambda ip: False) + error = buses.check_interface(buses.PORT_TYPE_ETHERNET, "192.168.1.11") + assert error is not None and "did not answer" in error + + +def test_ping_reachable_controller_ips_filters_by_subnet_mac_and_ping( + monkeypatch: pytest.MonkeyPatch, +) -> None: + class _Result: + def __init__(self, *, stdout: str = "", returncode: int = 0) -> None: + self.stdout = stdout + self.stderr = "" + self.returncode = returncode + + neighbors = "\n".join( + ( + "192.168.1.11 dev eth0 lladdr 04:e9:e5:11:22:33 REACHABLE", + "192.168.1.12 dev eth0 lladdr 04:e9:e5:44:55:66 STALE", + "192.168.1.50 dev eth0 lladdr aa:bb:cc:dd:ee:ff REACHABLE", + "192.168.4.11 dev eth0 lladdr 04:e9:e5:77:88:99 REACHABLE", + ) + ) + + def fake_run(command: list[str], **kwargs: object) -> _Result: + del kwargs + if command[:3] == ["ip", "neigh", "show"]: + return _Result(stdout=neighbors) + if command[0] == "ping": + return _Result(returncode=0 if command[-1] == "192.168.1.12" else 1) + raise AssertionError(command) + + monkeypatch.setattr(trossen_eth.subprocess, "run", fake_run) + + assert trossen_eth.ping_reachable_controller_ips(["192.168.1"]) == ["192.168.1.12"] + + +def _fake_net_iface( + root: pathlib.Path, name: str, *, dev_type: str, operstate: str, wireless: bool = False +) -> None: + iface = root / name + iface.mkdir(parents=True) + (iface / "type").write_text(f"{dev_type}\n") + (iface / "operstate").write_text(f"{operstate}\n") + if wireless: + (iface / "wireless").mkdir() + + +def test_wired_up_interfaces_excludes_wireless_loopback_and_down(tmp_path: pathlib.Path) -> None: + _fake_net_iface(tmp_path, "eth0", dev_type="1", operstate="up") + _fake_net_iface(tmp_path, "eth1", dev_type="1", operstate="down") + _fake_net_iface(tmp_path, "wlan0", dev_type="1", operstate="up", wireless=True) + _fake_net_iface(tmp_path, "lo", dev_type="772", operstate="unknown") + _fake_net_iface(tmp_path, "can0", dev_type="280", operstate="up") + + assert trossen_eth.wired_up_interfaces(tmp_path) == ["eth0"] + + +def test_free_host_ip_skips_arm_candidate_octets_and_addresses_in_use( + monkeypatch: pytest.MonkeyPatch, +) -> None: + in_use = {"192.168.1.200", "192.168.1.201"} + monkeypatch.setattr(trossen_eth, "ip_in_use", lambda ip, interface=None: ip in in_use) + + assert trossen_eth.free_host_ip("192.168.1", "eth0") == "192.168.1.202" + # The arm provisioning candidates (.211/.212/.221/.222) are never offered + # as host addresses even when free. + assert trossen_eth.HOST_OCTET_START == 200 + in_use.update(f"192.168.1.{octet}" for octet in range(200, 215)) + assert trossen_eth.free_host_ip("192.168.1", "eth0") == "192.168.1.215" + monkeypatch.setattr(trossen_eth, "ip_in_use", lambda ip, interface=None: True) + assert trossen_eth.free_host_ip("192.168.1", "eth0") is None + + +def test_temporary_address_add_and_remove_run_ip_addr(monkeypatch: pytest.MonkeyPatch) -> None: + commands: list[list[str]] = [] + + class _Result: + stdout = "" + stderr = "" + returncode = 0 + + monkeypatch.setattr( + trossen_eth.subprocess, + "run", + lambda command, **kwargs: commands.append(command) or _Result(), + ) + + trossen_eth.add_temporary_address("eth0", "192.168.1.200/24") + trossen_eth.remove_temporary_address("eth0", "192.168.1.200/24") + + assert commands == [ + ["sudo", "ip", "addr", "add", "192.168.1.200/24", "dev", "eth0"], + ["sudo", "ip", "addr", "del", "192.168.1.200/24", "dev", "eth0"], + ] + + +def test_add_temporary_address_raises_on_failure(monkeypatch: pytest.MonkeyPatch) -> None: + class _Result: + stdout = "" + stderr = "RTNETLINK answers: Operation not permitted" + returncode = 2 + + monkeypatch.setattr(trossen_eth.subprocess, "run", lambda command, **kwargs: _Result()) + + with pytest.raises(RuntimeError, match="Operation not permitted"): + trossen_eth.add_temporary_address("eth0", "192.168.1.200/24") + + +def test_add_persistent_address_runs_nmcli_modify_and_up(monkeypatch: pytest.MonkeyPatch) -> None: + commands: list[list[str]] = [] + + class _Result: + def __init__(self, stdout: str = "", returncode: int = 0) -> None: + self.stdout = stdout + self.stderr = "" + self.returncode = returncode + + def fake_run(command: list[str], **kwargs: object) -> _Result: + commands.append(command) + if command[:4] == ["nmcli", "-g", "GENERAL.CONNECTION", "device"]: + return _Result(stdout="Wired connection 1\n") + return _Result() + + monkeypatch.setattr(trossen_eth.shutil, "which", lambda name: "/usr/bin/nmcli") + monkeypatch.setattr(trossen_eth.subprocess, "run", fake_run) + + trossen_eth.add_persistent_address("eth0", "192.168.1.200/24") + + assert commands[1] == [ + "sudo", "nmcli", "connection", "modify", "Wired connection 1", + "+ipv4.addresses", "192.168.1.200/24", + ] + assert commands[2] == ["sudo", "nmcli", "connection", "up", "Wired connection 1"] + + +def test_add_persistent_address_requires_active_nm_connection( + monkeypatch: pytest.MonkeyPatch, +) -> None: + class _Result: + stdout = "--\n" + stderr = "" + returncode = 0 + + monkeypatch.setattr(trossen_eth.shutil, "which", lambda name: "/usr/bin/nmcli") + monkeypatch.setattr(trossen_eth.subprocess, "run", lambda command, **kwargs: _Result()) + + with pytest.raises(RuntimeError, match="no active NetworkManager connection"): + trossen_eth.add_persistent_address("eth0", "192.168.1.200/24") From a4821981a890df01300c9405ca3514f5f3236d60 Mon Sep 17 00:00:00 2001 From: Adnan Esmail Date: Fri, 24 Jul 2026 13:45:23 -0700 Subject: [PATCH 4/5] native+models: Trossen WidowX AI iNerve Ethernet controller support New TROSSEN_ETHERNET driver stack for whole-arm-controller hardware: - pi_trossen_shim: isolation .so around the prebuilt libtrossen_arm.a. The vendor archive statically bundles its own pinocchio/urdfdom/ tinyxml2; linking it into the node directly interposes those symbols with the node's shared pinocchio and SIGSEGVs in ModelTpl::addJoint during configure. The archive links into a dedicated shared library with every archive symbol hidden (--exclude-libs,ALL), so only the shim API is exported. - DriverController: vendor-neutral whole-arm-controller Driver base with a stall watchdog -- vendor stacks keep streaming the last command from a driver-internal daemon even when the host loop hangs, so a watchdog thread idles the arm after 1 s of driver silence. - DriverTrossen: the iNerve Ethernet driver (leader/follower mass profiles, clear_error recovery, configure timeout); ServoController / DeviceEffectorController map controller joints onto the existing device model ('controller' arm/effector types). - Trossen_wai_ctrl + E_Trossen_ctrl model assets. The gripper protective-stop limit is +-120 (gripper effort is Newtons, not Nm; firm ~75 N grasps tripped a +-30 threshold). - Python: EthernetConnection reachability validation and the controller IP as the node's --control_port; get_field_value_optional() silences missing-optional-field warnings shared by the new stacks. Co-Authored-By: Claude Fable 5 --- native/pi_control/CMakeLists.txt | 36 ++ native/pi_control/include/pi_control.hpp | 7 + .../pi_control/include/pi_device_config.hpp | 30 ++ .../include/pi_device_effector_controller.hpp | 41 ++ .../include/pi_driver_controller.hpp | 275 +++++++++++++ .../pi_control/include/pi_driver_trossen.hpp | 66 +++ native/pi_control/include/pi_servo.hpp | 4 +- .../include/pi_servo_controller.hpp | 160 ++++++++ native/pi_control/include/pi_trossen_shim.hpp | 119 ++++++ native/pi_control/src/pi_device.cpp | 12 + .../src/pi_device_effector_controller.cpp | 18 + native/pi_control/src/pi_driver.cpp | 15 +- .../pi_control/src/pi_driver_controller.cpp | 385 ++++++++++++++++++ native/pi_control/src/pi_driver_trossen.cpp | 211 ++++++++++ native/pi_control/src/pi_servo.cpp | 5 + native/pi_control/src/pi_servo_controller.cpp | 201 +++++++++ native/pi_control/src/pi_trossen_shim.cpp | 283 +++++++++++++ src/openpi_control/config.py | 10 +- .../Trossen_wai_ctrl/Trossen_wai_ctrl.json | 268 ++++++++++++ .../Trossen_wai_ctrl/Trossen_wai_ctrl.urdf | 230 +++++++++++ .../Trossen_wai_ctrl/Trossen_wai_ctrl_01.json | 87 ++++ .../E_Trossen_ctrl/E_Trossen_ctrl.json | 66 +++ .../E_Trossen_ctrl/E_Trossen_ctrl_01.json | 28 ++ .../E_Trossen_ctrl/E_Trossen_ctrl_mass.json | 16 + src/openpi_control/native.py | 25 +- 25 files changed, 2592 insertions(+), 6 deletions(-) create mode 100644 native/pi_control/include/pi_device_effector_controller.hpp create mode 100644 native/pi_control/include/pi_driver_controller.hpp create mode 100644 native/pi_control/include/pi_driver_trossen.hpp create mode 100644 native/pi_control/include/pi_servo_controller.hpp create mode 100644 native/pi_control/include/pi_trossen_shim.hpp create mode 100644 native/pi_control/src/pi_device_effector_controller.cpp create mode 100644 native/pi_control/src/pi_driver_controller.cpp create mode 100644 native/pi_control/src/pi_driver_trossen.cpp create mode 100644 native/pi_control/src/pi_servo_controller.cpp create mode 100644 native/pi_control/src/pi_trossen_shim.cpp create mode 100644 src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json create mode 100644 src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.urdf create mode 100644 src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json create mode 100644 src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json create mode 100644 src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json create mode 100644 src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_mass.json diff --git a/native/pi_control/CMakeLists.txt b/native/pi_control/CMakeLists.txt index 2651270..3b5b61f 100644 --- a/native/pi_control/CMakeLists.txt +++ b/native/pi_control/CMakeLists.txt @@ -37,6 +37,12 @@ find_package(Eigen3 REQUIRED) pkg_check_modules(PC_ZMQ REQUIRED libzmq) pkg_check_modules(PINOCCHIO REQUIRED pinocchio) +# Trossen iNerve controller SDK (DriverTrossen). Provisioned by +# scripts/build_deps.sh from the pinned TrossenRobotics/trossen_arm release +# (prebuilt static lib + headers); provides the imported STATIC target +# `libtrossen_arm`. +find_package(libtrossen_arm REQUIRED) + # Let CMake's runtime dependency scanner resolve local build dependencies # without hard-coding those directories into the installed executable. set(CMAKE_BUILD_RPATH ${PINOCCHIO_LIBRARY_DIRS} ${PC_ZMQ_LIBRARY_DIRS}) @@ -72,6 +78,10 @@ set(COMMON_SOURCES src/pi_driver.cpp src/pi_driver_arx.cpp src/pi_driver_arx_encoder.cpp + src/pi_driver_controller.cpp + src/pi_driver_trossen.cpp + src/pi_servo_controller.cpp + src/pi_device_effector_controller.cpp src/pi_driver_can.cpp src/pi_joint.cpp src/pi_servo.cpp @@ -95,12 +105,31 @@ set(COMMON_SOURCES ) +# --------------------------------------------------------------------------- +# libpi_trossen_shim: isolation boundary around the prebuilt libtrossen_arm.a. +# The vendor archive statically bundles its own pinocchio/urdfdom/tinyxml2; +# linking it into the node directly makes those symbols interpose with the +# node's shared pinocchio and mixes two pinocchio builds in one process +# (SIGSEGV in ModelTpl::addJoint during TrossenArmDriver::configure). The +# archive is therefore linked into this dedicated shared library and its +# symbols are hidden from the dynamic symbol table, so only the +# TrossenArmShim API (compiled here, not from the archive) is exported. +# --------------------------------------------------------------------------- +add_library(pi_trossen_shim SHARED src/pi_trossen_shim.cpp) +target_include_directories(pi_trossen_shim PRIVATE include) +target_link_libraries(pi_trossen_shim PRIVATE libtrossen_arm pthread) +# Mark every symbol pulled from static archives (i.e. libtrossen_arm.a and +# its bundled third-party copies) as LOCAL in the .so's dynamic symbol +# table. Internal references bind at link time; nothing leaks out. +target_link_options(pi_trossen_shim PRIVATE "LINKER:--exclude-libs,ALL") + add_executable(${OPENPI_CONTROL_TARGET} ${COMMON_SOURCES}) target_link_libraries(${OPENPI_CONTROL_TARGET} ${PC_ZMQ_LDFLAGS} ${PINOCCHIO_LDFLAGS} Boost::program_options + pi_trossen_shim ) # Use a transitive DT_RPATH instead of DT_RUNPATH. Libraries copied beside the @@ -129,6 +158,7 @@ if(OPENPI_CONTROL_BUILD_TESTING) ${PC_ZMQ_LDFLAGS} ${PINOCCHIO_LDFLAGS} Boost::program_options + pi_trossen_shim ) gtest_discover_tests( pi_topic_zmq_tests @@ -140,6 +170,11 @@ install(TARGETS ${OPENPI_CONTROL_TARGET} RUNTIME_DEPENDENCY_SET openpi_control_runtime_dependencies RUNTIME DESTINATION openpi_control/bin) +# The shim is a build product (not scanned as an external runtime dependency), +# so bundle it explicitly next to the other shared libraries; the node's +# $ORIGIN/../libs RPATH resolves it after wheel installation. +install(TARGETS pi_trossen_shim LIBRARY DESTINATION openpi_control/libs) + # Ship the node's complete non-system shared-library closure. The executable's # $ORIGIN/../libs RPATH above resolves these files after wheel installation; # a binary consumer therefore does not need Pinocchio, ZeroMQ, or Boost from @@ -184,6 +219,7 @@ if(OPENPI_CONTROL_BUILD_FUZZERS) ${PC_ZMQ_LDFLAGS} ${PINOCCHIO_LDFLAGS} Boost::program_options + pi_trossen_shim ) endforeach() endif() diff --git a/native/pi_control/include/pi_control.hpp b/native/pi_control/include/pi_control.hpp index 3ba174d..59edfff 100644 --- a/native/pi_control/include/pi_control.hpp +++ b/native/pi_control/include/pi_control.hpp @@ -122,6 +122,13 @@ #define ARX_ENCODER_WARMUP_MAX_RETRY 50 ///< Max cache-freshness polls during encoder enable. #define ARX_ENCODER_WARMUP_SLEEP_US 1000 ///< Microseconds between encoder warmup polls (1 ms). +// Whole-arm controller (DriverController) stall watchdog. Vendor controller +// stacks (Trossen iNerve etc.) keep streaming the last command from a driver- +// internal daemon thread even when the host control loop hangs, so a separate +// watchdog thread idles the arm when the loop stops calling group read/write. +#define CONTROLLER_STALL_WATCHDOG_TIMEOUT_MS 1000 ///< Driver-interaction silence (ms) before the arm is idled. +#define CONTROLLER_STALL_WATCHDOG_PERIOD_MS 100 ///< Watchdog polling period (ms). + /*! * @enum Role * @brief Device roles in teleoperation. diff --git a/native/pi_control/include/pi_device_config.hpp b/native/pi_control/include/pi_device_config.hpp index 0ad8f56..b11b70a 100644 --- a/native/pi_control/include/pi_device_config.hpp +++ b/native/pi_control/include/pi_device_config.hpp @@ -43,9 +43,11 @@ class DeviceConfig { const std::string fn_arm_type = "arm_type"; ///< Field name for arm type. const std::string val_arm_type_arx = "arx"; ///< Value for arm type arx. + const std::string val_arm_type_controller = "controller"; ///< Value for arms managed by a whole-arm controller (DriverController). const std::string fn_effector_type = "effector_type"; ///< Field name for effector type. const std::string val_effector_type_arx = "arx"; ///< Value for effector type arx. + const std::string val_effector_type_controller = "controller"; ///< Value for effectors managed by a whole-arm controller (DriverController). const std::string val_effector_type_none = "None"; ///< Value for effector type none. const std::string fn_effector_control_mode = "control_mode"; ///< Field name for effector control mode. const std::string val_effector_control_mode_torque = "torque"; ///< Value for effector control mode torque. @@ -61,6 +63,8 @@ class DeviceConfig { const std::string fn_driver_type = "driver_type"; ///< Field name for driver type. const std::string val_driver_type_can = "CAN"; ///< Value for driver type CAN. const std::string val_driver_type_can_encoder = "CAN_ENCODER"; ///< Value for driver type CAN read-only encoder (DriverArxEncoder). + const std::string val_driver_type_trossen = "TROSSEN_ETHERNET"; ///< Value for driver type Trossen iNerve controller over Ethernet (DriverTrossen). + const std::string fn_controller_model = "controller_model"; ///< Field name for the vendor controller model string (e.g. "wxai_v0"). const std::string fn_algo_type = "algo_type"; ///< Field name for algorithm type. const std::string val_algo_type_algo = "Algo"; ///< Value for algorithm type Algo. @@ -111,6 +115,7 @@ class DeviceConfig { const std::string val_servo_model_encos_A4310 = "Encos EC-A4310-P2-36"; ///< Encos EC-A4310-P2-36 (CAN). const std::string val_servo_model_can_passive_encoder = "CAN Passive Encoder"; ///< YAM teaching-handle trigger encoder (CAN request/response poll, read-only). const std::string val_servo_model_arx_encoder = "ARX Remote Encoder"; ///< ARX read-only joint encoder (CAN, 2-byte angle, no actuation). + const std::string val_servo_model_trossen_wxai = "Trossen WXAI Joint"; ///< One joint of a Trossen WidowX AI arm managed by the iNerve controller (Ethernet). const std::string fn_servo_id = "servo_id"; ///< Field name for servo ID. const std::string fn_servo_data_index = "data_index"; ///< Field name for servo data index. const std::string fn_servo_pos_min = "pos_min"; ///< Field name for servo position minimum (relative radian). @@ -203,6 +208,31 @@ class DeviceConfig { return ReturnCode::SUCCESS; } + /*! + * @brief Extracts a typed value for an optional field without warning when it is absent. + * + * Identical to get_field_value() except a missing field returns INVALID_PARAM silently. + * Use for fields that have a documented default so startup logs are not flooded with + * "is not defined" warnings for perfectly valid configs. + * @param json_data JSON object containing the configuration data. + * @param field_name Name of the field to extract (should use one of the fn_* constants). + * @param value Output parameter that will be populated with the extracted value. + */ + template + ReturnCode get_field_value_optional(const json& json_data, const std::string& field_name, T& value) const { + if (!json_data.contains(field_name)) { + return ReturnCode::INVALID_PARAM; + } + try { + value = json_data[field_name].get(); + } catch (const nlohmann::json::type_error& e) { + std::string what = e.what(); + PI_ERROR("Type Error: %s", what.c_str()); + return ReturnCode::INVALID_PARAM; + } + return ReturnCode::SUCCESS; + } + private: /*! * @brief Loads and parses a configuration file from the given file path. diff --git a/native/pi_control/include/pi_device_effector_controller.hpp b/native/pi_control/include/pi_device_effector_controller.hpp new file mode 100644 index 0000000..42ceef9 --- /dev/null +++ b/native/pi_control/include/pi_device_effector_controller.hpp @@ -0,0 +1,41 @@ +/*! + * @file pi_device_effector_controller.hpp + * @brief DeviceEffectorController class for grippers managed by a whole-arm controller. + */ +#pragma once + +#include "pi_device_effector_arx.hpp" + +/*! + * @class DeviceEffectorController + * @brief Effector device for grippers driven by a whole-arm controller (DriverController). + * + * Inherits the ARX torque-gripper motion logic (distance-to-torque via + * apply_torque_with_damping(), which ServoController maps to controller + * external efforts). Unlike ARX, controller-managed grippers need real + * leader/follower mode switching (position vs external effort on the vendor + * controller), so set_control_mode() restores the DeviceEffector base + * behavior of delegating to Joint::change_control_mode_for_{leader,follower}(). + */ +class DeviceEffectorController : public DeviceEffectorArx { + public: + /*! + * @brief Constructor. + * @param cla Command-line arguments. + */ + explicit DeviceEffectorController(const CommandLineArgs& cla); + + /*! + * @brief Destructor. + */ + ~DeviceEffectorController() override; + + /*! + * @brief Delegates mode switching to the joints (DeviceEffector base + * behavior), undoing the ARX no-op override. + * @param target_role Target role (LEADER or FOLLOWER). + * @param intent Control mode intent. + * @return ReturnCode indicating success or failure. + */ + ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; +}; diff --git a/native/pi_control/include/pi_driver_controller.hpp b/native/pi_control/include/pi_driver_controller.hpp new file mode 100644 index 0000000..d483897 --- /dev/null +++ b/native/pi_control/include/pi_driver_controller.hpp @@ -0,0 +1,275 @@ +/*! + * @file pi_driver_controller.hpp + * @brief DriverController -- vendor-neutral base class for whole-arm robot controllers. + * + * Covers arms that are NOT driven servo-by-servo over CAN/serial but through a + * vendor controller box exposing a whole-arm API over Ethernet (Trossen iNerve, + * and structurally similar stacks like Franka FCI or UR RTDE). The class keeps + * the existing Driver group read/write contract so DeviceArm / DeviceEffector / + * Joint / Servo code runs unchanged: + * + * - group_read_hardware_values() -> one whole-arm state fetch into a cache; + * read_hardware_values(Servo*) copies the per-joint slice into ServoController. + * - ServoController::move()/apply_torque() queue per-joint commands here; + * group_write_hardware_values() flushes them as one whole-arm transaction. + * - Per-joint control modes (position / external effort) are buffered and + * applied lazily right before the next command flush, so a leader/follower + * switch costs one vendor configuration call instead of one per joint. + * + * Lifecycle safety (vendor-neutral): + * - open() is a template method: vendor_connect() -> joint count validation -> + * initial state read, so a half-configured controller can never enter the + * control loop. + * - make_safe() idles the whole arm exactly once (idempotent, thread-safe); + * close() and the stall watchdog both funnel into it. + * - arm_comm_loss_protection() starts a stall watchdog thread: if the host + * control loop stops calling group read/write while the vendor daemon keeps + * streaming the last command (host hang), the watchdog idles the arm. + */ + +#pragma once +#include +#include +#include +#include +#include +#include + +#include "pi_driver.hpp" + +class ServoController; + +/*! + * @brief Vendor-neutral base class for whole-arm controller drivers. + */ +class DriverController : public Driver { + public: + /*! + * @brief Per-joint control mode requested from the vendor controller. + */ + enum class JointCommandMode { + UNSET, ///< No mode selected yet (joint not started). + POSITION, ///< Controller-side position servoing (follower / ready moves). + EXTERNAL_EFFORT, ///< Torque on top of the controller's own gravity/friction compensation (leader). + }; + + /*! + * @brief Per-joint command buffered until the next group write flush. + */ + struct JointCommand { + JointCommandMode mode = JointCommandMode::UNSET; ///< Mode the command was queued for. + float position = 0; ///< Goal position (rad; gripper joints use meters). + float velocity_ff = 0; ///< Feedforward velocity (rad/s), 0 when unused. + float external_effort = 0; ///< External effort (Nm; gripper joints use N). + bool pending = false; ///< True when queued and not yet flushed. + }; + + /*! + * @brief Per-joint state cache filled by group_read_hardware_values(). + */ + struct JointState { + float position = 0; ///< Position (rad; gripper joints use meters). + float velocity = 0; ///< Velocity (rad/s). + float effort = 0; ///< Effort (Nm). + float temperature = 0; ///< Motor temperature (degrees Celsius). + bool valid = false; ///< True once at least one whole-arm read succeeded. + }; + + /*! + * @brief Constructor. + * @param p_device Pointer to the Device instance. + * @param cla Command-line arguments (control_port_name carries the controller IP address). + */ + DriverController(Device* p_device, const CommandLineArgs& cla); + + /*! + * @brief Destructor. Funnels into close() so the arm is idled even when the + * owning Device skips its stop path (idempotent). + */ + ~DriverController() override; + + /*! + * @brief Connects to the controller (template method around vendor_connect()). + * + * Fast-fails when the vendor reports fewer joints than the model config + * registered, and performs one initial whole-arm read so the position + * caches are valid before any servo is started. + * @param baud_rate Unused for Ethernet controllers (Driver interface compatibility). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode open(int baud_rate) override; + + /*! + * @brief Idles the arm and disconnects. Idempotent: safe to call repeatedly + * and from the destructor. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode close() override; + + /*! + * @brief Copies the cached whole-arm state slice of one joint into its ServoController. + * @param p_servo Pointer to the Servo instance (must be a ServoController). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode read_hardware_values(Servo* p_servo) override; + + /*! + * @brief Fetches the whole-arm state from the controller into the joint state cache. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode group_read_hardware_values() override; + + /*! + * @brief Applies pending mode changes, then flushes the queued per-joint + * commands as whole-arm vendor transactions. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode group_write_hardware_values() override; + + /*! + * @brief Starts the stall watchdog. Called once by the node right before + * the main control loop starts. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode arm_comm_loss_protection() override; + + /*! + * @brief Requests a control mode for one joint. Buffered; the vendor + * configuration call happens once in the next group write flush. + * @param data_index Joint slot (Servo::data_index_). + * @param mode Requested mode. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_joint_command_mode(int data_index, JointCommandMode mode); + + /*! + * @brief Queues a position command for one joint. + * @param data_index Joint slot (Servo::data_index_). + * @param position Goal position (rad; gripper joints use meters). + * @param velocity_ff Feedforward velocity (rad/s), 0 when unused. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode queue_position(int data_index, float position, float velocity_ff); + + /*! + * @brief Queues an external effort command for one joint. + * @param data_index Joint slot (Servo::data_index_). + * @param external_effort External effort (Nm; gripper joints use N). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode queue_external_effort(int data_index, float external_effort); + + /*! + * @brief Puts the whole arm in the vendor's safe passive state exactly once. + * + * Thread-safe and idempotent: park_safely() of every ServoController, close(), + * the stall watchdog, and the destructor all funnel into this method; only the + * first caller performs the vendor call. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode make_safe(); + + /*! + * @brief Number of joints the vendor controller reports (arm + gripper). + */ + int controller_joint_count() const { return joint_count_; } + + /*! + * @brief True once open() completed and close() has not run. + */ + bool is_open() const { return opened_; } + + /*! + * @brief True once at least one whole-arm state read succeeded. + */ + bool has_valid_state() const { return read_success_count_ > 0; } + + protected: + // ---- Vendor hooks (implemented by DriverTrossen and future controller drivers) ---- + + /*! + * @brief Establishes the connection to the controller and returns its joint count. + * @param address Controller network address (from --control_port_name). + * @param joint_count Output: number of joints the controller manages. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_connect(const std::string& address, int& joint_count) = 0; + + /*! + * @brief Tears the connection down. Must tolerate being called when the + * connection is already gone. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_disconnect() = 0; + + /*! + * @brief Reads the whole-arm state. + * @param states Output vector sized to controller_joint_count(). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_read_state(std::vector& states) = 0; + + /*! + * @brief Sends the pending commands (only entries with pending == true). + * @param commands Command buffer sized to controller_joint_count(). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_write_commands(const std::vector& commands) = 0; + + /*! + * @brief Applies the requested per-joint control modes on the controller. + * @param modes Mode vector sized to controller_joint_count(); UNSET entries + * must be mapped to the vendor's passive/idle mode. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_apply_command_modes(const std::vector& modes) = 0; + + /*! + * @brief Puts every joint in the vendor's safe passive state (e.g. idle with + * controller-side gravity compensation). Must not throw. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_make_safe() = 0; + + private: + /*! + * @brief Validates a joint slot index against the vendor joint count. + */ + ReturnCode validate_data_index(int data_index) const; + + /*! + * @brief Stall watchdog loop: idles the arm when the host control loop + * stops interacting with the driver while commands may still stream. + */ + void watchdog_loop(); + + /*! + * @brief Records a successful driver interaction for the stall watchdog. + */ + void feed_watchdog(); + + /*! + * @brief Stops the watchdog thread (idempotent). + */ + void stop_watchdog(); + + int joint_count_ = 0; ///< Joint count reported by the vendor controller. + bool opened_ = false; ///< True between successful open() and close(). + std::atomic read_success_count_{0}; ///< Successful whole-arm reads since open(). + + std::mutex command_mutex_; ///< Guards commands_, desired_modes_, modes_dirty_. + std::vector commands_; ///< Per-joint command buffer (indexed by data_index). + std::vector desired_modes_; ///< Per-joint requested modes. + bool modes_dirty_ = false; ///< True when desired_modes_ changed since the last flush. + + std::mutex state_mutex_; ///< Guards states_. + std::vector states_; ///< Per-joint state cache (indexed by data_index). + + std::mutex safe_mutex_; ///< Serializes make_safe() / close(). + bool made_safe_ = false; ///< True once vendor_make_safe() ran. + + std::thread watchdog_thread_; ///< Stall watchdog thread. + std::atomic watchdog_running_{false}; ///< Watchdog thread lifecycle flag. + std::atomic watchdog_tripped_{false}; ///< Latched when the watchdog idled the arm. + std::atomic last_interaction_ms_{0}; ///< Monotonic ms of the last successful read/write. +}; diff --git a/native/pi_control/include/pi_driver_trossen.hpp b/native/pi_control/include/pi_driver_trossen.hpp new file mode 100644 index 0000000..8386482 --- /dev/null +++ b/native/pi_control/include/pi_driver_trossen.hpp @@ -0,0 +1,66 @@ +/*! + * @file pi_driver_trossen.hpp + * @brief DriverTrossen -- Trossen iNerve arm controller driver (via TrossenArmShim over Ethernet). + * + * Implements the DriverController vendor hooks against TrossenArmShim, the + * isolation boundary around the prebuilt libtrossen_arm SDK (see + * pi_trossen_shim.hpp for why the SDK cannot be linked into the node + * directly): + * - vendor_connect() -> shim configure() (TCP handshake + UDP stream) + * - vendor_read_state() -> cached whole-arm output from the SDK daemon thread + * - vendor_write_commands() -> set_arm_positions / set_arm_external_efforts / + * set_gripper_* streaming calls (goal_time 0, non-blocking) + * - vendor_apply_command_modes() -> set_joint_modes() (one TCP configuration call) + * - vendor_make_safe() -> set_all_modes_idle(); the controller then holds the + * arm passively with its own gravity/friction compensation + * + * All positions/velocities/efforts are exchanged in the controller's native + * joint space (radians and Nm; the gripper joint uses meters and Newtons). + * Zeroing is owned by the controller (position_offset joint characteristic, + * managed from the Python setup backend), so model configs use zero_pos = 0. + * + * The SDK reports failures as C++ exceptions (including UDP stream loss + * detected by its daemon thread, rethrown on the next SDK call); the shim + * catches them and every hook converts to fast-failing ReturnCodes. + */ + +#pragma once +#include +#include +#include + +#include "pi_driver_controller.hpp" +#include "pi_trossen_shim.hpp" + +/*! + * @brief Trossen iNerve whole-arm controller driver. + */ +class DriverTrossen : public DriverController { + public: + /*! + * @brief Constructor. + * @param p_device Pointer to the Device instance. + * @param cla Command-line arguments (control_port_name carries the controller + * IP address; role selects the leader/follower end-effector properties). + * @param controller_model Trossen model string from the model config (e.g. "wxai_v0"). + */ + DriverTrossen(Device* p_device, const CommandLineArgs& cla, const std::string& controller_model); + + /*! + * @brief Destructor. Idles the arm and disconnects (idempotent). + */ + ~DriverTrossen() override; + + protected: + ReturnCode vendor_connect(const std::string& address, int& joint_count) override; + ReturnCode vendor_disconnect() override; + ReturnCode vendor_read_state(std::vector& states) override; + ReturnCode vendor_write_commands(const std::vector& commands) override; + ReturnCode vendor_apply_command_modes(const std::vector& modes) override; + ReturnCode vendor_make_safe() override; + + private: + std::string controller_model_; ///< Trossen model string from the model config. + bool is_leader_role_ = false; ///< True when this device runs as a leader. + std::unique_ptr p_arm_; ///< Shim around the vendor SDK (see pi_trossen_shim.hpp). +}; diff --git a/native/pi_control/include/pi_servo.hpp b/native/pi_control/include/pi_servo.hpp index 7a6d8ec..0e2febd 100644 --- a/native/pi_control/include/pi_servo.hpp +++ b/native/pi_control/include/pi_servo.hpp @@ -26,7 +26,9 @@ enum class ServoType { DM_4340 = 151, ///< Dinamo DM J4340 servo (CAN protocol). DM_4310 = 152, ///< Dinamo DM J4310 servo (CAN protocol). - CAN_PASSIVE_ENCODER = 701 ///< Passive CAN trigger encoder (YAM teaching handle): request/response poll, read-only, never commanded. + CAN_PASSIVE_ENCODER = 701, ///< Passive CAN trigger encoder (YAM teaching handle): request/response poll, read-only, never commanded. + + CONTROLLER_JOINT = 801 ///< One joint of a whole-arm controller (Trossen iNerve etc.; no direct bus access). }; /*! diff --git a/native/pi_control/include/pi_servo_controller.hpp b/native/pi_control/include/pi_servo_controller.hpp new file mode 100644 index 0000000..01fa849 --- /dev/null +++ b/native/pi_control/include/pi_servo_controller.hpp @@ -0,0 +1,160 @@ +/*! + * @file pi_servo_controller.hpp + * @brief ServoController -- per-joint view onto a whole-arm controller (DriverController). + * + * Joints of controller-managed arms (Trossen iNerve etc.) are not individual + * bus servos; the vendor controller owns the servo loops. This class adapts + * one controller joint to the Servo interface so Joint / DeviceArm / + * DeviceEffector logic (leash, safe mode, ready moves, torque grippers) runs + * unchanged: + * + * - move() queues a position target into the DriverController command buffer + * (flushed once per cycle as a whole-arm transaction). + * - apply_torque() queues an external effort on top of the controller's own + * gravity/friction compensation (leader spring / force feedback / torque + * gripper path). + * - change_control_mode_for_{leader,follower}() request per-joint mode + * switches, applied lazily as one vendor configuration call. + * + * Positions are exchanged in the controller's native joint space (radians; + * gripper joints use meters), so model configs use zero_pos = 0 and the + * controller-side position offset is the single zeroing authority. + */ + +#pragma once +#include "pi_driver_controller.hpp" +#include "pi_servo.hpp" + +/*! + * @brief Parameter container class for controller-managed joint configuration. + */ +class ServoControllerParam : public ServoParam { + public: + /*! + * @brief Constructor. + * @param tolerable_pos_difference_rad Tolerable position difference threshold in radians. + * @param max_pos_difference_rad Maximum position difference threshold in radians. + * @param velocity_threshold_rad_sec Velocity threshold in rad/sec. + */ + ServoControllerParam(float tolerable_pos_difference_rad, float max_pos_difference_rad, + float velocity_threshold_rad_sec) + : ServoParam(tolerable_pos_difference_rad, max_pos_difference_rad, velocity_threshold_rad_sec) {} +}; + +/*! + * @brief Manages one joint of a whole-arm controller through the Servo interface. + */ +class ServoController : public Servo { + public: + /*! + * @brief Constructor. + * @param p_device Pointer to the Device object. + * @param p_joint Pointer to the Joint object. + * @param p_driver Pointer to the Driver object (must be a DriverController). + */ + ServoController(Device* p_device, Joint* p_joint, Driver* p_driver); + + /*! + * @brief Destructor. + */ + ~ServoController(); + + /*! + * @brief Initializes the servo with model configuration. + * @param servo_config JSON object containing the servo model configuration. + * @param p_config Pointer to the device model configuration object. + * @return ReturnCode indicating success or failure. + */ + ReturnCode init_config_model(const json& servo_config, const DeviceConfig* p_config) override; + + /*! + * @brief Starts the joint: requests POSITION mode so the controller holds + * the current pose until ready moves take over. + * @return ReturnCode indicating success or failure. + */ + ReturnCode start_hardware() override; + + /*! + * @brief Parks the joint: funnels into DriverController::make_safe(), which + * idles the whole arm exactly once (idempotent across joints). + * @return ReturnCode indicating success or failure. + */ + ReturnCode park_safely() override; + + /*! + * @brief Fails fast when no whole-arm state read has succeeded yet. + * @return ReturnCode indicating success or failure. + */ + ReturnCode verify_position_fresh() override; + + /*! + * @brief Queues a position target for the next whole-arm command flush. + * @param target_pos Target position in relative radians (gripper: meters). + * @return ReturnCode indicating success or failure. + */ + ReturnCode move(float target_pos) override; + + /*! + * @brief Queues a position target with a feedforward velocity. + * + * The controller runs its own servo loops, so target_tor cannot bound the + * motion per-command and is not consumed (joint limits on the controller + * bound the effort). + * @param target_pos Target position in relative radians (gripper: meters). + * @param target_vel Target velocity in relative rad/sec (feedforward). + * @param target_tor Target torque in Nm (not consumed). + * @return ReturnCode indicating success or failure. + */ + ReturnCode move(float target_pos, float target_vel, float target_tor) override; + + /*! + * @brief Queues an external effort on top of the controller's own + * gravity/friction compensation. + * @param torque Torque in relative Nm (gripper: N). + * @return ReturnCode indicating success or failure. + */ + ReturnCode apply_torque(float torque) override; + + /*! + * @brief Same as apply_torque(); the controller applies its own damping. + * @param torque Torque in relative Nm (gripper: N). + * @return ReturnCode indicating success or failure. + */ + ReturnCode apply_torque_with_damping(float torque) override; + + /*! + * @brief Requests EXTERNAL_EFFORT mode for leader (teleoperation) operation. + * + * The controller keeps its own gravity/friction compensation active in this + * mode; the model's spring / force-feedback torques ride on top as external + * efforts, so the C++ Pinocchio gravity path must stay disabled + * (individual config gravity_compensation = false). + * @return ReturnCode indicating success or failure. + */ + ReturnCode change_control_mode_for_leader() override; + + /*! + * @brief Requests POSITION mode for follower operation. + * @return ReturnCode indicating success or failure. + */ + ReturnCode change_control_mode_for_follower() override; + + /*! + * @brief Gets the joint's current position in the controller's native unit. + * @return Current position (radians; gripper joints use meters). + */ + float get_pos_servo() override { return curr_pos_abs_; } + + /*! + * @brief Copies one whole-arm state slice into this servo's caches. + * Called by DriverController::read_hardware_values(). + * @param position Position (rad; gripper: meters). + * @param velocity Velocity (rad/s). + * @param effort Effort (Nm). + * @param temperature Motor temperature (degrees Celsius). + */ + void update_from_controller_state(float position, float velocity, float effort, float temperature); + + protected: + DriverController* p_driver_controller_ = nullptr; ///< Pointer to the whole-arm controller driver. +}; diff --git a/native/pi_control/include/pi_trossen_shim.hpp b/native/pi_control/include/pi_trossen_shim.hpp new file mode 100644 index 0000000..0e5f2ad --- /dev/null +++ b/native/pi_control/include/pi_trossen_shim.hpp @@ -0,0 +1,119 @@ +/*! + * @file pi_trossen_shim.hpp + * @brief TrossenArmShim -- isolation boundary around the prebuilt libtrossen_arm SDK. + * + * libtrossen_arm.a statically bundles its own copies of pinocchio, urdfdom and + * tinyxml2. Linking that archive directly into pi_control_node makes those + * symbols coexist (and interpose) with the shared pinocchio the node links for + * AlgoPino, which mixes two different pinocchio builds inside one process and + * crashes (SIGSEGV in pinocchio::ModelTpl::addJoint during + * TrossenArmDriver::configure). The shim compiles the vendor archive into a + * separate shared library (libpi_trossen_shim) whose archive symbols are + * hidden (Linux: -Wl,--exclude-libs,ALL), so the vendor's pinocchio copy is + * invisible to the rest of the process. + * + * This header is the ONLY interface the node sees: no vendor types, no vendor + * headers. Every vendor exception is caught inside the shim and surfaced as a + * false return + last_error() message so no vendor typeinfo crosses the + * library boundary. + */ + +#pragma once +#include +#include +#include + +/*! + * @brief Thin client for one Trossen iNerve controller connection. + * + * All methods return true on success. On failure they return false and + * last_error() carries the vendor error message (fast-fail: the caller is + * expected to convert to its own error handling immediately). + */ +class TrossenArmShim { + public: + /*! @brief Joint command mode understood by the controller. */ + enum class Mode { + IDLE = 0, ///< Passive hold (controller-side gravity/friction compensation). + POSITION = 1, ///< Streamed position control. + EXTERNAL_EFFORT = 2, ///< Streamed external-effort (torque) control. + }; + + TrossenArmShim(); + ~TrossenArmShim(); + + TrossenArmShim(const TrossenArmShim&) = delete; + TrossenArmShim& operator=(const TrossenArmShim&) = delete; + + /*! + * @brief Connect and configure the controller (TCP handshake + UDP stream). + * @param model Controller model string from the model config (e.g. "wxai_v0"). + * @param leader_end_effector True selects the vendor leader end-effector mass + * properties (teleop handle); false selects the follower set. + * @param address Controller IPv4 address. + * @param clear_error True clears a stale latched controller error before configuring. + * @param timeout_s TCP handshake timeout in seconds. + * @return True on success. + */ + bool configure(const std::string& model, bool leader_end_effector, const std::string& address, bool clear_error, + double timeout_s); + + /*! @brief Release the controller connection (idempotent). @return True on success. */ + bool cleanup(); + + /*! @brief Number of joints reported by the controller (arm + gripper). */ + int num_joints() const; + + /*! @brief Vendor driver (SDK) version string. */ + std::string driver_version() const; + + /*! @brief Controller firmware version string. */ + std::string controller_version() const; + + /*! + * @brief Read the cached whole-arm state (positions, velocities, efforts, rotor temperatures). + * @return True on success; all vectors are resized to num_joints(). + */ + bool read_state(std::vector& positions, std::vector& velocities, std::vector& efforts, + std::vector& temperatures); + + /*! + * @brief Stream position targets for all arm joints (gripper excluded). + */ + bool set_arm_positions(const std::vector& positions, double goal_time, bool blocking, + const std::vector& velocity_ffs); + + /*! + * @brief Stream external-effort targets for all arm joints (gripper excluded). + */ + bool set_arm_external_efforts(const std::vector& efforts, double goal_time, bool blocking); + + /*! @brief Stream a position target for one joint. */ + bool set_joint_position(int joint_index, double position, double goal_time, bool blocking, double velocity_ff); + + /*! @brief Stream an external-effort target for one joint. */ + bool set_joint_external_effort(int joint_index, double effort, double goal_time, bool blocking); + + /*! @brief Stream a position target for the gripper joint. */ + bool set_gripper_position(double position, double goal_time, bool blocking, double velocity_ff); + + /*! @brief Stream an external-effort target for the gripper joint. */ + bool set_gripper_external_effort(double effort, double goal_time, bool blocking); + + /*! + * @brief Apply per-joint command modes (one TCP configuration call). + * @param modes One mode per controller joint. + */ + bool set_joint_modes(const std::vector& modes); + + /*! @brief Idle every joint (controller-side passive hold). */ + bool set_all_modes_idle(); + + /*! @brief Message of the most recent failure (empty when the last call succeeded). */ + const std::string& last_error() const; + + private: + struct Impl; ///< Hides every vendor type behind the shim boundary. + std::unique_ptr impl_; ///< Vendor driver instance (pimpl). + std::string last_error_; ///< Last failure message (vendor exception text). +}; diff --git a/native/pi_control/src/pi_device.cpp b/native/pi_control/src/pi_device.cpp index 59a0d4a..c3278e2 100644 --- a/native/pi_control/src/pi_device.cpp +++ b/native/pi_control/src/pi_device.cpp @@ -11,6 +11,7 @@ #include "pi_device_arm_arx.hpp" #include "pi_device_config.hpp" #include "pi_device_effector_arx.hpp" +#include "pi_device_effector_controller.hpp" #include "pi_info.hpp" Device::Device(const CommandLineArgs& cla) @@ -577,6 +578,13 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf p_device = new DeviceArmArx(cla); PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArmArx for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); + } else if (arm_type == cfg_model.val_arm_type_controller) { + // Whole-arm controller arms use the DeviceArm base directly: + // its set_control_mode() delegates leader/follower switching + // to the joints, which is exactly what ServoController needs. + p_device = new DeviceArm(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArm (controller) for %s_%s", + cla.device_model.c_str(), cla.device_id.c_str()); } else { PI_ERROR("Invalid arm type: %s", arm_type.c_str()); return nullptr; @@ -593,6 +601,10 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf p_device = new DeviceEffectorArx(cla); PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorArx for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); + } else if (effector_type == cfg_model.val_effector_type_controller) { + p_device = new DeviceEffectorController(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorController for %s_%s", + cla.device_model.c_str(), cla.device_id.c_str()); } else { PI_ERROR("Invalid effector type: %s", effector_type.c_str()); return nullptr; diff --git a/native/pi_control/src/pi_device_effector_controller.cpp b/native/pi_control/src/pi_device_effector_controller.cpp new file mode 100644 index 0000000..da1a2dc --- /dev/null +++ b/native/pi_control/src/pi_device_effector_controller.cpp @@ -0,0 +1,18 @@ +/*! + * @file pi_device_effector_controller.cpp + * @brief Implementation of the DeviceEffectorController class. + */ + +#include "pi_device_effector_controller.hpp" + +DeviceEffectorController::DeviceEffectorController(const CommandLineArgs& cla) : DeviceEffectorArx(cla) {} + +DeviceEffectorController::~DeviceEffectorController() {} + +ReturnCode DeviceEffectorController::set_control_mode(Role target_role, ControlModeIntent intent) { + // Controller-managed grippers switch between position (follower / ready + // move) and external effort (leader / torque gripper) on the vendor + // controller; the DeviceEffector base implementation delegates exactly + // that to the joints. + return DeviceEffector::set_control_mode(target_role, intent); +} diff --git a/native/pi_control/src/pi_driver.cpp b/native/pi_control/src/pi_driver.cpp index 56679c0..ffe88fd 100644 --- a/native/pi_control/src/pi_driver.cpp +++ b/native/pi_control/src/pi_driver.cpp @@ -8,6 +8,7 @@ #include "pi_device_config.hpp" #include "pi_driver_arx.hpp" #include "pi_driver_arx_encoder.hpp" +#include "pi_driver_trossen.hpp" #include "pi_servo.hpp" Driver::Driver(Device* p_device, const CommandLineArgs& cla) { @@ -39,8 +40,20 @@ std::shared_ptr Driver::new_driver(Device* p_device, const DeviceConfig* p_driver = p_driver_encoder; PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created CAN read-only encoder driver (DriverArxEncoder)"); + } else if (driver_type == p_config->val_driver_type_trossen) { + std::string controller_model; + return_code = p_config->get_field_value(p_config->values_, p_config->fn_controller_model, controller_model); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("controller_model is not defined in the model config (required for TROSSEN_ETHERNET)"); + return nullptr; + } + p_driver = std::make_shared(p_device, cla, controller_model); + PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created Trossen controller driver (DriverTrossen, model=%s)", + controller_model.c_str()); + } else { - PI_ERROR("Unsupported driver type: '%s' (supported types: CAN, CAN_ENCODER)", driver_type.c_str()); + PI_ERROR("Unsupported driver type: '%s' (supported types: CAN, CAN_ENCODER, TROSSEN_ETHERNET)", + driver_type.c_str()); return nullptr; } diff --git a/native/pi_control/src/pi_driver_controller.cpp b/native/pi_control/src/pi_driver_controller.cpp new file mode 100644 index 0000000..e644d29 --- /dev/null +++ b/native/pi_control/src/pi_driver_controller.cpp @@ -0,0 +1,385 @@ +/*! + * @file pi_driver_controller.cpp + * @brief Implementation of the DriverController vendor-neutral whole-arm controller base class. + */ + +#include "pi_driver_controller.hpp" + +#include + +#include "pi_servo_controller.hpp" + +namespace { + +/*! + * @brief Monotonic milliseconds for the stall watchdog. + */ +int64_t steady_now_ms() { + return std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +} // namespace + +DriverController::DriverController(Device* p_device, const CommandLineArgs& cla) : Driver(p_device, cla) {} + +DriverController::~DriverController() { + // Vendor hooks are gone once the derived destructor ran, so the derived + // class must call close() in its own destructor. This base destructor only + // stops the watchdog thread as a last line of defense. + stop_watchdog(); +} + +ReturnCode DriverController::open(int baud_rate) { + (void)baud_rate; // Ethernet controllers have no baud rate. + + if (opened_) { + PI_INFO("DriverController", InfoLevel::HELPFUL_1, "open() called on an already-open controller connection"); + return ReturnCode::SUCCESS; + } + + if (control_port_name_.empty()) { + PI_ERROR("Controller address is empty (--control_port_name must carry the controller IP address)"); + return ReturnCode::INVALID_PARAM; + } + + int joint_count = 0; + ReturnCode return_code = vendor_connect(control_port_name_, joint_count); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to connect to the arm controller at '%s'", control_port_name_.c_str()); + return return_code; + } + if (joint_count <= 0) { + PI_ERROR("Arm controller at '%s' reported an invalid joint count: %d", control_port_name_.c_str(), + joint_count); + vendor_disconnect(); + return ReturnCode::HARDWARE_FAULT; + } + joint_count_ = joint_count; + + { + std::lock_guard lock(command_mutex_); + commands_.assign(joint_count_, JointCommand{}); + desired_modes_.assign(joint_count_, JointCommandMode::UNSET); + modes_dirty_ = false; + } + { + std::lock_guard lock(state_mutex_); + states_.assign(joint_count_, JointState{}); + } + read_success_count_ = 0; + made_safe_ = false; + watchdog_tripped_ = false; + + opened_ = true; + + // One initial whole-arm read so every ServoController position cache is + // valid before start_hardware() / hold-at-current seeding runs. + return_code = group_read_hardware_values(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Initial whole-arm state read failed for controller at '%s'", control_port_name_.c_str()); + close(); + return return_code; + } + + PI_INFO("DriverController", InfoLevel::ESSENTIAL_0, "Connected to arm controller at '%s' (%d joints)", + control_port_name_.c_str(), joint_count_); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::close() { + stop_watchdog(); + + if (!opened_) { + return ReturnCode::SUCCESS; + } + + // Idle the arm before dropping the connection so the controller never + // keeps executing the last streamed command. + make_safe(); + + ReturnCode return_code = vendor_disconnect(); + opened_ = false; + + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Controller disconnect failed for '%s'", control_port_name_.c_str()); + return return_code; + } + + PI_INFO("DriverController", InfoLevel::ESSENTIAL_0, "Disconnected from arm controller at '%s'", + control_port_name_.c_str()); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::read_hardware_values(Servo* p_servo) { + ServoController* p_servo_controller = dynamic_cast(p_servo); + if (p_servo_controller == nullptr) { + PI_ERROR("read_hardware_values() called with a non-ServoController servo"); + return ReturnCode::INVALID_PARAM; + } + + const int data_index = p_servo_controller->data_index_; + ReturnCode return_code = validate_data_index(data_index); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + JointState state; + { + std::lock_guard lock(state_mutex_); + state = states_[data_index]; + } + if (!state.valid) { + PI_ERROR("Joint slot %d has no valid controller state yet (servo ID %d)", data_index, + p_servo_controller->id_); + return ReturnCode::NO_RESPONSE; + } + + p_servo_controller->update_from_controller_state(state.position, state.velocity, state.effort, state.temperature); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::group_read_hardware_values() { + if (!opened_) { + PI_ERROR("group_read_hardware_values() called before open()"); + return ReturnCode::NOT_INITIALIZED; + } + + std::vector states(joint_count_); + ReturnCode return_code = vendor_read_state(states); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Whole-arm state read failed for controller at '%s'", control_port_name_.c_str()); + return return_code; + } + + { + std::lock_guard lock(state_mutex_); + states_ = std::move(states); + } + read_success_count_++; + feed_watchdog(); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::group_write_hardware_values() { + if (!opened_) { + PI_ERROR("group_write_hardware_values() called before open()"); + return ReturnCode::NOT_INITIALIZED; + } + if (watchdog_tripped_) { + PI_ERROR("Stall watchdog idled the arm; refusing further commands (restart required)"); + return ReturnCode::SAFE_MODE_SIG; + } + + std::vector commands; + std::vector modes; + bool apply_modes = false; + { + std::lock_guard lock(command_mutex_); + if (modes_dirty_) { + modes = desired_modes_; + apply_modes = true; + modes_dirty_ = false; + } + commands = commands_; + for (auto& command : commands_) { + command.pending = false; + } + } + + if (apply_modes) { + ReturnCode return_code = vendor_apply_command_modes(modes); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to apply joint control modes on controller at '%s'", control_port_name_.c_str()); + { + // Restore the dirty flag so the next flush retries the mode change + // before any command is interpreted under a stale mode. + std::lock_guard lock(command_mutex_); + modes_dirty_ = true; + } + return return_code; + } + { + // The arm is active again; a later shutdown must run vendor_make_safe() + // even when an earlier make_safe() already latched. + std::lock_guard lock(safe_mutex_); + made_safe_ = false; + } + } + + bool any_pending = false; + for (const auto& command : commands) { + if (command.pending) { + any_pending = true; + break; + } + } + if (any_pending) { + ReturnCode return_code = vendor_write_commands(commands); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Whole-arm command write failed for controller at '%s'", control_port_name_.c_str()); + return return_code; + } + } + + feed_watchdog(); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::arm_comm_loss_protection() { + if (!opened_) { + PI_ERROR("arm_comm_loss_protection() called before open()"); + return ReturnCode::NOT_INITIALIZED; + } + if (watchdog_running_) { + return ReturnCode::SUCCESS; + } + + last_interaction_ms_ = steady_now_ms(); + watchdog_running_ = true; + watchdog_thread_ = std::thread(&DriverController::watchdog_loop, this); + + PI_INFO("DriverController", InfoLevel::ESSENTIAL_0, + "Stall watchdog armed (timeout %d ms): the arm is idled when the host control loop stops", + CONTROLLER_STALL_WATCHDOG_TIMEOUT_MS); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::set_joint_command_mode(int data_index, JointCommandMode mode) { + ReturnCode return_code = validate_data_index(data_index); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + if (mode == JointCommandMode::UNSET) { + PI_ERROR("Joint slot %d: UNSET is not a requestable control mode", data_index); + return ReturnCode::INVALID_PARAM; + } + + std::lock_guard lock(command_mutex_); + if (desired_modes_[data_index] == mode) { + return ReturnCode::SUCCESS; + } + desired_modes_[data_index] = mode; + modes_dirty_ = true; + // Drop a command queued for the previous mode; it must not be sent under + // the new mode. + commands_[data_index] = JointCommand{}; + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::queue_position(int data_index, float position, float velocity_ff) { + ReturnCode return_code = validate_data_index(data_index); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + std::lock_guard lock(command_mutex_); + if (desired_modes_[data_index] != JointCommandMode::POSITION) { + PI_ERROR("Joint slot %d: position command queued while the joint is not in POSITION mode", data_index); + return ReturnCode::INVALID_PARAM; + } + JointCommand& command = commands_[data_index]; + command.mode = JointCommandMode::POSITION; + command.position = position; + command.velocity_ff = velocity_ff; + command.external_effort = 0; + command.pending = true; + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::queue_external_effort(int data_index, float external_effort) { + ReturnCode return_code = validate_data_index(data_index); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + std::lock_guard lock(command_mutex_); + if (desired_modes_[data_index] != JointCommandMode::EXTERNAL_EFFORT) { + PI_ERROR("Joint slot %d: external effort queued while the joint is not in EXTERNAL_EFFORT mode", data_index); + return ReturnCode::INVALID_PARAM; + } + JointCommand& command = commands_[data_index]; + command.mode = JointCommandMode::EXTERNAL_EFFORT; + command.external_effort = external_effort; + command.position = 0; + command.velocity_ff = 0; + command.pending = true; + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::make_safe() { + std::lock_guard lock(safe_mutex_); + if (made_safe_ || !opened_) { + return ReturnCode::SUCCESS; + } + + ReturnCode return_code = vendor_make_safe(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to put the arm controller at '%s' into its safe passive state", control_port_name_.c_str()); + return return_code; + } + + made_safe_ = true; + { + std::lock_guard command_lock(command_mutex_); + for (auto& mode : desired_modes_) { + mode = JointCommandMode::UNSET; + } + for (auto& command : commands_) { + command = JointCommand{}; + } + modes_dirty_ = false; + } + + PI_INFO("DriverController", InfoLevel::ESSENTIAL_0, "Arm controller at '%s' put into safe passive state", + control_port_name_.c_str()); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::validate_data_index(int data_index) const { + if (data_index < 0 || data_index >= joint_count_) { + PI_ERROR("Joint slot %d is out of range (controller manages %d joints)", data_index, joint_count_); + return ReturnCode::INVALID_PARAM; + } + return ReturnCode::SUCCESS; +} + +void DriverController::watchdog_loop() { + while (watchdog_running_) { + std::this_thread::sleep_for(std::chrono::milliseconds(CONTROLLER_STALL_WATCHDOG_PERIOD_MS)); + if (!watchdog_running_) { + break; + } + + const int64_t idle_ms = steady_now_ms() - last_interaction_ms_; + if (idle_ms <= CONTROLLER_STALL_WATCHDOG_TIMEOUT_MS) { + continue; + } + if (watchdog_tripped_) { + continue; + } + + PI_ERROR( + "Stall watchdog: no driver interaction for %lld ms (host control loop hang?); " + "idling the arm controller at '%s'", + (long long)idle_ms, control_port_name_.c_str()); + watchdog_tripped_ = true; + make_safe(); + } +} + +void DriverController::feed_watchdog() { + last_interaction_ms_ = steady_now_ms(); +} + +void DriverController::stop_watchdog() { + watchdog_running_ = false; + if (watchdog_thread_.joinable()) { + watchdog_thread_.join(); + } +} diff --git a/native/pi_control/src/pi_driver_trossen.cpp b/native/pi_control/src/pi_driver_trossen.cpp new file mode 100644 index 0000000..d21b285 --- /dev/null +++ b/native/pi_control/src/pi_driver_trossen.cpp @@ -0,0 +1,211 @@ +/*! + * @file pi_driver_trossen.cpp + * @brief Implementation of the DriverTrossen Trossen iNerve controller driver. + */ + +#include "pi_driver_trossen.hpp" + +// Streaming command interpolation horizon. 0 executes the target immediately +// (the host-side trajectory planning / leash already shapes the motion); the +// SDK treats sub-millisecond horizons as direct linear stepping. +#define TROSSEN_STREAM_GOAL_TIME_S 0.0 + +// TCP handshake timeout for configure(). Generous enough for a controller +// that is still booting, small enough to fail fast on a wrong IP. +#define TROSSEN_CONFIGURE_TIMEOUT_S 10.0 + +DriverTrossen::DriverTrossen(Device* p_device, const CommandLineArgs& cla, const std::string& controller_model) + : DriverController(p_device, cla), controller_model_(controller_model) { + is_leader_role_ = (cla.role == Role::LEADER); +} + +DriverTrossen::~DriverTrossen() { + // Must run here (not only in the base destructor): close() calls the + // vendor hooks, which are gone once this destructor finished. + close(); +} + +ReturnCode DriverTrossen::vendor_connect(const std::string& address, int& joint_count) { + p_arm_ = std::make_unique(); + // clear_error = true: recover from a stale error latched by a previous + // session (e.g. a hard-killed host) instead of refusing to configure. + // Leader and follower differ in end-effector mass properties (the leader + // carries the teleop handle); using the wrong set skews the controller's + // own gravity compensation. + if (!p_arm_->configure(controller_model_, is_leader_role_, address, true, TROSSEN_CONFIGURE_TIMEOUT_S)) { + PI_ERROR("libtrossen_arm configure failed for '%s': %s", address.c_str(), p_arm_->last_error().c_str()); + p_arm_.reset(); + return ReturnCode::NO_RESPONSE; + } + + joint_count = p_arm_->num_joints(); + PI_INFO("DriverTrossen", InfoLevel::ESSENTIAL_0, + "Configured Trossen controller at '%s': model=%s, driver=%s, controller_firmware=%s, joints=%d", + address.c_str(), controller_model_.c_str(), p_arm_->driver_version().c_str(), + p_arm_->controller_version().c_str(), joint_count); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverTrossen::vendor_disconnect() { + if (p_arm_ == nullptr) { + return ReturnCode::SUCCESS; + } + + const bool clean = p_arm_->cleanup(); + if (!clean) { + PI_ERROR("libtrossen_arm cleanup failed: %s", p_arm_->last_error().c_str()); + } + p_arm_.reset(); + return clean ? ReturnCode::SUCCESS : ReturnCode::FAIL; +} + +ReturnCode DriverTrossen::vendor_read_state(std::vector& states) { + if (p_arm_ == nullptr) { + PI_ERROR("vendor_read_state() called without a configured Trossen driver"); + return ReturnCode::NOT_INITIALIZED; + } + + std::vector positions; + std::vector velocities; + std::vector efforts; + std::vector temperatures; + if (!p_arm_->read_state(positions, velocities, efforts, temperatures)) { + PI_ERROR("libtrossen_arm state read failed: %s", p_arm_->last_error().c_str()); + return ReturnCode::NO_RESPONSE; + } + + if (positions.size() != states.size() || velocities.size() != states.size() || efforts.size() != states.size() || + temperatures.size() != states.size()) { + PI_ERROR("Trossen state size mismatch: expected %zu joints, got pos=%zu vel=%zu eff=%zu temp=%zu", + states.size(), positions.size(), velocities.size(), efforts.size(), temperatures.size()); + return ReturnCode::HARDWARE_FAULT; + } + + for (size_t i = 0; i < states.size(); i++) { + states[i].position = (float)positions[i]; + states[i].velocity = (float)velocities[i]; + states[i].effort = (float)efforts[i]; + states[i].temperature = (float)temperatures[i]; + states[i].valid = true; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverTrossen::vendor_write_commands(const std::vector& commands) { + if (p_arm_ == nullptr) { + PI_ERROR("vendor_write_commands() called without a configured Trossen driver"); + return ReturnCode::NOT_INITIALIZED; + } + + const int joint_count = (int)commands.size(); + const int arm_joint_count = joint_count - 1; // Trossen layout: N-1 arm joints + 1 gripper (last index). + const int gripper_index = joint_count - 1; + + // Whole-group transactions when every arm joint carries the same pending + // command type; per-joint fallback otherwise (e.g. during staggered + // start-up moves). + int arm_pending_positions = 0; + int arm_pending_efforts = 0; + for (int i = 0; i < arm_joint_count; i++) { + if (!commands[i].pending) { + continue; + } + if (commands[i].mode == JointCommandMode::POSITION) { + arm_pending_positions++; + } else if (commands[i].mode == JointCommandMode::EXTERNAL_EFFORT) { + arm_pending_efforts++; + } + } + + bool write_ok = true; + if (arm_pending_positions == arm_joint_count) { + std::vector positions(arm_joint_count); + std::vector velocity_ffs(arm_joint_count); + for (int i = 0; i < arm_joint_count; i++) { + positions[i] = commands[i].position; + velocity_ffs[i] = commands[i].velocity_ff; + } + write_ok = p_arm_->set_arm_positions(positions, TROSSEN_STREAM_GOAL_TIME_S, false, velocity_ffs); + } else if (arm_pending_efforts == arm_joint_count) { + std::vector efforts(arm_joint_count); + for (int i = 0; i < arm_joint_count; i++) { + efforts[i] = commands[i].external_effort; + } + write_ok = p_arm_->set_arm_external_efforts(efforts, TROSSEN_STREAM_GOAL_TIME_S, false); + } else { + for (int i = 0; i < arm_joint_count && write_ok; i++) { + if (!commands[i].pending) { + continue; + } + if (commands[i].mode == JointCommandMode::POSITION) { + write_ok = p_arm_->set_joint_position(i, commands[i].position, TROSSEN_STREAM_GOAL_TIME_S, false, + commands[i].velocity_ff); + } else if (commands[i].mode == JointCommandMode::EXTERNAL_EFFORT) { + write_ok = p_arm_->set_joint_external_effort(i, commands[i].external_effort, + TROSSEN_STREAM_GOAL_TIME_S, false); + } + } + } + + if (write_ok && commands[gripper_index].pending) { + const JointCommand& gripper = commands[gripper_index]; + if (gripper.mode == JointCommandMode::POSITION) { + write_ok = p_arm_->set_gripper_position(gripper.position, TROSSEN_STREAM_GOAL_TIME_S, false, + gripper.velocity_ff); + } else if (gripper.mode == JointCommandMode::EXTERNAL_EFFORT) { + write_ok = p_arm_->set_gripper_external_effort(gripper.external_effort, TROSSEN_STREAM_GOAL_TIME_S, false); + } + } + + if (!write_ok) { + PI_ERROR("libtrossen_arm command write failed: %s", p_arm_->last_error().c_str()); + return ReturnCode::FAIL; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverTrossen::vendor_apply_command_modes(const std::vector& modes) { + if (p_arm_ == nullptr) { + PI_ERROR("vendor_apply_command_modes() called without a configured Trossen driver"); + return ReturnCode::NOT_INITIALIZED; + } + + std::vector shim_modes(modes.size(), TrossenArmShim::Mode::IDLE); + for (size_t i = 0; i < modes.size(); i++) { + switch (modes[i]) { + case JointCommandMode::POSITION: + shim_modes[i] = TrossenArmShim::Mode::POSITION; + break; + case JointCommandMode::EXTERNAL_EFFORT: + shim_modes[i] = TrossenArmShim::Mode::EXTERNAL_EFFORT; + break; + case JointCommandMode::UNSET: + shim_modes[i] = TrossenArmShim::Mode::IDLE; + break; + } + } + + if (!p_arm_->set_joint_modes(shim_modes)) { + PI_ERROR("libtrossen_arm set_joint_modes failed: %s", p_arm_->last_error().c_str()); + return ReturnCode::FAIL; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverTrossen::vendor_make_safe() { + if (p_arm_ == nullptr) { + return ReturnCode::SUCCESS; + } + + // idle: the controller stops executing streamed commands and holds the + // arm passively with its own gravity/friction compensation. + if (!p_arm_->set_all_modes_idle()) { + PI_ERROR("libtrossen_arm set_all_modes(idle) failed: %s", p_arm_->last_error().c_str()); + return ReturnCode::FAIL; + } + + return ReturnCode::SUCCESS; +} diff --git a/native/pi_control/src/pi_servo.cpp b/native/pi_control/src/pi_servo.cpp index 110f193..7f22822 100644 --- a/native/pi_control/src/pi_servo.cpp +++ b/native/pi_control/src/pi_servo.cpp @@ -10,6 +10,7 @@ #include "pi_joint.hpp" #include "pi_servo_can_encoder.hpp" +#include "pi_servo_controller.hpp" #include "pi_servo_dm.hpp" #define MAX_CNT_POS_EXCEED \ @@ -364,6 +365,10 @@ ReturnCode Servo::new_servos(const json& joint_config, p_servo = std::make_unique(p_device, p_joint, p_driver); PI_INFO("Servo", InfoLevel::HELPFUL_1, "Created ServoDm instance"); + } else if (servo_model == p_config_model->val_servo_model_trossen_wxai) { + p_servo = std::make_unique(p_device, p_joint, p_driver); + PI_INFO("Servo", InfoLevel::HELPFUL_1, + "Created ServoController instance"); } else if (servo_model == p_config_model->val_servo_model_can_passive_encoder) { p_servo = std::make_unique(p_device, p_joint, diff --git a/native/pi_control/src/pi_servo_controller.cpp b/native/pi_control/src/pi_servo_controller.cpp new file mode 100644 index 0000000..1f0a7c7 --- /dev/null +++ b/native/pi_control/src/pi_servo_controller.cpp @@ -0,0 +1,201 @@ +/*! + * @file pi_servo_controller.cpp + * @brief Implementation of the ServoController per-joint view onto a whole-arm controller. + */ + +#include "pi_servo_controller.hpp" + +#include "pi_joint.hpp" + +const ServoControllerParam g_servo_controller_param(DEFAULT_TOLERABLE_POS_DIFFERENCE_RAD, MAX_POS_DIFFERENCE_RAD, + DEFAULT_VELOCITY_THRESHOLD_RAD_SEC); + +ServoController::ServoController(Device* p_device, Joint* p_joint, Driver* p_driver) + : Servo(p_device, p_joint, p_driver) { + p_driver_controller_ = dynamic_cast(p_driver); +} + +ServoController::~ServoController() {} + +ReturnCode ServoController::init_config_model(const json& servo_config, const DeviceConfig* p_config) { + p_servo_param_ = &g_servo_controller_param; + + ReturnCode return_code = Servo::init_config_model(servo_config, p_config); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + if (servo_model_ == p_config->val_servo_model_trossen_wxai) { + type_ = ServoType::CONTROLLER_JOINT; + } else { + PI_ERROR("Unsupported servo model '%s' (servo ID %d)", servo_model_.c_str(), id_); + return ReturnCode::NOT_SUPPORTED; + } + + if (p_driver_controller_ == nullptr) { + PI_ERROR("Servo ID %d requires a DriverController-based driver (check driver_type in the model config)", id_); + return ReturnCode::INVALID_PARAM; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::start_hardware() { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_ == nullptr) { + PI_ERROR("Device pointer is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping start hardware", id_); + return ReturnCode::SUCCESS; + } + + // POSITION mode makes the controller hold the current pose; the mode is + // applied together with the hold-at-current seed in the first command flush, + // so there is no window where a stale target could be executed. + ReturnCode return_code = + p_driver_controller_->set_joint_command_mode(data_index_, DriverController::JointCommandMode::POSITION); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to request POSITION mode (servo ID %d)", id_); + return return_code; + } + + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo %d started hardware (controller joint slot %d)", id_, + data_index_); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::park_safely() { + if (parked_ == true) { + return ReturnCode::SUCCESS; + } + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping park safely", id_); + return ReturnCode::SUCCESS; + } + if (p_driver_controller_ == nullptr) { + PI_ERROR("Controller driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + // Whole-arm controllers park all joints at once; make_safe() is idempotent + // so the first parked joint idles the arm and the rest are no-ops. + ReturnCode return_code = p_driver_controller_->make_safe(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to park controller joint (servo ID %d)", id_); + return return_code; + } + + parked_ = true; + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d parked safely (controller idled)", id_); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::verify_position_fresh() { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Controller driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (!p_driver_controller_->has_valid_state()) { + PI_ERROR("Servo ID %d: no whole-arm state has been received from the controller yet", id_); + return ReturnCode::NO_RESPONSE; + } + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::move(float target_pos) { + return move(target_pos, 0, 0); +} + +ReturnCode ServoController::move(float target_pos, float target_vel, float target_tor) { + // The controller runs its own servo loops; per-command torque bounds are + // not part of the whole-arm command set (joint limits bound the effort). + (void)target_tor; + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + float clipped_pos = clipping(target_pos, pos_min_rel_, pos_max_rel_); + float target_pos_absolute = get_pos_rad_absolute(clipped_pos); + float velocity_ff = target_vel * dir_invert_; + + ReturnCode return_code = p_driver_controller_->queue_position(data_index_, target_pos_absolute, velocity_ff); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to queue position command (servo ID %d)", id_); + return return_code; + } + + PI_INFO("Servo", InfoLevel::FREQUENT_3, "Joint ID %d, Servo ID %d: move target_pos=%.3f, absolute=%.3f", + p_joint_->id_, id_, target_pos, target_pos_absolute); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::apply_torque(float torque) { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + ReturnCode return_code = p_driver_controller_->queue_external_effort(data_index_, torque * dir_invert_); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to queue external effort command (servo ID %d)", id_); + return return_code; + } + + PI_INFO("Servo", InfoLevel::FREQUENT_3, "Joint ID %d, Servo ID %d: external effort %.3f", p_joint_->id_, id_, + torque); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::apply_torque_with_damping(float torque) { + // Damping runs inside the vendor controller's servo loops. + return apply_torque(torque); +} + +ReturnCode ServoController::change_control_mode_for_leader() { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: device is read only, skipping change_control_mode_for_leader", id_); + return ReturnCode::SUCCESS; + } + + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: leader mode, requesting EXTERNAL_EFFORT (controller gravity compensation stays active)", + id_); + return p_driver_controller_->set_joint_command_mode(data_index_, + DriverController::JointCommandMode::EXTERNAL_EFFORT); +} + +ReturnCode ServoController::change_control_mode_for_follower() { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: device is read only, skipping change_control_mode_for_follower", id_); + return ReturnCode::SUCCESS; + } + + return p_driver_controller_->set_joint_command_mode(data_index_, DriverController::JointCommandMode::POSITION); +} + +void ServoController::update_from_controller_state(float position, float velocity, float effort, float temperature) { + curr_pos_abs_ = position; + curr_vel_ = velocity; + curr_tor_ = effort; + temperature_ = temperature; +} diff --git a/native/pi_control/src/pi_trossen_shim.cpp b/native/pi_control/src/pi_trossen_shim.cpp new file mode 100644 index 0000000..04eac4a --- /dev/null +++ b/native/pi_control/src/pi_trossen_shim.cpp @@ -0,0 +1,283 @@ +/*! + * @file pi_trossen_shim.cpp + * @brief Implementation of TrossenArmShim (compiled into libpi_trossen_shim). + * + * This is the only translation unit that includes vendor headers. It is built + * into a shared library together with the prebuilt libtrossen_arm.a; the + * archive's symbols (including its bundled pinocchio/urdfdom/tinyxml2 copies) + * are hidden from the rest of the process (see CMakeLists.txt), which is what + * prevents the ODR clash with the node's own shared pinocchio. + */ + +#include "pi_trossen_shim.hpp" + +#include + +#include "libtrossen_arm/trossen_arm.hpp" + +// Trossen model strings accepted in model configs (fn_controller_model). +#define TROSSEN_MODEL_WXAI_V0 "wxai_v0" + +struct TrossenArmShim::Impl { + trossen_arm::TrossenArmDriver driver; +}; + +namespace { + +trossen_arm::Mode to_vendor_mode(TrossenArmShim::Mode mode) { + switch (mode) { + case TrossenArmShim::Mode::POSITION: + return trossen_arm::Mode::position; + case TrossenArmShim::Mode::EXTERNAL_EFFORT: + return trossen_arm::Mode::external_effort; + case TrossenArmShim::Mode::IDLE: + break; + } + return trossen_arm::Mode::idle; +} + +} // namespace + +TrossenArmShim::TrossenArmShim() = default; + +TrossenArmShim::~TrossenArmShim() { + if (impl_ != nullptr) { + try { + impl_->driver.cleanup(); + } catch (const std::exception&) { + // Destructor: nothing actionable; the connection dies with the process. + } + } +} + +bool TrossenArmShim::configure(const std::string& model, bool leader_end_effector, const std::string& address, + bool clear_error, double timeout_s) { + trossen_arm::Model vendor_model; + if (model == TROSSEN_MODEL_WXAI_V0) { + vendor_model = trossen_arm::Model::wxai_v0; + } else { + last_error_ = "unsupported Trossen controller model '" + model + "' (supported: " TROSSEN_MODEL_WXAI_V0 ")"; + return false; + } + + const trossen_arm::EndEffector end_effector = leader_end_effector + ? trossen_arm::StandardEndEffector::wxai_v0_leader + : trossen_arm::StandardEndEffector::wxai_v0_follower; + + try { + impl_ = std::make_unique(); + impl_->driver.configure(vendor_model, end_effector, address, clear_error, timeout_s); + } catch (const std::exception& e) { + last_error_ = e.what(); + impl_.reset(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::cleanup() { + if (impl_ == nullptr) { + return true; + } + + try { + impl_->driver.cleanup(); + } catch (const std::exception& e) { + last_error_ = e.what(); + impl_.reset(); + return false; + } + + impl_.reset(); + last_error_.clear(); + return true; +} + +int TrossenArmShim::num_joints() const { + if (impl_ == nullptr) { + return 0; + } + return (int)impl_->driver.get_num_joints(); +} + +std::string TrossenArmShim::driver_version() const { + if (impl_ == nullptr) { + return ""; + } + return impl_->driver.get_driver_version(); +} + +std::string TrossenArmShim::controller_version() const { + if (impl_ == nullptr) { + return ""; + } + return impl_->driver.get_controller_version(); +} + +bool TrossenArmShim::read_state(std::vector& positions, std::vector& velocities, + std::vector& efforts, std::vector& temperatures) { + if (impl_ == nullptr) { + last_error_ = "read_state() called without a configured driver"; + return false; + } + + try { + positions = impl_->driver.get_all_positions(); + velocities = impl_->driver.get_all_velocities(); + efforts = impl_->driver.get_all_efforts(); + temperatures = impl_->driver.get_all_rotor_temperatures(); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_arm_positions(const std::vector& positions, double goal_time, bool blocking, + const std::vector& velocity_ffs) { + if (impl_ == nullptr) { + last_error_ = "set_arm_positions() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_arm_positions(positions, goal_time, blocking, velocity_ffs); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_arm_external_efforts(const std::vector& efforts, double goal_time, bool blocking) { + if (impl_ == nullptr) { + last_error_ = "set_arm_external_efforts() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_arm_external_efforts(efforts, goal_time, blocking); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_joint_position(int joint_index, double position, double goal_time, bool blocking, + double velocity_ff) { + if (impl_ == nullptr) { + last_error_ = "set_joint_position() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_joint_position((uint8_t)joint_index, position, goal_time, blocking, velocity_ff); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_joint_external_effort(int joint_index, double effort, double goal_time, bool blocking) { + if (impl_ == nullptr) { + last_error_ = "set_joint_external_effort() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_joint_external_effort((uint8_t)joint_index, effort, goal_time, blocking); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_gripper_position(double position, double goal_time, bool blocking, double velocity_ff) { + if (impl_ == nullptr) { + last_error_ = "set_gripper_position() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_gripper_position(position, goal_time, blocking, velocity_ff); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_gripper_external_effort(double effort, double goal_time, bool blocking) { + if (impl_ == nullptr) { + last_error_ = "set_gripper_external_effort() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_gripper_external_effort(effort, goal_time, blocking); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_joint_modes(const std::vector& modes) { + if (impl_ == nullptr) { + last_error_ = "set_joint_modes() called without a configured driver"; + return false; + } + + std::vector vendor_modes(modes.size(), trossen_arm::Mode::idle); + for (size_t i = 0; i < modes.size(); i++) { + vendor_modes[i] = to_vendor_mode(modes[i]); + } + + try { + impl_->driver.set_joint_modes(vendor_modes); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_all_modes_idle() { + if (impl_ == nullptr) { + last_error_ = "set_all_modes_idle() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_all_modes(trossen_arm::Mode::idle); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +const std::string& TrossenArmShim::last_error() const { return last_error_; } diff --git a/src/openpi_control/config.py b/src/openpi_control/config.py index f224a20..073cdd8 100644 --- a/src/openpi_control/config.py +++ b/src/openpi_control/config.py @@ -22,9 +22,17 @@ "ARX_ENC", "ARX_L5", "ARX_X5", + "Trossen_wai_ctrl", "Yam", ) -SUPPORTED_EFFECTORS = ("E_ARX", "E_ARX_ENC", "E_Yam", "E_Yam_Handle", "E_Yam_Handle_compat") +SUPPORTED_EFFECTORS = ( + "E_ARX", + "E_ARX_ENC", + "E_Trossen_ctrl", + "E_Yam", + "E_Yam_Handle", + "E_Yam_Handle_compat", +) @dataclass(frozen=True, slots=True) diff --git a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json new file mode 100644 index 0000000..2ce0345 --- /dev/null +++ b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json @@ -0,0 +1,268 @@ +{ + "config_version": "1.1.1", + "device_model": "Trossen_wai_ctrl", + "device_type": "arm", + "arm_type": "controller", + "topic_type": "ROS", + "driver_type": "TROSSEN_ETHERNET", + "controller_model": "wxai_v0", + "algo_type": "KDL", + "planning_type": "None", + "catalog": { + "display_label": "Trossen_wai_ctrl: Trossen WidowX AI (Ethernet controller)", + "order": 8, + "baudrate": 1000000, + "default_effector_model": "E_Trossen_ctrl", + "port_type": "Ethernet" + }, + "joint_init_sequence": [ + 3, + 2, + 5, + 4, + 1, + 6 + ], + "joints": [ + { + "joint_id": 0, + "nominal_stall_ok": false, + "vel_max": 0.5, + "follow_vel_max": 2.5, + "torq_min": -15, + "torq_max": 15, + "safe_torq_min": -7, + "safe_torq_max": 7, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.1, + "spring_preload": 0.0, + "spring_force_config": true, + "spring_type": 2, + "threshold_angle_change": 0.00872664625997165, + "threshold_time_sec": 0.3, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 1, + "data_index": 0, + "dir_invert": 1, + "pos_min": -3.1416, + "pos_max": 3.1416, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 1, + "nominal_stall_ok": true, + "vel_max": 0.5, + "follow_vel_max": 2.6, + "torq_min": -15, + "torq_max": 15, + "safe_torq_min": -7, + "safe_torq_max": 7, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 2, + "data_index": 1, + "dir_invert": 1, + "pos_min": 0.0, + "pos_max": 3.1416, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 2, + "nominal_stall_ok": true, + "vel_max": 0.5, + "follow_vel_max": 2.8, + "torq_min": -15, + "torq_max": 15, + "safe_torq_min": -7, + "safe_torq_max": 7, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.4, + "spring_preload": 0, + "spring_force_config": true, + "spring_type": -1, + "threshold_angle_change": 0.00872664625997165, + "threshold_time_sec": 0.3, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 3, + "data_index": 2, + "dir_invert": 1, + "pos_min": 0.0, + "pos_max": 2.3562, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 3, + "nominal_stall_ok": true, + "vel_max": 0.5, + "follow_vel_max": 6.0, + "torq_min": -7, + "torq_max": 7, + "safe_torq_min": -5, + "safe_torq_max": 5, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.0, + "spring_preload": 0.0, + "spring_force_config": false, + "spring_type": 1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 4, + "data_index": 3, + "dir_invert": 1, + "pos_min": -1.5708, + "pos_max": 1.5708, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 4, + "nominal_stall_ok": false, + "vel_max": 0.5, + "follow_vel_max": 6.0, + "torq_min": -7, + "torq_max": 7, + "safe_torq_min": -5, + "safe_torq_max": 5, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.0, + "spring_preload": 0.0, + "spring_force_config": false, + "spring_type": 1, + "threshold_angle_change": 0, + "threshold_time_sec": 10, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 5, + "data_index": 4, + "dir_invert": 1, + "pos_min": -1.5708, + "pos_max": 1.5708, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 5, + "nominal_stall_ok": false, + "vel_max": 0.5, + "follow_vel_max": 6.0, + "torq_min": -7, + "torq_max": 7, + "safe_torq_min": -5, + "safe_torq_max": 5, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.0, + "spring_preload": 0.0, + "spring_force_config": false, + "spring_type": 1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 6, + "data_index": 5, + "dir_invert": 1, + "pos_min": -3.1416, + "pos_max": 3.1416, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.urdf b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.urdf new file mode 100644 index 0000000..bc943e0 --- /dev/null +++ b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.urdf @@ -0,0 +1,230 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json new file mode 100644 index 0000000..1daf2fa --- /dev/null +++ b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json @@ -0,0 +1,87 @@ +{ + "config_version": "1.1.1", + "device_model": "Trossen_wai_ctrl", + "device_type": "arm", + "arm_type": "controller", + "spring_effect": false, + "gravity_compensation": false, + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 0, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 1, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 2, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 2, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 3, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 3, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 4, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 4, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 5, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 5, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 6, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json new file mode 100644 index 0000000..bb0ff0c --- /dev/null +++ b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json @@ -0,0 +1,66 @@ +{ + "config_version": "1.1.1", + "device_model": "E_Trossen_ctrl", + "device_type": "effector", + "effector_type": "controller", + "topic_type": "ROS", + "driver_type": "TROSSEN_ETHERNET", + "algo_type": "Algo", + "planning_type": "None", + "catalog": { + "display_label": "E_Trossen_ctrl: Trossen Gripper (Ethernet controller)", + "order": 8, + "baudrate": 1000000, + "need_repeated_command": true + }, + "joint_init_sequence": [ + 3, + 2, + 5, + 4, + 1, + 6 + ], + "joints": [ + { + "joint_id": 6, + "vel_max": 0.1, + "torq_min": -120, + "torq_max": 120, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1, + "pos_rescale": 1, + "pos_error_margin": 0.01, + "pos_max_safety_margin": 0.002, + "safe_mode_derating": 0.2, + "accel_max": 1.0, + "spring_constant": 25, + "spring_preload": 0, + "spring_force_config": true, + "spring_type": 1, + "threshold_angle_change": 0.0005, + "threshold_time_sec": 0.1, + "max_length": 0.04, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 7, + "data_index": 6, + "dir_invert": 1, + "reverse_flag": false, + "pos_max": 0.04, + "pos_min": 0, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json new file mode 100644 index 0000000..ba7c9c3 --- /dev/null +++ b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json @@ -0,0 +1,28 @@ +{ + "config_version": "1.1.1", + "device_model": "E_Trossen_ctrl", + "device_type": "effector", + "effector_type": "controller", + "spring_effect": false, + "control_mode": "position", + "dist_to_torque_const": 2.0, + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 6, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 7, + "zero_pos": 0.0, + "home_pos": 0, + "spring_home_pos": 0.03 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_mass.json b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_mass.json new file mode 100644 index 0000000..e77917e --- /dev/null +++ b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_mass.json @@ -0,0 +1,16 @@ +{ + "mass": 0.47779, + "center_of_mass": [ + 0.03767, + 0.00074, + 0.01295 + ], + "inertia": { + "ixx": 0.00044008483, + "iyy": 0.00058988009, + "izz": 0.00037294564, + "ixy": 0.00000154213, + "ixz": 0.00009455425, + "iyz": 0.00001636757 + } +} \ No newline at end of file diff --git a/src/openpi_control/native.py b/src/openpi_control/native.py index 51d5192..08603f6 100644 --- a/src/openpi_control/native.py +++ b/src/openpi_control/native.py @@ -21,7 +21,13 @@ from . import log_paths from .backend import ArmBackend -from .config import ArmConfig, InputLayout, ResolvedArmAssets, SocketCanConnection +from .config import ( + ArmConfig, + ArmConnection, + EthernetConnection, + InputLayout, + ResolvedArmAssets, +) from .exceptions import ( CommandRejectedError, ConfigurationError, @@ -47,6 +53,7 @@ encode_command, port_candidates, ) +from .servos import trossen_eth from .types import ( ArmCapabilities, ArmMode, @@ -107,7 +114,14 @@ def native_executable() -> Path: return Path(found) if found else packaged -def validate_connection(connection: SocketCanConnection) -> None: +def validate_connection(connection: ArmConnection) -> None: + if isinstance(connection, EthernetConnection): + if not trossen_eth.reachable(connection.ip): + raise ConnectionUnavailableError( + f"Ethernet controller at {connection.ip} is not reachable; check the cable, " + "power, and that the host has an address on the controller's subnet" + ) + return path = Path("/sys/class/net") / connection.interface if not path.exists(): raise ConnectionUnavailableError( @@ -360,7 +374,12 @@ def _connect_prepared( self._inputs_sub = _Subscriber(self._context, topics.inputs) if role is ArmRole.FOLLOWER: self._direct_pub = _Publisher(self._context, topics.direct_command) - connection_args = ["--control_port", config.connection.interface] + # The native node takes the bus identity as an opaque string: a SocketCAN + # interface name, or the controller IPv4 address for Ethernet drivers. + if isinstance(config.connection, EthernetConnection): + connection_args = ["--control_port", config.connection.ip] + else: + connection_args = ["--control_port", config.connection.interface] args = [ str(executable), "--role", From d6f77be61c158d9d331b34d6980176cb0e06dd16 Mon Sep 17 00:00:00 2001 From: Adnan Esmail Date: Fri, 24 Jul 2026 13:47:21 -0700 Subject: [PATCH 5/5] native+models: SO-ARM101 FeeTech STS3215 serial bus support New FEETECH driver stack for USB-serial bus-servo arms: - DriverSerial: protocol-neutral serial bus driver (a future Dynamixel port slots in beside it); DriverFt speaks the FeeTech SMS/STS protocol; ServoFt maps STS3215 registers (position/velocity/load, torque enable, profile acceleration via prof_accel). - DeviceArmNello / DeviceEffectorNello: the hobby bus-servo device types ('nello' arm/effector); the effector toggles gripper torque through the new Servo::enable_torque() virtual, which fast-fails on families without a torque-enable register. - Baud plumbing end to end: --baud_rate (default 3,000,000 bps) beside the existing node options, start(cla.baud_rate), and the Python backend passing the model catalog's declared baud with the serial device path. - SO101 + E_SO101 model assets (5-DOF; the catalog's only non-6-DOF arm) and a dedicated movements-test trajectory CSV. - SerialConnection device-existence validation client-side. Inference configs are not included -- this lands the hardware bring-up path. Co-Authored-By: Claude Fable 5 --- native/pi_control/CMakeLists.txt | 13 +- .../include/pi_command_line_args.hpp | 2 + native/pi_control/include/pi_control.hpp | 15 +- .../include/pi_device_arm_nello.hpp | 42 + .../pi_control/include/pi_device_config.hpp | 5 + .../include/pi_device_effector_nello.hpp | 76 + native/pi_control/include/pi_driver_ft.hpp | 312 ++++ .../pi_control/include/pi_driver_serial.hpp | 89 ++ native/pi_control/include/pi_servo.hpp | 18 + native/pi_control/include/pi_servo_ft.hpp | 183 +++ .../pi_control/src/pi_command_line_args.cpp | 11 + native/pi_control/src/pi_control_node.cpp | 2 +- native/pi_control/src/pi_device.cpp | 10 + native/pi_control/src/pi_device_arm_nello.cpp | 74 + .../src/pi_device_effector_nello.cpp | 160 ++ native/pi_control/src/pi_driver.cpp | 7 +- native/pi_control/src/pi_driver_ft.cpp | 768 ++++++++++ native/pi_control/src/pi_driver_serial.cpp | 409 +++++ native/pi_control/src/pi_servo.cpp | 12 +- native/pi_control/src/pi_servo_ft.cpp | 395 +++++ .../arm_commands/movements_test_so101.csv | 1352 +++++++++++++++++ src/openpi_control/config.py | 2 + .../models/arms/SO101/SO101.json | 221 +++ .../models/arms/SO101/SO101.urdf | 414 +++++ .../models/arms/SO101/SO101_01.json | 75 + .../models/effectors/E_SO101/E_SO101.json | 58 + .../models/effectors/E_SO101/E_SO101_01.json | 32 + .../effectors/E_SO101/E_SO101_mass.json | 16 + src/openpi_control/native.py | 19 +- tests/test_config.py | 10 +- uv.lock | 37 +- 31 files changed, 4819 insertions(+), 20 deletions(-) create mode 100644 native/pi_control/include/pi_device_arm_nello.hpp create mode 100644 native/pi_control/include/pi_device_effector_nello.hpp create mode 100644 native/pi_control/include/pi_driver_ft.hpp create mode 100644 native/pi_control/include/pi_driver_serial.hpp create mode 100644 native/pi_control/include/pi_servo_ft.hpp create mode 100644 native/pi_control/src/pi_device_arm_nello.cpp create mode 100644 native/pi_control/src/pi_device_effector_nello.cpp create mode 100644 native/pi_control/src/pi_driver_ft.cpp create mode 100644 native/pi_control/src/pi_driver_serial.cpp create mode 100644 native/pi_control/src/pi_servo_ft.cpp create mode 100644 src/openpi_control/arm_commands/movements_test_so101.csv create mode 100644 src/openpi_control/models/arms/SO101/SO101.json create mode 100644 src/openpi_control/models/arms/SO101/SO101.urdf create mode 100644 src/openpi_control/models/arms/SO101/SO101_01.json create mode 100644 src/openpi_control/models/effectors/E_SO101/E_SO101.json create mode 100644 src/openpi_control/models/effectors/E_SO101/E_SO101_01.json create mode 100644 src/openpi_control/models/effectors/E_SO101/E_SO101_mass.json diff --git a/native/pi_control/CMakeLists.txt b/native/pi_control/CMakeLists.txt index 3b5b61f..8b98965 100644 --- a/native/pi_control/CMakeLists.txt +++ b/native/pi_control/CMakeLists.txt @@ -78,21 +78,28 @@ set(COMMON_SOURCES src/pi_driver.cpp src/pi_driver_arx.cpp src/pi_driver_arx_encoder.cpp + src/pi_driver_can.cpp src/pi_driver_controller.cpp src/pi_driver_trossen.cpp - src/pi_servo_controller.cpp - src/pi_device_effector_controller.cpp - src/pi_driver_can.cpp + # Serial bus-servo stack (protocol-neutral DriverSerial + FeeTech; a future + # Dynamixel port adds pi_driver_dxl.cpp / pi_servo_dxl.cpp here). + src/pi_driver_serial.cpp + src/pi_driver_ft.cpp + src/pi_servo_ft.cpp src/pi_joint.cpp src/pi_servo.cpp + src/pi_servo_controller.cpp src/pi_servo_dm.cpp src/pi_servo_dm_status.cpp src/pi_servo_can_encoder.cpp src/pi_device.cpp src/pi_device_arm.cpp src/pi_device_arm_arx.cpp + src/pi_device_arm_nello.cpp src/pi_device_effector.cpp src/pi_device_effector_arx.cpp + src/pi_device_effector_controller.cpp + src/pi_device_effector_nello.cpp src/pi_device_config.cpp src/pi_algo.cpp src/pi_algo_pino.cpp diff --git a/native/pi_control/include/pi_command_line_args.hpp b/native/pi_control/include/pi_command_line_args.hpp index 0c31a5f..c145069 100644 --- a/native/pi_control/include/pi_command_line_args.hpp +++ b/native/pi_control/include/pi_command_line_args.hpp @@ -16,6 +16,7 @@ #define OPT_EFFECTOR_MODEL "effector_model" ///< Effector model option. #define OPT_EFFECTOR_ID "effector_id" ///< Effector ID option. #define OPT_CONTROL_PORT "control_port" ///< Control port option. +#define OPT_BAUD_RATE "baud_rate" ///< Baud rate option (serial buses). #define OPT_INFO_LEVEL "info_level" ///< Logging level option. #define OPT_INFO_GROUPS "info_groups" ///< Information groups option. #define OPT_TOPIC_JOINT "topic_joint" ///< Joint topic option. @@ -104,6 +105,7 @@ class CommandLineArgs { std::string topic_joystick; ///< Joystick topic name. std::string info_groups; ///< Information groups (comma-separated). std::string control_port_name; ///< Control port name. + int baud_rate; ///< Baud rate for serial buses (bps). int info_level; ///< Logging level. int dof_arm; ///< Arm DOF. int servo_num_arm; ///< Arm servo count. diff --git a/native/pi_control/include/pi_control.hpp b/native/pi_control/include/pi_control.hpp index 59edfff..0fd953d 100644 --- a/native/pi_control/include/pi_control.hpp +++ b/native/pi_control/include/pi_control.hpp @@ -4,6 +4,7 @@ */ #pragma once +#define DEFAULT_BAUD_RATE 3000000 ///< Default baud rate (bits per second). #define DEFAULT_DOF_ARM 6 ///< Default arm DOF. #define DEFAULT_SERVO_NUM_ARM DEFAULT_DOF_ARM ///< Default number of servos in arm. #define DEFAULT_DOF_EFFECTOR 1 ///< Default effector DOF. @@ -104,6 +105,13 @@ // to the policy but never trip the 10 s dead detector. #define ARX_STALL_WARN_AGE_MS 250 ///< Frame age (ms) that triggers the warn-only stall log. +// Whole-arm controller (DriverController) stall watchdog. Vendor controller +// stacks (Trossen iNerve etc.) keep streaming the last command from a driver- +// internal daemon thread even when the host control loop hangs, so a separate +// watchdog thread idles the arm when the loop stops calling group read/write. +#define CONTROLLER_STALL_WATCHDOG_TIMEOUT_MS 1000 ///< Driver-interaction silence (ms) before the arm is idled. +#define CONTROLLER_STALL_WATCHDOG_PERIOD_MS 100 ///< Watchdog polling period (ms). + // ARX read-only joint encoder (DriverArxEncoder) CAN feedback decoding. // Per the ARX encoder CAN protocol: each joint encoder broadcasts a fixed 2-byte // mechanical angle at 200 Hz. raw = (data[0] << 8) | data[1], covering 0..16384 @@ -122,13 +130,6 @@ #define ARX_ENCODER_WARMUP_MAX_RETRY 50 ///< Max cache-freshness polls during encoder enable. #define ARX_ENCODER_WARMUP_SLEEP_US 1000 ///< Microseconds between encoder warmup polls (1 ms). -// Whole-arm controller (DriverController) stall watchdog. Vendor controller -// stacks (Trossen iNerve etc.) keep streaming the last command from a driver- -// internal daemon thread even when the host control loop hangs, so a separate -// watchdog thread idles the arm when the loop stops calling group read/write. -#define CONTROLLER_STALL_WATCHDOG_TIMEOUT_MS 1000 ///< Driver-interaction silence (ms) before the arm is idled. -#define CONTROLLER_STALL_WATCHDOG_PERIOD_MS 100 ///< Watchdog polling period (ms). - /*! * @enum Role * @brief Device roles in teleoperation. diff --git a/native/pi_control/include/pi_device_arm_nello.hpp b/native/pi_control/include/pi_device_arm_nello.hpp new file mode 100644 index 0000000..f5f5b28 --- /dev/null +++ b/native/pi_control/include/pi_device_arm_nello.hpp @@ -0,0 +1,42 @@ +/*! + * @file pi_device_arm_nello.hpp + * @brief Defines the DeviceArmNello class for Nello robotic arm device. + */ +#pragma once +#include "pi_device_arm.hpp" + +/*! + * @class DeviceArmNello + * @brief Nello robotic arm device implementation. + */ +class DeviceArmNello : public DeviceArm { + public: + /*! + * @brief Constructs a new DeviceArmNello instance. + * + * @param cla Command-line arguments containing device configuration parameters. + */ + DeviceArmNello(const CommandLineArgs& cla); + + // Destroys the DeviceArmNello instance. + ~DeviceArmNello(); + + /*! + * @brief Moves the arm to the ready position using Nello-specific movement sequence. + * + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode move_to_ready_position() override; + + /*! + * @brief Sets control mode for Nello arm. + * + * Nello: leader and follower require different servo operation modes. + * - NORMAL_OPERATION: follow the target_role policy. + * - READY_MOVE_OVERRIDE: force a safe position-based mode so the arm can move to home from current pose. + */ + ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; + + private: +}; + diff --git a/native/pi_control/include/pi_device_config.hpp b/native/pi_control/include/pi_device_config.hpp index b11b70a..94a56de 100644 --- a/native/pi_control/include/pi_device_config.hpp +++ b/native/pi_control/include/pi_device_config.hpp @@ -44,10 +44,12 @@ class DeviceConfig { const std::string fn_arm_type = "arm_type"; ///< Field name for arm type. const std::string val_arm_type_arx = "arx"; ///< Value for arm type arx. const std::string val_arm_type_controller = "controller"; ///< Value for arms managed by a whole-arm controller (DriverController). + const std::string val_arm_type_nello = "nello"; ///< Value for arm type nello (serial bus-servo arms, e.g. SO-ARM101). const std::string fn_effector_type = "effector_type"; ///< Field name for effector type. const std::string val_effector_type_arx = "arx"; ///< Value for effector type arx. const std::string val_effector_type_controller = "controller"; ///< Value for effectors managed by a whole-arm controller (DriverController). + const std::string val_effector_type_nello = "nello"; ///< Value for effector type nello (serial bus-servo grippers, e.g. SO-ARM101). const std::string val_effector_type_none = "None"; ///< Value for effector type none. const std::string fn_effector_control_mode = "control_mode"; ///< Field name for effector control mode. const std::string val_effector_control_mode_torque = "torque"; ///< Value for effector control mode torque. @@ -64,6 +66,7 @@ class DeviceConfig { const std::string val_driver_type_can = "CAN"; ///< Value for driver type CAN. const std::string val_driver_type_can_encoder = "CAN_ENCODER"; ///< Value for driver type CAN read-only encoder (DriverArxEncoder). const std::string val_driver_type_trossen = "TROSSEN_ETHERNET"; ///< Value for driver type Trossen iNerve controller over Ethernet (DriverTrossen). + const std::string val_driver_type_ft = "FEETECH"; ///< Value for driver type FeeTech SMS/STS serial bus (DriverFt). const std::string fn_controller_model = "controller_model"; ///< Field name for the vendor controller model string (e.g. "wxai_v0"). const std::string fn_algo_type = "algo_type"; ///< Field name for algorithm type. @@ -116,6 +119,7 @@ class DeviceConfig { const std::string val_servo_model_can_passive_encoder = "CAN Passive Encoder"; ///< YAM teaching-handle trigger encoder (CAN request/response poll, read-only). const std::string val_servo_model_arx_encoder = "ARX Remote Encoder"; ///< ARX read-only joint encoder (CAN, 2-byte angle, no actuation). const std::string val_servo_model_trossen_wxai = "Trossen WXAI Joint"; ///< One joint of a Trossen WidowX AI arm managed by the iNerve controller (Ethernet). + const std::string val_servo_model_ft_sts3215 = "FeeTech STS3215"; ///< FeeTech STS3215 (SO-ARM100/101, Serial); also covers Hiwonder HX-30HM/HX-10HM (identical SMS/STS protocol). const std::string fn_servo_id = "servo_id"; ///< Field name for servo ID. const std::string fn_servo_data_index = "data_index"; ///< Field name for servo data index. const std::string fn_servo_pos_min = "pos_min"; ///< Field name for servo position minimum (relative radian). @@ -137,6 +141,7 @@ class DeviceConfig { const std::string fn_servo_ka = "kA"; ///< Field name for servo current value to mA conversion constant. const std::string fn_servo_kv = "kV"; ///< Field name for servo velocity value to rpm conversion constant. const std::string fn_servo_resolution = "servo_resolution"; ///< Field name for servo resolution. + const std::string fn_servo_prof_accel = "prof_accel"; ///< Field name for acceleration for servo profile control. const std::string fn_servo_dir_invert = "dir_invert"; ///< Field name for servo direction invert: inverted = -1, not inverted = 1. const std::string fn_servo_zero_pos = "zero_pos"; ///< Field name for servo zero position (absolute radian). const std::string fn_servo_position_wrap_period = "position_wrap_period"; ///< Optional single-turn feedback wrap period (relative radian). diff --git a/native/pi_control/include/pi_device_effector_nello.hpp b/native/pi_control/include/pi_device_effector_nello.hpp new file mode 100644 index 0000000..aeffbf0 --- /dev/null +++ b/native/pi_control/include/pi_device_effector_nello.hpp @@ -0,0 +1,76 @@ +/*! + * @file pi_device_effector_nello.hpp + * @brief Nello effector device implementation. + */ +#pragma once +#include "pi_device_effector.hpp" + +/*! + * @brief Nello effector device implementation. + */ +class DeviceEffectorNello : public DeviceEffector { + public: + /*! + * @brief Constructor. + * @param cla Command-line arguments. + */ + DeviceEffectorNello(const CommandLineArgs& cla); + + /*! + * @brief Destructor. + */ + ~DeviceEffectorNello(); + + // + // Override functions + // + + /*! + * @brief Initializes the Nello effector device. + * @param cla Command-line arguments. + * @param argc Argument count. + * @param argv Argument values. + * @param p_topic Topic instance for communication. + * @param p_driver Driver instance for hardware communication. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode init(const CommandLineArgs& cla, int argc, char** argv, std::shared_ptr p_topic, + std::shared_ptr p_driver) override; + + /*! + * @brief Moves the effector to the ready position. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode move_to_ready_position() override; + + /*! + * @brief Moves a joint using torque control. + * @param p_joint Pointer to the joint. + * @param target_pos Target position (relative radians). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode move_joint_with_torque(Joint* p_joint, float target_pos) override; + + /*! + * @brief Sets control mode for Nello effector. + * + * Nello: leader and follower require different servo operation modes, and follower behavior also depends on + * effector control type (torque vs position). READY_MOVE_OVERRIDE forces a safe position-based mode via the + * base-class override flag. + */ + ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; + + /*! + * @brief Resets ready state so a commanded move-to-ready re-engages torque once. + */ + void reset_ready_state_for_move_to_ready() override { + DeviceEffector::reset_ready_state_for_move_to_ready(); + ready_move_torque_engaged_ = false; + } + + private: + /// True once the ready move has engaged servo torque; prevents per-cycle + /// enable(true) re-sends that race the leader-passive torque disable. + bool ready_move_torque_engaged_ = false; +}; + diff --git a/native/pi_control/include/pi_driver_ft.hpp b/native/pi_control/include/pi_driver_ft.hpp new file mode 100644 index 0000000..65a970a --- /dev/null +++ b/native/pi_control/include/pi_driver_ft.hpp @@ -0,0 +1,312 @@ +/*! + * @file pi_driver_ft.hpp + * @brief DriverFt -- FeeTech SMS/STS serial bus servo driver (STS3215 class). + * + * Speaks the FeeTech UART protocol (0xFF 0xFF header, complement checksum, + * INST_READ/WRITE plus SYNC READ 0x82 / SYNC WRITE 0x83 bulk transfers) on a + * plain USB serial port. Mirrors the DriverDxl structure: cached bulk feedback + * per control cycle, queued group writes flushed once per cycle, and sticky + * dead-servo tracking for daisy-chain cable breaks. + * + * Covers the SO-ARM100/101 arms (FeeTech STS3215; Hiwonder HX-30HM / HX-10HM + * are register-compatible and use the same model string). SYNC READ requires + * STS3215 firmware >= 3.9 class; open() verifies it once and fast-fails when + * the bus does not answer a bulk read (no sequential-read fallback). + * + * The Python mirror of this protocol lives in pi_control/servos/ft_serial.py. + */ + +#pragma once +#include +#include +#include + +#include "pi_driver_serial.hpp" + +class ServoFt; + +/*! + * @brief FeeTech SMS/STS serial bus servo driver. + */ +class DriverFt : public DriverSerial { + public: + /*! + * @brief Constructor. + * @param p_device Pointer to the Device instance. + * @param cla Command-line arguments. + */ + DriverFt(Device* p_device, const CommandLineArgs& cla); + + /*! + * @brief Destructor. + */ + ~DriverFt() override; + + /*! + * @brief Opens the serial port, sizes the feedback caches, and verifies + * SYNC READ support with one bulk read (fast-fail when unsupported). + * @param baud_rate Baud rate for serial communication (SO-ARM default 1000000). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode open(int baud_rate) override; + + /*! + * @brief Closes the serial port. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode close() override; + + /*! + * @brief Copies the cached bulk-read feedback of one servo into its Servo object. + * @param p_servo Pointer to the Servo instance (must be a ServoFt). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode read_hardware_values(Servo* p_servo) override; + + /*! + * @brief Reads feedback from all servos with one SYNC READ transaction. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode group_read_hardware_values() override; + + /*! + * @brief Flushes the queued goal position / torque limit SYNC WRITE packets. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode group_write_hardware_values() override; + + /*! + * @brief Returns the lowest dead servo id (daisy-chain break point), or -1. + */ + int last_failed_servo_id() const override { return last_failed_servo_id_; } + + /*! + * @brief Returns the sticky set of servo ids classified dead on the bus. + */ + std::set dead_servo_ids() const override { return dead_servo_ids_; } + + /*! + * @brief Pings a servo (INST_PING) and validates the status packet. + * @param id Servo ID. + * @return ReturnCode::SUCCESS when the servo answered, otherwise an error code. + */ + ReturnCode ping(int id); + + /*! + * @brief Reads a 1/2-byte register from a servo (INST_READ, little-endian). + * @param id Servo ID. + * @param address Register address. + * @param length Register length in bytes (1 or 2). + * @param value Output register value (unsigned raw). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode read_register(int id, uint8_t address, uint8_t length, int32_t& value); + + /*! + * @brief Writes a 1/2-byte register on a servo (INST_WRITE, little-endian). + * + * EEPROM registers (address below the SRAM boundary) are automatically + * wrapped with a Lock Flag unlock/re-lock pair, which the STS3215 firmware + * requires for the write to take effect. + * @param id Servo ID. + * @param address Register address. + * @param value Register value (unsigned raw; pre-encode sign-magnitude values). + * @param length Register length in bytes (1 or 2). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode write_register(int id, uint8_t address, int32_t value, uint8_t length); + + /*! + * @brief Enables or disables torque output (Torque Enable register). + * @param p_servo Pointer to the ServoFt instance. + * @param enable True to enable, false to disable. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode enable_torque(ServoFt* p_servo, bool enable); + + /*! + * @brief Sets the operating mode (0 position / 1 velocity / 2 PWM / 3 step). + * @param p_servo Pointer to the ServoFt instance. + * @param mode Operating mode value. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_operating_mode(ServoFt* p_servo, uint8_t mode); + + /*! + * @brief Writes the position loop P/I/D gains (one byte each, EEPROM). + * @param p_servo Pointer to the ServoFt instance. + * @param pos_p Position P gain (0-254). + * @param pos_i Position I gain (0-254). + * @param pos_d Position D gain (0-254). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_position_pid(ServoFt* p_servo, float pos_p, float pos_i, float pos_d); + + /*! + * @brief Writes the Acceleration register (unit: 100 steps/s^2, 0 disables the ramp). + * @param p_servo Pointer to the ServoFt instance. + * @param acceleration Acceleration register value (0-254). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_acceleration(ServoFt* p_servo, uint8_t acceleration); + + /*! + * @brief Writes the goal position of one servo directly (single WRITE). + * @param p_servo Pointer to the ServoFt instance. + * @param goal_position Goal position in encoder steps (0-4095). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_goal_position_direct(ServoFt* p_servo, int32_t goal_position); + + /*! + * @brief Queues a goal position for the per-cycle SYNC WRITE flush. + * @param p_servo Pointer to the ServoFt instance. + * @param goal_position Goal position in encoder steps (0-4095). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_goal_position_group(ServoFt* p_servo, int32_t goal_position); + + /*! + * @brief Queues a torque limit (0-1000, 0.1% of max torque) for the SYNC WRITE flush. + * @param p_servo Pointer to the ServoFt instance. + * @param torque_limit Torque Limit register value (0-1000). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_torque_limit_group(ServoFt* p_servo, int32_t torque_limit); + + /*! + * @brief Discards one servo's queued goal position / torque limit group-write entries. + * + * Used when switching that servo to the passive leader mode: goal entries queued + * earlier in the same control cycle (the final move-to-ready step) must not be + * flushed after its torque is disabled, because a goal position command can + * re-engage torque (Hiwonder HX firmware on SO-ARM101 kits). + * @param p_servo Pointer to the ServoFt instance whose entries are dropped. + */ + void discard_pending_group_writes(ServoFt* p_servo); + + /*! + * @brief Reads the Torque Enable register of one servo. + * @param p_servo Pointer to the ServoFt instance. + * @param enabled Output raw register value (0 = torque off, 1 = torque on). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode get_torque_enabled(ServoFt* p_servo, int32_t& enabled); + + /*! + * @brief Reads the present position of one servo (single READ). + * @param p_servo Pointer to the ServoFt instance. + * @param present_position Output present position in encoder steps. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode get_present_position(ServoFt* p_servo, int32_t& present_position); + + /*! + * @brief Reads the present position and writes it as the goal position. + * + * Called before enabling torque so the servo holds its current pose instead + * of jumping to a stale goal register (same rationale as DriverDxl). + * @param p_servo Pointer to the ServoFt instance. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode sync_goal_position_to_present(ServoFt* p_servo); + + /*! + * @brief Decodes a FeeTech sign-magnitude value (dedicated sign bit). + * @param value Raw register value. + * @param sign_bit Index of the sign bit (velocity: 15, load: 10). + * @return Signed value. + */ + static int32_t decode_sign_magnitude(uint32_t value, int sign_bit); + + /*! + * @brief Encodes a signed value into FeeTech sign-magnitude representation. + * @param value Signed value. + * @param sign_bit Index of the sign bit. + * @return Raw register value. + */ + static uint32_t encode_sign_magnitude(int32_t value, int sign_bit); + + private: + /*! + * @brief Builds a FeeTech instruction packet and sends it. + * @param id Servo ID (or the broadcast ID). + * @param instruction Instruction byte. + * @param p_params Instruction parameter bytes (may be nullptr when length is 0). + * @param param_length Number of parameter bytes. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode send_instruction(uint8_t id, uint8_t instruction, const uint8_t* p_params, size_t param_length); + + /*! + * @brief Reads one status packet (FF FF ID LEN ERR PARAMS.. CHK) with a deadline. + * @param expected_id Servo ID the status packet must carry; pass the broadcast ID (0xFE) to accept any servo + * (used for SYNC READ responses, which are matched by the ID they carry). + * @param p_params Output buffer for the status parameters (may be nullptr when param_length is 0). + * @param param_length Expected number of status parameter bytes. + * @param timeout_ms Deadline for the complete packet. + * @param p_packet_id Optional output for the ID the packet actually carried (may be nullptr). + * @return ReturnCode::SUCCESS if a valid packet arrived, otherwise an error code. + */ + ReturnCode receive_status(uint8_t expected_id, uint8_t* p_params, size_t param_length, int timeout_ms, + int* p_packet_id); + + /*! + * @brief Sends an instruction and reads the matching status packet, with retries. + * @param id Servo ID. + * @param instruction Instruction byte. + * @param p_params Instruction parameter bytes. + * @param param_length Number of instruction parameter bytes. + * @param p_rsp_params Output buffer for status parameters. + * @param rsp_param_length Expected number of status parameter bytes. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode txrx_instruction(uint8_t id, uint8_t instruction, const uint8_t* p_params, size_t param_length, + uint8_t* p_rsp_params, size_t rsp_param_length); + + /*! + * @brief Writes a register without the EEPROM unlock wrapper (used by the wrapper itself). + */ + ReturnCode write_register_raw(int id, uint8_t address, int32_t value, uint8_t length); + + /*! + * @brief Reads exactly ``length`` bytes from the port before ``deadline_ms`` elapses. + * @return Number of bytes actually read (may be short on timeout). + */ + size_t read_exact(uint8_t* p_buf, size_t length, int deadline_ms); + + /*! + * @brief Discards any stale bytes pending in the receive buffer. + */ + void flush_input(); + + /*! + * @brief Sends one SYNC WRITE packet for a fixed register window. + * @param address Register start address. + * @param data_length Bytes per servo. + * @param entries (servo id, raw value) pairs; values are little-endian encoded. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode sync_write(uint8_t address, uint8_t data_length, const std::vector>& entries); + + /*! + * @brief Probes unclassified servos with ping() after a bulk-read failure + * and moves the silent ones into dead_servo_ids_ (sticky). + */ + void classify_dead_servos(); + + int servo_num_total_ = 0; ///< Total number of servos registered on the bus. + std::vector sync_read_ids_; ///< Alive servo ids included in the SYNC READ transaction. + std::vector all_servo_ids_; ///< All servo ids as reported by the device at open(). + std::set dead_servo_ids_; ///< Sticky set of servos classified dead (cable break). + int last_failed_servo_id_ = -1; ///< Lowest dead servo id, or -1 when all alive. + + std::vector pres_pos_; ///< Cached present position (raw steps, unsigned single-turn). + std::vector pres_vel_; ///< Cached present velocity (steps/s, sign-magnitude decoded). + std::vector pres_load_; ///< Cached present load (0.1% units, sign-magnitude decoded). + std::vector pres_temp_; ///< Cached present temperature (degrees Celsius). + std::vector pres_status_; ///< Cached Servo Status error bits. + + std::vector> pending_goal_position_; ///< Queued (id, steps) for the goal position SYNC WRITE. + std::vector> pending_torque_limit_; ///< Queued (id, 0.1% units) for the torque limit SYNC WRITE. +}; diff --git a/native/pi_control/include/pi_driver_serial.hpp b/native/pi_control/include/pi_driver_serial.hpp new file mode 100644 index 0000000..5953142 --- /dev/null +++ b/native/pi_control/include/pi_driver_serial.hpp @@ -0,0 +1,89 @@ +/*! + * @file pi_driver_serial.hpp + * @brief DriverSerial -- minimal raw POSIX serial (termios) wrapper. + * + * Designed for protocols that ride a plain USB-CDC `/dev/ttyACM*` port and do + * their own framing. This driver knows nothing about a specific servo/motor + * wire format -- it only owns the file descriptor, sets 8N1 with the + * requested baud rate, and gives the device class raw byte access plus an + * optional asynchronous reception thread. Protocol drivers (e.g. + * :class:`DriverFt` for FeeTech SMS/STS, a future Dynamixel driver) derive + * from it and implement their packet layer on top. + */ + +#pragma once +#include + +#include +#include +#include +#include +#include + +#include "pi_driver.hpp" + +/*! + * @brief Minimal raw serial driver (no framing, no protocol). + */ +class DriverSerial : public Driver { + public: + /*! + * @brief Callback type for received serial bytes. + * @param p_data_buf Pointer to the receive buffer. + * @param read_bytes Number of bytes actually read. + */ + typedef std::function callback_t; + + DriverSerial(Device* p_device, const CommandLineArgs& cla); + ~DriverSerial() override; + + /*! + * @brief Open the serial port at ``baud_rate`` (8N1, no flow control). + * + * The port path comes from the base class' ``control_port_name_`` (set + * by the ``Driver`` constructor from ``cla.control_port_name``). + */ + ReturnCode open(int baud_rate) override; + ReturnCode close() override; + + /*! + * @brief Start a background thread that drains the serial port and + * invokes ``callback`` for every chunk of bytes read. + * + * Idempotent: stops a previous reception loop first if one is running. + */ + ReturnCode start_reception(const callback_t& callback); + + /*! + * @brief Stop the reception thread (no-op if not running). + */ + ReturnCode stop_reception(); + + /*! + * @brief Write raw bytes to the serial port. Blocking ``::write`` semantics. + */ + ReturnCode write_bytes(const uint8_t* p_data, size_t size); + + /*! + * @brief Convenience overload: write a NUL-terminated text command. + */ + ReturnCode write_text(const char* text); + + /*! + * @brief Whether ``open()`` succeeded and we still own a valid fd. + */ + bool is_open() const { return port_handler_ >= 0; } + + protected: + void receive_loop(callback_t callback); + + /*! + * @brief Map a baud-rate integer to the matching termios ``B*`` constant. + * @return The mapped speed_t, or 0 if ``baud_rate`` is unsupported. + */ + static speed_t baud_constant(int baud_rate); + + int port_handler_ = -1; ///< POSIX file descriptor (-1 when closed). + std::atomic is_running_{false}; + std::thread reception_thread_; +}; diff --git a/native/pi_control/include/pi_servo.hpp b/native/pi_control/include/pi_servo.hpp index 0e2febd..baf87bd 100644 --- a/native/pi_control/include/pi_servo.hpp +++ b/native/pi_control/include/pi_servo.hpp @@ -26,6 +26,8 @@ enum class ServoType { DM_4340 = 151, ///< Dinamo DM J4340 servo (CAN protocol). DM_4310 = 152, ///< Dinamo DM J4310 servo (CAN protocol). + FT_STS3215 = 301, ///< FeeTech STS3215 servo (SMS/STS serial protocol; also covers Hiwonder HX-30HM/HX-10HM). + CAN_PASSIVE_ENCODER = 701, ///< Passive CAN trigger encoder (YAM teaching handle): request/response poll, read-only, never commanded. CONTROLLER_JOINT = 801 ///< One joint of a whole-arm controller (Trossen iNerve etc.; no direct bus access). @@ -207,6 +209,22 @@ class Servo { */ virtual ReturnCode apply_torque(float torque) = 0; + /*! + * @brief Enables or disables torque output of the servo. + * + * Virtual so device code (e.g. DeviceEffectorNello) can toggle torque on + * any bus-servo family without casting to a concrete type. The default + * implementation fast-fails for servo families without a torque enable + * register. + * @param enable True to enable torque output, false to disable it. + * @return ReturnCode indicating success or failure. + */ + virtual ReturnCode enable_torque(bool enable) { + (void)enable; + PI_ERROR("enable_torque is not supported for servo ID %d (model %s)", id_, servo_model_.c_str()); + return ReturnCode::NOT_SUPPORTED; + } + /*! * @brief Applies torque while retaining the servo's configured derivative damping. * diff --git a/native/pi_control/include/pi_servo_ft.hpp b/native/pi_control/include/pi_servo_ft.hpp new file mode 100644 index 0000000..72371a6 --- /dev/null +++ b/native/pi_control/include/pi_servo_ft.hpp @@ -0,0 +1,183 @@ +/*! + * @file pi_servo_ft.hpp + * @brief ServoFt class for managing FeeTech SMS/STS serial bus servos (STS3215 class). + * + * Covers the SO-ARM100/101 servos: FeeTech STS3215 and the register-compatible + * Hiwonder HX-30HM / HX-10HM (SO-ARM101 kits), which all share the canonical + * "FeeTech STS3215" servo model string. Single-turn position control with a + * hardware Torque Limit register; there is no current control loop, so + * apply_torque() fast-fails and torque-mode effectors approximate torque + * limiting via move(pos, vel, tor). + */ + +#pragma once +#include + +#include "pi_driver_ft.hpp" +#include "pi_servo.hpp" + +/*! + * @brief Parameter container class for FeeTech servo configuration. + */ +class ServoFtParam : public ServoParam { + public: + /*! + * @brief Constructor. + * @param tolerable_pos_difference_rad Tolerable position difference threshold in radians. + * @param max_pos_difference_rad Maximum position difference threshold in radians. + * @param velocity_threshold_rad_sec Velocity threshold in rad/sec. + */ + ServoFtParam(float tolerable_pos_difference_rad, float max_pos_difference_rad, float velocity_threshold_rad_sec) + : ServoParam(tolerable_pos_difference_rad, max_pos_difference_rad, velocity_threshold_rad_sec) {} +}; + +/*! + * @brief Manages FeeTech SMS/STS-type servo motors controlled via serial interface. + */ +class ServoFt : public Servo { + public: + /*! + * @brief Constructor. + * @param p_device Pointer to the Device object. + * @param p_joint Pointer to the Joint object. + * @param p_driver Pointer to the Driver object (must be a DriverFt). + */ + ServoFt(Device* p_device, Joint* p_joint, Driver* p_driver); + + /*! + * @brief Destructor. + */ + ~ServoFt(); + + /*! + * @brief Safely parks the servo before shutdown (torque off, goal aligned to present). + * @return ReturnCode indicating success or failure. + */ + ReturnCode park_safely() override; + + /*! + * @brief Initializes the servo with model configuration. + * @param servo_config JSON object containing the servo model configuration. + * @param p_config Pointer to the device model configuration object. + * @return ReturnCode indicating success or failure. + */ + ReturnCode init_config_model(const json& servo_config, const DeviceConfig* p_config) override; + + /*! + * @brief Starts the servo hardware: position mode, PID gains, acceleration, + * goal-to-present sync, then torque on. + * @return ReturnCode indicating success or failure. + */ + ReturnCode start_hardware() override; + + /*! + * @brief Gets the servo's current position as raw encoder steps. + * @return Current position in encoder steps. + */ + float get_pos_servo() override { return (float)curr_pos_steps_; } + + /*! + * @brief Moves the servo to the target position. + * @param target_pos Target position in relative radians. + * @return ReturnCode indicating success or failure. + */ + ReturnCode move(float target_pos) override; + + /*! + * @brief Moves the servo to the target position with a torque limit. + * + * The STS3215 has no current loop; target_tor is approximated by writing + * the Torque Limit register (0-1000, 0.1% of stall torque) alongside the + * goal position. target_vel is not consumed (the ready-move stepping and + * the Acceleration register bound the motion). + * @param target_pos Target position in relative radians. + * @param target_vel Target velocity in rad/sec (unused). + * @param target_tor Target torque limit in Nm. + * @return ReturnCode indicating success or failure. + */ + ReturnCode move(float target_pos, float target_vel, float target_tor) override; + + /*! + * @brief Direct torque command -- not supported (no current control loop). + * @param torque Torque in Nm. + * @return ReturnCode::NOT_SUPPORTED always. + */ + ReturnCode apply_torque(float torque) override; + + /*! + * @brief Enables or disables torque output of the servo. + * @param enable True to enable torque output, false to disable it. + * @return ReturnCode indicating success or failure. + */ + ReturnCode enable_torque(bool enable) override; + + /*! + * @brief Changes the servo control mode for leader (teleoperation) operation. + * + * The SO-ARM leader is fully passive (LeRobot convention): torque is simply + * disabled so the operator back-drives the arm. Force feedback requires a + * current loop the STS3215 does not have, so it fast-fails. + * @return ReturnCode indicating success or failure. + */ + ReturnCode change_control_mode_for_leader() override; + + /*! + * @brief Changes the servo control mode for follower operation + * (position mode, goal synced to present, torque on). + * @return ReturnCode indicating success or failure. + */ + ReturnCode change_control_mode_for_follower() override; + + /*! + * @brief Gets the servo's encoder resolution. + * @return Servo resolution (encoder counts per revolution). + */ + int get_servo_resolution() override { return servo_resolution_; } + + /*! + * @brief Converts a raw FeeTech velocity value (steps/s) to rad/sec. + * @param servo_value Raw velocity value (sign-magnitude already decoded). + * @return Velocity in rad/sec. + */ + float get_vel_rad_sec(int32_t servo_value) { return servo_value * kv_; } + + /*! + * @brief Converts a raw FeeTech load value (0.1% units) to Nm. + * @param servo_value Raw load value (sign-magnitude already decoded). + * @return Torque in Nm. + */ + float get_tor_nm(int32_t servo_value) { return servo_value * kt_; } + + /*! + * @brief Converts encoder steps to radians (center step = 0 rad). + * @param steps Position in encoder steps. + * @return Position in radians. + */ + float steps_to_rad(int32_t steps) { + return (steps - center_offset_) * (2.0 * M_PI) / (float)servo_resolution_; + } + + /*! + * @brief Converts radians to encoder steps (0 rad = center step). + * @param rad_value Position in radians. + * @return Position in encoder steps. + */ + int32_t rad_to_steps(float rad_value) { + return (int32_t)(rad_value * ((float)servo_resolution_ / (2.0 * M_PI)) + center_offset_); + } + + /*! + * @brief Converts a torque value in Nm to a Torque Limit register value. + * @param torque Torque in Nm (magnitude is used; the register is unsigned). + * @return Torque Limit register value (0-1000, 0.1% of stall torque). + */ + int32_t torque_to_torque_limit(float torque); + + int32_t curr_pos_steps_ = 0; ///< Current position in encoder steps (unsigned single-turn). + uint32_t servo_resolution_ = 0; ///< Servo encoder resolution (counts per full revolution). + int32_t center_offset_ = 0; ///< Encoder step corresponding to 0 rad (calibrated middle, 2048). + + protected: + DriverFt* p_driver_ft_ = nullptr; ///< Pointer to the FeeTech driver. + int prof_accel_ = 0; ///< Optional Acceleration register value (unit 100 steps/s^2; 0 = ramp off). +}; diff --git a/native/pi_control/src/pi_command_line_args.cpp b/native/pi_control/src/pi_command_line_args.cpp index cfc1627..e6c8508 100644 --- a/native/pi_control/src/pi_command_line_args.cpp +++ b/native/pi_control/src/pi_command_line_args.cpp @@ -45,6 +45,8 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { OPT_INFO_LEVEL, po::value()->default_value(0), "The level of information message to see: lower value shows less " "frequent messages")( + OPT_BAUD_RATE, po::value()->default_value(DEFAULT_BAUD_RATE), + "To set the baud rate of the serial port (bps)")( OPT_DOF_ARM, po::value()->default_value(DEFAULT_DOF_ARM), "To set the DOF of the arm")( OPT_SERVO_NUM_ARM, @@ -308,6 +310,15 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { exit(2); } + if (vm.count(OPT_BAUD_RATE)) { + baud_rate = vm[OPT_BAUD_RATE].as(); + PI_INFO("main()", InfoLevel::ESSENTIAL_0, "Baud rate: %d bps", + baud_rate); + } else { + PI_ERROR("--%s is not set", OPT_BAUD_RATE); + exit(2); + } + if (vm.count(OPT_DOF_ARM)) { dof_arm = vm[OPT_DOF_ARM].as(); PI_INFO("main()", InfoLevel::ESSENTIAL_0, "Arm degrees of freedom: %d", diff --git a/native/pi_control/src/pi_control_node.cpp b/native/pi_control/src/pi_control_node.cpp index 9fee8a3..b876cbb 100644 --- a/native/pi_control/src/pi_control_node.cpp +++ b/native/pi_control/src/pi_control_node.cpp @@ -166,7 +166,7 @@ int main(int argc, char** argv) { PI_INFO("main()", InfoLevel::ESSENTIAL_0, "Starting devices..."); - return_code = p_device->start(0); + return_code = p_device->start(cla.baud_rate); if (return_code != ReturnCode::SUCCESS) { // Communication failures should not keep the process running for // long. diff --git a/native/pi_control/src/pi_device.cpp b/native/pi_control/src/pi_device.cpp index c3278e2..44a60ec 100644 --- a/native/pi_control/src/pi_device.cpp +++ b/native/pi_control/src/pi_device.cpp @@ -9,9 +9,11 @@ #include "pi_device.hpp" #include "pi_device_arm_arx.hpp" +#include "pi_device_arm_nello.hpp" #include "pi_device_config.hpp" #include "pi_device_effector_arx.hpp" #include "pi_device_effector_controller.hpp" +#include "pi_device_effector_nello.hpp" #include "pi_info.hpp" Device::Device(const CommandLineArgs& cla) @@ -585,6 +587,10 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf p_device = new DeviceArm(cla); PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArm (controller) for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); + } else if (arm_type == cfg_model.val_arm_type_nello) { + p_device = new DeviceArmNello(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArmNello for %s_%s", + cla.device_model.c_str(), cla.device_id.c_str()); } else { PI_ERROR("Invalid arm type: %s", arm_type.c_str()); return nullptr; @@ -605,6 +611,10 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf p_device = new DeviceEffectorController(cla); PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorController for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); + } else if (effector_type == cfg_model.val_effector_type_nello) { + p_device = new DeviceEffectorNello(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorNello for %s_%s", + cla.device_model.c_str(), cla.device_id.c_str()); } else { PI_ERROR("Invalid effector type: %s", effector_type.c_str()); return nullptr; diff --git a/native/pi_control/src/pi_device_arm_nello.cpp b/native/pi_control/src/pi_device_arm_nello.cpp new file mode 100644 index 0000000..797a11d --- /dev/null +++ b/native/pi_control/src/pi_device_arm_nello.cpp @@ -0,0 +1,74 @@ +/*! + * @file pi_device_arm_nello.cpp + * @brief Implementation of the DeviceArmNello class for Nello robotic arm device control. + */ + +#include + +#include "pi_device_arm_nello.hpp" +#include "pi_joint.hpp" +#include "pi_profile.hpp" + +DeviceArmNello::DeviceArmNello(const CommandLineArgs& cla) : DeviceArm(cla) {} + +DeviceArmNello::~DeviceArmNello() {} + +ReturnCode DeviceArmNello::set_control_mode(Role target_role, ControlModeIntent intent) { + // Implementation note: + // - Nello leader/follower switching is already expressed via Joint::change_control_mode_for_{leader,follower}(), + // which maps to servo/driver operation modes. + // - For READY_MOVE_OVERRIDE we still treat it as follower-like (position-based) so that move_to_ready_position() + // can safely send position targets from the current pose. + // - After ready completion, normal operation will call this again with intent NORMAL_OPERATION and the actual role. + ReturnCode rc = ReturnCode::SUCCESS; + + const bool use_follower_like = (intent == ControlModeIntent::READY_MOVE_OVERRIDE) || (target_role == Role::FOLLOWER); + if (use_follower_like) { + for (auto& p_joint : joints_) { + rc = p_joint->change_control_mode_for_follower(); + if (rc != ReturnCode::SUCCESS) return rc; + } + return ReturnCode::SUCCESS; + } + + // Leader + prof_time_t current_time = Profile::get_time_now(); + for (auto& p_joint : joints_) { + rc = p_joint->change_control_mode_for_leader(current_time); + if (rc != ReturnCode::SUCCESS) return rc; + } + + // Leader only: chain to the attached effector (same as DeviceArm::set_control_mode). + // The effector's own step switches its mode once at its ready transition, but that single + // attempt can fail -- the passive-leader torque disable races the final ready-move writes + // on SO-ARM101 -- so re-applying here at arm-ready gives it a second, later chance. The + // call is idempotent. The follower-like branch above intentionally does not chain: Nello + // follower effector modes depend on the configured effector control type and are handled + // by the effector's own ready transition, matching the long-standing Nello behavior. + // Skipped during emergency recovery for the same bus-timeout reason as the base class. + if (p_effector_ && !is_in_emergency_recovery()) { + rc = p_effector_->set_control_mode(p_effector_->get_device_role(), intent); + if (rc != ReturnCode::SUCCESS) return rc; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceArmNello::move_to_ready_position() { + ReturnCode return_code = ReturnCode::SUCCESS; + + return_code = DeviceArm::move_to_ready_position(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to move Nello arm to ready position"); + return return_code; + } + + if (is_ready_ == true) { + // Control-mode restoration is handled by the generic move-to-ready command state machine + // (Device::step()) via set_control_mode(...). Keep move_to_ready_position() focused on motion. + + PI_INFO("DeviceArm", InfoLevel::DETAIL_2, "Ready flag set: %s_%s", model_.c_str(), id_.c_str()); + } + + return return_code; +} diff --git a/native/pi_control/src/pi_device_effector_nello.cpp b/native/pi_control/src/pi_device_effector_nello.cpp new file mode 100644 index 0000000..6e34eb5 --- /dev/null +++ b/native/pi_control/src/pi_device_effector_nello.cpp @@ -0,0 +1,160 @@ +/*! + * @file pi_device_effector_nello.cpp + * @brief Implementation of the DeviceEffectorNello class for Nello effector device control. + */ + + #include + +#include "pi_device_effector_nello.hpp" +#include "pi_joint.hpp" +#include "pi_servo.hpp" + +DeviceEffectorNello::DeviceEffectorNello(const CommandLineArgs& cla) : DeviceEffector(cla) {} + +DeviceEffectorNello::~DeviceEffectorNello() {} + +ReturnCode DeviceEffectorNello::set_control_mode(Role target_role, ControlModeIntent intent) { + ReturnCode rc = ReturnCode::SUCCESS; + + // READY_MOVE_OVERRIDE forces a safe position-based behavior regardless of configured effector control type. + // See DeviceEffector::get_effective_control_mode() docs for why we use an override flag instead of mutating + // the persistent control_mode_ (avoid save/restore and make restoration deterministic). + if (intent == ControlModeIntent::READY_MOVE_OVERRIDE) { + set_ready_move_force_position_mode(true); + } else { + set_ready_move_force_position_mode(false); + } + + const bool use_follower_like = (intent == ControlModeIntent::READY_MOVE_OVERRIDE) || (target_role == Role::FOLLOWER); + + if (use_follower_like) { + for (auto& p_joint : joints_) { + rc = p_joint->change_control_mode_for_follower(); + if (rc != ReturnCode::SUCCESS) return rc; + } + return ReturnCode::SUCCESS; + } + + // Leader + prof_time_t current_time = Profile::get_time_now(); + for (auto& p_joint : joints_) { + rc = p_joint->change_control_mode_for_leader(current_time); + if (rc != ReturnCode::SUCCESS) return rc; + } + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceEffectorNello::init(const CommandLineArgs& cla, int argc, char** argv, std::shared_ptr p_topic, + std::shared_ptr p_driver) { + ReturnCode return_code = DeviceEffector::init(cla, argc, argv, p_topic, p_driver); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Base effector initialization failed"); + return return_code; + } + + ///< @note JSON configuration assumes effector servos follow arm servos in the data array. When no arm is + ///< attached, effector servo data indices are adjusted to start from 0. + if (p_arm_ == nullptr) { + int servo_data_index = 0; + for (auto& p_joint : joints_) { + for (auto& p_servo : p_joint->get_servos()) { + // data_index_ is a base Servo member; no family-specific cast + // is needed (works for Dynamixel and FeeTech servos alike). + p_servo->data_index_ = servo_data_index; + servo_data_index++; + } + } + } + + return return_code; +} + +ReturnCode DeviceEffectorNello::move_to_ready_position() { + ReturnCode return_code = ReturnCode::SUCCESS; + + if (is_read_only() == true) { + // Read-only Nello effector (e.g. T6_2.0A leader-side or T6_2.0A as a follower + // gripper that reports position but isn't actuated): mirror the base-class + // short-circuit so the device's is_ready_ flag actually flips. Without this, + // pi_control_node never publishes DEVICE_INFO_READY_NOW and the UI hangs in + // "waiting for ready" forever. + is_ready_ = true; + PI_INFO("DeviceEffector", InfoLevel::ESSENTIAL_0, + "%s_%s is read only, skipping move to ready position", + model_.c_str(), id_.c_str()); + return ReturnCode::SUCCESS; + } + + int i = 0; + for (auto& p_joint : joints_) { + tele_pos_[i] = p_joint->get_pos_rad_relative(); + + // Engage torque once per ready move, not on every control cycle. Re-sending + // enable(true) on the cycle where the effector reaches ready races the + // leader-passive torque disable that follows in the same cycle: SO-ARM101 + // servos (Hiwonder HX firmware) ignore a torque-off arriving right after a + // torque-on, leaving the leader gripper locked. + if (ready_move_torque_engaged_ == false) { + for (auto& p_servo : p_joint->get_servos()) { + // enable_torque() is virtual on the Servo base class, so this path + // works for every bus-servo family (Dynamixel, FeeTech). + p_servo->enable_torque(true); + usleep(100); + } + } + + i++; + } + ready_move_torque_engaged_ = true; + + return_code = DeviceEffector::move_to_ready_position(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to move Nello effector to ready position"); + return return_code; + } + + if (is_ready_ == true) { + // Control-mode restoration is handled by the generic move-to-ready command state machine + // (Device::step()) via set_control_mode(...). Keep move_to_ready_position() focused on motion. + + PI_INFO("DeviceEffector", InfoLevel::DETAIL_2, "Ready flag set: %s_%s", model_.c_str(), id_.c_str()); + } + + return return_code; +} + +ReturnCode DeviceEffectorNello::move_joint_with_torque(Joint* p_joint, float target_pos) { + ReturnCode return_code = ReturnCode::SUCCESS; + + if (is_read_only() == true) { + // If the device is read only, skip the move joint with torque + return ReturnCode::SUCCESS; + } + + ///< @note Torque-based position control requires fast position monitoring (more than 100Hz) for stability + + float clipped_target_pos = + p_joint->clipping(target_pos, p_joint->get_pos_min_relative(), p_joint->get_pos_max_relative()); + float clipped_curr_pos = p_joint->clipping(p_joint->get_pos_rad_relative(), p_joint->get_pos_min_relative(), + p_joint->get_pos_max_relative()); + + float distance = clipped_target_pos - clipped_curr_pos; + + float torq_to_apply = distance_to_torque_ * distance; + + ///< @note The torque is calculated from the parameter joint but applied to all joints + for (auto& p_joint_iter : joints_) { + return_code = p_joint_iter->move(clipped_target_pos, 0, torq_to_apply); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to apply torque to joint %d in %s_%s", p_joint_iter->id_, model_.c_str(), id_.c_str()); + return return_code; + } + } + + PI_INFO("DeviceEffector", InfoLevel::FREQUENT_3, + "Nello effector torque control: target_pos_rel=%.3f, clipped_target_pos=%.3f, curr_pos_rel=%.3f, " + "clipped_curr_pos=%.3f, distance=%.3f, torq_to_apply=%.3f, distance_to_torque_=%.3f", + target_pos, clipped_target_pos, p_joint->get_pos_rad_relative(), clipped_curr_pos, distance, torq_to_apply, + distance_to_torque_); + return return_code; +} diff --git a/native/pi_control/src/pi_driver.cpp b/native/pi_control/src/pi_driver.cpp index ffe88fd..0cdad71 100644 --- a/native/pi_control/src/pi_driver.cpp +++ b/native/pi_control/src/pi_driver.cpp @@ -8,6 +8,7 @@ #include "pi_device_config.hpp" #include "pi_driver_arx.hpp" #include "pi_driver_arx_encoder.hpp" +#include "pi_driver_ft.hpp" #include "pi_driver_trossen.hpp" #include "pi_servo.hpp" @@ -51,8 +52,12 @@ std::shared_ptr Driver::new_driver(Device* p_device, const DeviceConfig* PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created Trossen controller driver (DriverTrossen, model=%s)", controller_model.c_str()); + } else if (driver_type == p_config->val_driver_type_ft) { + p_driver = std::make_shared(p_device, cla); + PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created FeeTech driver (DriverFt)"); + } else { - PI_ERROR("Unsupported driver type: '%s' (supported types: CAN, CAN_ENCODER, TROSSEN_ETHERNET)", + PI_ERROR("Unsupported driver type: '%s' (supported types: CAN, CAN_ENCODER, TROSSEN_ETHERNET, FEETECH)", driver_type.c_str()); return nullptr; } diff --git a/native/pi_control/src/pi_driver_ft.cpp b/native/pi_control/src/pi_driver_ft.cpp new file mode 100644 index 0000000..1ef0cdf --- /dev/null +++ b/native/pi_control/src/pi_driver_ft.cpp @@ -0,0 +1,768 @@ +/*! + * @file pi_driver_ft.cpp + * @brief Implementation of the DriverFt class for FeeTech SMS/STS serial bus servos. + */ + +#include "pi_driver_ft.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include "pi_info.hpp" +#include "pi_servo_ft.hpp" + +namespace { + +// FeeTech SMS/STS wire protocol (mirrors pi_control/servos/ft_serial.py). +constexpr uint8_t kFtHeaderByte = 0xFF; +constexpr uint8_t kFtBroadcastId = 0xFE; + +constexpr uint8_t kFtInstPing = 0x01; +constexpr uint8_t kFtInstRead = 0x02; +constexpr uint8_t kFtInstWrite = 0x03; +constexpr uint8_t kFtInstSyncRead = 0x82; +constexpr uint8_t kFtInstSyncWrite = 0x83; + +// SMS/STS control table (subset used by the C++ control loop). +constexpr uint8_t kFtAddrPositionPGain = 21; +constexpr uint8_t kFtAddrPositionDGain = 22; +constexpr uint8_t kFtAddrPositionIGain = 23; +constexpr uint8_t kFtAddrOperatingMode = 33; +constexpr uint8_t kFtAddrTorqueEnable = 40; +constexpr uint8_t kFtAddrAcceleration = 41; +constexpr uint8_t kFtAddrGoalPosition = 42; +constexpr uint8_t kFtAddrTorqueLimit = 48; +constexpr uint8_t kFtAddrLockFlag = 55; +constexpr uint8_t kFtAddrPresentPosition = 56; + +// Bulk feedback window: Present Position(2) + Velocity(2) + Load(2) + +// Voltage(1) + Temperature(1) + Async Write Flag(1) + Servo Status(1), +// contiguous at addresses 56..65. +constexpr uint8_t kFtFeedbackLength = 10; + +// Registers below this address live in EEPROM on SMS/STS servos and require +// the Lock Flag (addr 55) to be cleared before a write takes effect. +constexpr uint8_t kFtSramStartAddr = 40; + +constexpr uint8_t kFtLockFlagLocked = 1; +constexpr uint8_t kFtLockFlagUnlocked = 0; + +// Sign-magnitude bit positions (STS3215 feedback encoding). +constexpr int kFtVelocitySignBit = 15; +constexpr int kFtLoadSignBit = 10; + +// Per-packet response deadline. At 1 Mbps a 16-byte status packet takes +// ~0.16 ms on the wire; the dominant term is USB latency (up to one 16 ms +// FTDI/CDC latency window), so allow two windows plus margin. +constexpr int kFtResponseTimeoutMs = 40; + +// Retry policy for single-register transactions; mirrors DriverDxl's +// kSingleRegMaxAttempts rationale (transient bus glitches on half-duplex +// serial should not abort a control-mode change). +constexpr int kFtSingleRegMaxAttempts = 3; + +// Instruction packet: FF FF ID LEN INST [params] CHK. +constexpr size_t kFtInstructionOverhead = 6; +constexpr size_t kFtTxBufCapacity = 64; +constexpr size_t kFtMaxInstructionParams = kFtTxBufCapacity - kFtInstructionOverhead; + +// Status packet parameter capacity (largest expected: the feedback window). +constexpr size_t kFtMaxStatusParams = 32; + +constexpr int kFtGoalPositionMin = 0; +constexpr int kFtGoalPositionMax = 4095; +constexpr int kFtTorqueLimitMin = 0; +constexpr int kFtTorqueLimitMax = 1000; + +uint8_t ft_checksum(const uint8_t* p_data, size_t length) { + unsigned int sum = 0; + for (size_t i = 0; i < length; i++) { + sum += p_data[i]; + } + return static_cast(~sum & 0xFF); +} + +} // namespace + +DriverFt::DriverFt(Device* p_device, const CommandLineArgs& cla) : DriverSerial(p_device, cla) { + if (p_device == nullptr) { + PI_ERROR("Device pointer is null in DriverFt constructor"); + } +} + +DriverFt::~DriverFt() {} + +int32_t DriverFt::decode_sign_magnitude(uint32_t value, int sign_bit) { + const uint32_t sign_mask = 1u << sign_bit; + if (value & sign_mask) { + return -static_cast(value & (sign_mask - 1)); + } + return static_cast(value); +} + +uint32_t DriverFt::encode_sign_magnitude(int32_t value, int sign_bit) { + if (value < 0) { + return static_cast(-value) | (1u << sign_bit); + } + return static_cast(value); +} + +ReturnCode DriverFt::open(int baud_rate) { + ReturnCode return_code = DriverSerial::open(baud_rate); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + if (p_device_ == nullptr) { + PI_ERROR("Device pointer is null in DriverFt::open()"); + return ReturnCode::NOT_INITIALIZED; + } + + all_servo_ids_.clear(); + p_device_->get_servo_ids(all_servo_ids_); + sync_read_ids_ = all_servo_ids_; + dead_servo_ids_.clear(); + last_failed_servo_id_ = -1; + + servo_num_total_ = static_cast(all_servo_ids_.size()); + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, "Total number of FeeTech servos: %d", servo_num_total_); + + pres_pos_.assign(servo_num_total_, 0); + pres_vel_.assign(servo_num_total_, 0); + pres_load_.assign(servo_num_total_, 0); + pres_temp_.assign(servo_num_total_, 0); + pres_status_.assign(servo_num_total_, 0); + + pending_goal_position_.clear(); + pending_torque_limit_.clear(); + pending_goal_position_.reserve(servo_num_total_); + pending_torque_limit_.reserve(servo_num_total_); + + if (servo_num_total_ == 0) { + // Devices with no bus servos of their own (mirrors the DriverDxl empty + // id-list tolerance); nothing to verify. + return ReturnCode::SUCCESS; + } + + // Verify SYNC READ support once (requires STS3215 firmware with the 0x82 + // instruction). Older firmware silently ignores it, which would otherwise + // surface as a timeout on every control cycle -- fast-fail here instead. + return_code = group_read_hardware_values(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR( + "FeeTech SYNC READ verification failed on '%s'. Check servo power/cabling and that the " + "servo firmware supports SYNC READ (0x82); there is no sequential-read fallback.", + control_port_name_.c_str()); + return ReturnCode::FAIL; + } + + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, "DriverFt initialization complete: %d servos ready to operate", + servo_num_total_); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::close() { return DriverSerial::close(); } + +void DriverFt::flush_input() { + if (port_handler_ >= 0) { + ::tcflush(port_handler_, TCIFLUSH); + } +} + +size_t DriverFt::read_exact(uint8_t* p_buf, size_t length, int deadline_ms) { + if (port_handler_ < 0 || p_buf == nullptr || length == 0) { + return 0; + } + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(deadline_ms); + size_t received = 0; + while (received < length) { + const ssize_t n = ::read(port_handler_, p_buf + received, length - received); + if (n > 0) { + received += static_cast(n); + continue; + } + if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR) { + PI_ERROR("DriverFt::read_exact: read failed on '%s': %s", control_port_name_.c_str(), + std::strerror(errno)); + return received; + } + + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline) { + return received; + } + const int remain_ms = + static_cast(std::chrono::duration_cast(deadline - now).count()); + struct pollfd pfd {}; + pfd.fd = port_handler_; + pfd.events = POLLIN; + ::poll(&pfd, 1, remain_ms > 0 ? remain_ms : 1); + } + return received; +} + +ReturnCode DriverFt::send_instruction(uint8_t id, uint8_t instruction, const uint8_t* p_params, size_t param_length) { + if (param_length > kFtMaxInstructionParams) { + PI_ERROR("DriverFt::send_instruction: param_length=%zu exceeds limit %zu (servo ID %d, instruction 0x%02X)", + param_length, kFtMaxInstructionParams, id, instruction); + return ReturnCode::INVALID_PARAM; + } + + uint8_t packet[kFtTxBufCapacity]; + packet[0] = kFtHeaderByte; + packet[1] = kFtHeaderByte; + packet[2] = id; + packet[3] = static_cast(param_length + 2); + packet[4] = instruction; + if (param_length > 0) { + std::memcpy(&packet[5], p_params, param_length); + } + // Checksum covers ID..params (bytes 2 .. 4+param_length). + packet[5 + param_length] = ft_checksum(&packet[2], param_length + 3); + + return write_bytes(packet, kFtInstructionOverhead + param_length); +} + +ReturnCode DriverFt::receive_status(uint8_t expected_id, uint8_t* p_params, size_t param_length, int timeout_ms, + int* p_packet_id) { + if (param_length > kFtMaxStatusParams) { + PI_ERROR("DriverFt::receive_status: param_length=%zu exceeds capacity %zu", param_length, kFtMaxStatusParams); + return ReturnCode::INVALID_PARAM; + } + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + auto remaining_ms = [&deadline]() { + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline) { + return 0; + } + return static_cast(std::chrono::duration_cast(deadline - now).count()); + }; + + // Resynchronize on the FF FF header, byte by byte. A servo ID is never + // 0xFF (max 253), so the first non-FF byte after two or more FF bytes is + // the packet ID. + uint8_t packet_id = 0; + int header_ff_count = 0; + while (true) { + uint8_t byte = 0; + const int remain = remaining_ms(); + if (remain <= 0 || read_exact(&byte, 1, remain) != 1) { + PI_ERROR("DriverFt: status packet timeout waiting for servo ID %d", expected_id); + return ReturnCode::NO_RESPONSE; + } + if (byte == kFtHeaderByte) { + header_ff_count++; + continue; + } + if (header_ff_count >= 2) { + packet_id = byte; + break; + } + header_ff_count = 0; + } + + uint8_t length_error[2] = {0, 0}; + if (read_exact(length_error, 2, remaining_ms()) != 2) { + PI_ERROR("DriverFt: status packet truncated after header (servo ID %d)", packet_id); + return ReturnCode::NO_RESPONSE; + } + const uint8_t packet_length = length_error[0]; + const uint8_t error_byte = length_error[1]; + + if (packet_length != param_length + 2) { + PI_ERROR("DriverFt: unexpected status length %d (expected %zu) from servo ID %d", packet_length, + param_length + 2, packet_id); + flush_input(); + return ReturnCode::FAIL; + } + + uint8_t body[kFtMaxStatusParams + 1]; + const size_t body_length = param_length + 1; // params + checksum + if (read_exact(body, body_length, remaining_ms()) != body_length) { + PI_ERROR("DriverFt: status packet truncated in body (servo ID %d)", packet_id); + return ReturnCode::NO_RESPONSE; + } + + // Checksum covers ID, LEN, ERR and params. + unsigned int sum = static_cast(packet_id) + packet_length + error_byte; + for (size_t i = 0; i < param_length; i++) { + sum += body[i]; + } + const uint8_t expected_checksum = static_cast(~sum & 0xFF); + if (body[param_length] != expected_checksum) { + PI_ERROR("DriverFt: status checksum mismatch from servo ID %d (received 0x%02X, expected 0x%02X)", packet_id, + body[param_length], expected_checksum); + flush_input(); + return ReturnCode::FAIL; + } + + if (expected_id != kFtBroadcastId && packet_id != expected_id) { + PI_ERROR("DriverFt: status packet from unexpected servo ID %d (expected %d)", packet_id, expected_id); + flush_input(); + return ReturnCode::FAIL; + } + + if (error_byte != 0) { + // Servo answered but reports fault bits (voltage/angle/overheat/ + // overcurrent/overload). Surface them but treat the transaction as + // completed; latched faults also show up in the Servo Status register. + PI_WARN("DriverFt: servo ID %d status error bits 0x%02X", packet_id, error_byte); + } + + if (param_length > 0 && p_params != nullptr) { + std::memcpy(p_params, body, param_length); + } + if (p_packet_id != nullptr) { + *p_packet_id = packet_id; + } + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::txrx_instruction(uint8_t id, uint8_t instruction, const uint8_t* p_params, size_t param_length, + uint8_t* p_rsp_params, size_t rsp_param_length) { + ReturnCode return_code = ReturnCode::FAIL; + for (int attempt = 1; attempt <= kFtSingleRegMaxAttempts; attempt++) { + // Discard any stale bytes (late response of a previously timed-out + // transaction) so they cannot be misread as this response. + flush_input(); + + return_code = send_instruction(id, instruction, p_params, param_length); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to send instruction 0x%02X to servo ID %d", instruction, id); + return return_code; + } + + return_code = receive_status(id, p_rsp_params, rsp_param_length, kFtResponseTimeoutMs, nullptr); + if (return_code == ReturnCode::SUCCESS) { + return ReturnCode::SUCCESS; + } + + if (attempt < kFtSingleRegMaxAttempts) { + PI_WARN("DriverFt: transaction 0x%02X with servo ID %d failed (attempt %d/%d), retrying", instruction, id, + attempt, kFtSingleRegMaxAttempts); + } + } + return return_code; +} + +ReturnCode DriverFt::ping(int id) { + return txrx_instruction(static_cast(id), kFtInstPing, nullptr, 0, nullptr, 0); +} + +ReturnCode DriverFt::read_register(int id, uint8_t address, uint8_t length, int32_t& value) { + if (length != 1 && length != 2) { + PI_ERROR("DriverFt::read_register: unsupported length %d (servo ID %d, address %d)", length, id, address); + return ReturnCode::INVALID_PARAM; + } + + const uint8_t params[2] = {address, length}; + uint8_t response[2] = {0, 0}; + ReturnCode return_code = txrx_instruction(static_cast(id), kFtInstRead, params, 2, response, length); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to read register %d (servo ID %d)", address, id); + return return_code; + } + + value = (length == 1) ? response[0] : static_cast(response[0] | (response[1] << 8)); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::write_register_raw(int id, uint8_t address, int32_t value, uint8_t length) { + if (length != 1 && length != 2) { + PI_ERROR("DriverFt::write_register: unsupported length %d (servo ID %d, address %d)", length, id, address); + return ReturnCode::INVALID_PARAM; + } + + uint8_t params[3]; + params[0] = address; + params[1] = static_cast(value & 0xFF); + size_t param_length = 2; + if (length == 2) { + params[2] = static_cast((value >> 8) & 0xFF); + param_length = 3; + } + + ReturnCode return_code = txrx_instruction(static_cast(id), kFtInstWrite, params, param_length, nullptr, 0); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to write register %d = %d (servo ID %d)", address, value, id); + } + return return_code; +} + +ReturnCode DriverFt::write_register(int id, uint8_t address, int32_t value, uint8_t length) { + if (address >= kFtSramStartAddr) { + return write_register_raw(id, address, value, length); + } + + // EEPROM register: the STS3215 ships write-locked (Lock Flag = 1) and + // silently ignores EEPROM writes in that state, so wrap the write with an + // unlock/re-lock pair. + ReturnCode return_code = write_register_raw(id, kFtAddrLockFlag, kFtLockFlagUnlocked, 1); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to unlock EEPROM before writing register %d (servo ID %d)", address, id); + return return_code; + } + + const ReturnCode write_result = write_register_raw(id, address, value, length); + + return_code = write_register_raw(id, kFtAddrLockFlag, kFtLockFlagLocked, 1); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to re-lock EEPROM after writing register %d (servo ID %d)", address, id); + return return_code; + } + + return write_result; +} + +ReturnCode DriverFt::sync_write(uint8_t address, uint8_t data_length, + const std::vector>& entries) { + if (entries.empty()) { + return ReturnCode::SUCCESS; + } + + // SYNC WRITE params: ADDR, DATALEN, then (ID, DATA...) per servo. + const size_t param_length = 2 + entries.size() * (1 + data_length); + if (param_length > kFtMaxInstructionParams) { + PI_ERROR("DriverFt::sync_write: packet too large for %zu servos (address %d)", entries.size(), address); + return ReturnCode::INVALID_PARAM; + } + + uint8_t params[kFtMaxInstructionParams]; + params[0] = address; + params[1] = data_length; + size_t offset = 2; + for (const auto& [id, value] : entries) { + params[offset++] = static_cast(id); + params[offset++] = static_cast(value & 0xFF); + if (data_length == 2) { + params[offset++] = static_cast((value >> 8) & 0xFF); + } + } + + // SYNC WRITE is broadcast; servos do not answer. + return send_instruction(kFtBroadcastId, kFtInstSyncWrite, params, param_length); +} + +void DriverFt::classify_dead_servos() { + bool need_probe = false; + for (int id : all_servo_ids_) { + if (dead_servo_ids_.count(id) == 0) { + need_probe = true; + break; + } + } + if (need_probe) { + for (int id : all_servo_ids_) { + if (dead_servo_ids_.count(id) != 0) continue; + if (ping(id) != ReturnCode::SUCCESS) { + flush_input(); + dead_servo_ids_.insert(id); + PI_ERROR("Identified dead FeeTech servo via ping: ID %d", id); + } + } + } + + // Rebuild the SYNC READ id list so subsequent bulk reads only talk to the + // alive servos (keeps their cached pres_* arrays fresh during recovery). + sync_read_ids_.clear(); + for (int id : all_servo_ids_) { + if (dead_servo_ids_.count(id) == 0) { + sync_read_ids_.push_back(id); + } + } + last_failed_servo_id_ = dead_servo_ids_.empty() ? -1 : *dead_servo_ids_.begin(); +} + +ReturnCode DriverFt::group_read_hardware_values() { + // last_failed_servo_id_ / dead_servo_ids_ are sticky across cycles; see + // DriverDxl::group_read_hardware_values() for the rationale. + + if (port_handler_ < 0) { + PI_ERROR("Serial port is not open in group_read_hardware_values()"); + return ReturnCode::NOT_INITIALIZED; + } + + if (all_servo_ids_.empty()) { + return ReturnCode::SUCCESS; + } + + if (sync_read_ids_.empty()) { + // Every servo is classified dead; nothing to read. + return ReturnCode::FAIL; + } + + // Build one SYNC READ over the feedback window for all alive servos. + const size_t param_length = 2 + sync_read_ids_.size(); + if (param_length > kFtMaxInstructionParams) { + PI_ERROR("DriverFt: too many servos (%zu) for one SYNC READ packet", sync_read_ids_.size()); + return ReturnCode::INVALID_PARAM; + } + + uint8_t params[kFtMaxInstructionParams]; + params[0] = kFtAddrPresentPosition; + params[1] = kFtFeedbackLength; + for (size_t i = 0; i < sync_read_ids_.size(); i++) { + params[2 + i] = static_cast(sync_read_ids_[i]); + } + + flush_input(); + ReturnCode return_code = send_instruction(kFtBroadcastId, kFtInstSyncRead, params, param_length); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to send SYNC READ packet"); + return return_code; + } + + // Collect one status packet per servo. Responses arrive in the id-list + // order, but a dropped response must not desynchronize the parse, so each + // packet is matched by the ID it carries rather than by arrival order. + std::set pending_ids(sync_read_ids_.begin(), sync_read_ids_.end()); + uint8_t feedback[kFtFeedbackLength]; + for (size_t i = 0; i < sync_read_ids_.size(); i++) { + int packet_id = -1; + return_code = receive_status(kFtBroadcastId, feedback, kFtFeedbackLength, kFtResponseTimeoutMs, &packet_id); + if (return_code != ReturnCode::SUCCESS) { + break; + } + + if (pending_ids.count(packet_id) == 0) { + PI_ERROR("DriverFt: SYNC READ response from unexpected servo ID %d", packet_id); + return_code = ReturnCode::FAIL; + break; + } + pending_ids.erase(packet_id); + + const int data_index = find_data_index(packet_id); + if (data_index < 0 || data_index >= servo_num_total_) { + PI_ERROR("DriverFt: no data index registered for servo ID %d", packet_id); + return_code = ReturnCode::FAIL; + break; + } + + pres_pos_[data_index] = static_cast(feedback[0] | (feedback[1] << 8)); + pres_vel_[data_index] = + decode_sign_magnitude(static_cast(feedback[2] | (feedback[3] << 8)), kFtVelocitySignBit); + pres_load_[data_index] = + decode_sign_magnitude(static_cast(feedback[4] | (feedback[5] << 8)), kFtLoadSignBit); + // feedback[6] is Present Input Voltage (0.1 V); openpi has no consumer for it. + pres_temp_[data_index] = feedback[7]; + pres_status_[data_index] = feedback[9]; + } + + if (return_code != ReturnCode::SUCCESS || !pending_ids.empty()) { + for (int id : pending_ids) { + PI_ERROR("DriverFt: no SYNC READ response from servo ID %d", id); + } + flush_input(); + classify_dead_servos(); + return ReturnCode::FAIL; + } + + // Bulk read succeeded for every still-registered servo; refresh the + // reported break point (dead_servo_ids_ stays sticky, see DriverDxl). + last_failed_servo_id_ = dead_servo_ids_.empty() ? -1 : *dead_servo_ids_.begin(); + + std::string info_pos, info_vel, info_load; + for (int i = 0; i < servo_num_total_; i++) { + info_pos += std::to_string(pres_pos_[i]) + ", "; + info_vel += std::to_string(pres_vel_[i]) + ", "; + info_load += std::to_string(pres_load_[i]) + ", "; + } + PI_INFO("Driver", InfoLevel::FREQUENT_3, "\nPositions (steps): %s\nVelocities (steps/s): %s\nLoads (0.1%%): %s", + info_pos.c_str(), info_vel.c_str(), info_load.c_str()); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::read_hardware_values(Servo* p_servo) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in read_hardware_values()"); + return ReturnCode::INVALID_PARAM; + } + + if (servo_num_total_ <= 0) { + PI_ERROR("Servo count is not properly initialized: %d", servo_num_total_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_servo->data_index_ < 0 || p_servo->data_index_ >= servo_num_total_) { + PI_ERROR("Servo data index %d (servo ID %d) exceeds maximum index %d", p_servo->data_index_, p_servo->id_, + servo_num_total_ - 1); + return ReturnCode::INVALID_PARAM; + } + + ServoFt* p_servo_ft = (ServoFt*)p_servo; + const int data_index = p_servo->data_index_; + + p_servo_ft->curr_pos_steps_ = pres_pos_[data_index]; + p_servo_ft->curr_pos_abs_ = p_servo_ft->steps_to_rad(pres_pos_[data_index]); + p_servo_ft->curr_vel_ = p_servo_ft->get_vel_rad_sec(pres_vel_[data_index]); + p_servo_ft->curr_tor_ = p_servo_ft->get_tor_nm(pres_load_[data_index]); + p_servo_ft->temperature_ = static_cast(pres_temp_[data_index]); + // Servo Status error bits: bit0 voltage, bit1 angle/sensor, bit2 overheat, + // bit3 overcurrent, bit5 overload (SMS/STS memory table). Raw value, no + // decoding -- same convention as the other servo families. + p_servo_ft->motor_error_code_ = pres_status_[data_index]; + + return Driver::read_hardware_values(p_servo); +} + +ReturnCode DriverFt::group_write_hardware_values() { + if (port_handler_ < 0) { + PI_ERROR("Serial port is not open in group_write_hardware_values()"); + return ReturnCode::NOT_INITIALIZED; + } + + // Torque limit first, then goal position (mirrors DriverDxl's goal-current- + // before-goal-position ordering so the limit is in place when motion starts). + ReturnCode return_code = sync_write(kFtAddrTorqueLimit, 2, pending_torque_limit_); + pending_torque_limit_.clear(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Group write for torque limit failed"); + pending_goal_position_.clear(); + return return_code; + } + + return_code = sync_write(kFtAddrGoalPosition, 2, pending_goal_position_); + pending_goal_position_.clear(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Group write for goal position failed"); + return return_code; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::enable_torque(ServoFt* p_servo, bool enable) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in enable_torque()"); + return ReturnCode::INVALID_PARAM; + } + return write_register(p_servo->id_, kFtAddrTorqueEnable, enable ? 1 : 0, 1); +} + +ReturnCode DriverFt::set_operating_mode(ServoFt* p_servo, uint8_t mode) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_operating_mode()"); + return ReturnCode::INVALID_PARAM; + } + return write_register(p_servo->id_, kFtAddrOperatingMode, mode, 1); +} + +ReturnCode DriverFt::set_position_pid(ServoFt* p_servo, float pos_p, float pos_i, float pos_d) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_position_pid()"); + return ReturnCode::INVALID_PARAM; + } + + auto to_gain_byte = [](float gain) { + if (gain < 0.0f) gain = 0.0f; + if (gain > 254.0f) gain = 254.0f; + return static_cast(gain); + }; + + ReturnCode return_code = write_register(p_servo->id_, kFtAddrPositionPGain, to_gain_byte(pos_p), 1); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + return_code = write_register(p_servo->id_, kFtAddrPositionDGain, to_gain_byte(pos_d), 1); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + return write_register(p_servo->id_, kFtAddrPositionIGain, to_gain_byte(pos_i), 1); +} + +ReturnCode DriverFt::set_acceleration(ServoFt* p_servo, uint8_t acceleration) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_acceleration()"); + return ReturnCode::INVALID_PARAM; + } + return write_register(p_servo->id_, kFtAddrAcceleration, acceleration, 1); +} + +ReturnCode DriverFt::set_goal_position_direct(ServoFt* p_servo, int32_t goal_position) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_goal_position_direct()"); + return ReturnCode::INVALID_PARAM; + } + if (goal_position < kFtGoalPositionMin) goal_position = kFtGoalPositionMin; + if (goal_position > kFtGoalPositionMax) goal_position = kFtGoalPositionMax; + return write_register(p_servo->id_, kFtAddrGoalPosition, goal_position, 2); +} + +ReturnCode DriverFt::set_goal_position_group(ServoFt* p_servo, int32_t goal_position) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_goal_position_group()"); + return ReturnCode::INVALID_PARAM; + } + if (dead_servo_ids_.count(p_servo->id_) != 0) { + // Same policy as DriverDxl group writes: skip dead servos so the + // control loop does not stall on their bus timeouts. + return ReturnCode::SUCCESS; + } + if (goal_position < kFtGoalPositionMin) goal_position = kFtGoalPositionMin; + if (goal_position > kFtGoalPositionMax) goal_position = kFtGoalPositionMax; + pending_goal_position_.emplace_back(p_servo->id_, static_cast(goal_position)); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::set_torque_limit_group(ServoFt* p_servo, int32_t torque_limit) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_torque_limit_group()"); + return ReturnCode::INVALID_PARAM; + } + if (dead_servo_ids_.count(p_servo->id_) != 0) { + return ReturnCode::SUCCESS; + } + if (torque_limit < kFtTorqueLimitMin) torque_limit = kFtTorqueLimitMin; + if (torque_limit > kFtTorqueLimitMax) torque_limit = kFtTorqueLimitMax; + pending_torque_limit_.emplace_back(p_servo->id_, static_cast(torque_limit)); + return ReturnCode::SUCCESS; +} + +void DriverFt::discard_pending_group_writes(ServoFt* p_servo) { + if (p_servo == nullptr) { + return; + } + const int id = p_servo->id_; + auto drop_id = [id](std::vector>& entries) { + entries.erase(std::remove_if(entries.begin(), entries.end(), + [id](const std::pair& entry) { return entry.first == id; }), + entries.end()); + }; + drop_id(pending_goal_position_); + drop_id(pending_torque_limit_); +} + +ReturnCode DriverFt::get_torque_enabled(ServoFt* p_servo, int32_t& enabled) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in get_torque_enabled()"); + return ReturnCode::INVALID_PARAM; + } + return read_register(p_servo->id_, kFtAddrTorqueEnable, 1, enabled); +} + +ReturnCode DriverFt::get_present_position(ServoFt* p_servo, int32_t& present_position) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in get_present_position()"); + return ReturnCode::INVALID_PARAM; + } + return read_register(p_servo->id_, kFtAddrPresentPosition, 2, present_position); +} + +ReturnCode DriverFt::sync_goal_position_to_present(ServoFt* p_servo) { + int32_t present_position = 0; + ReturnCode return_code = get_present_position(p_servo, present_position); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to read present position for goal sync (servo ID %d)", p_servo ? p_servo->id_ : -1); + return return_code; + } + return set_goal_position_direct(p_servo, present_position); +} diff --git a/native/pi_control/src/pi_driver_serial.cpp b/native/pi_control/src/pi_driver_serial.cpp new file mode 100644 index 0000000..c639565 --- /dev/null +++ b/native/pi_control/src/pi_driver_serial.cpp @@ -0,0 +1,409 @@ +/*! + * @file pi_driver_serial.cpp + * @brief Implementation of the DriverSerial raw-tty wrapper. + */ + +#include "pi_driver_serial.hpp" + +#include +#include +#include +#include + +#if defined(__linux__) +#include +#elif defined(__APPLE__) +#include +#endif + +#include +#include +#include +#include +#include +#include +#include + +#include "pi_info.hpp" + +namespace { + +std::string tty_name_from_path(const std::string& path) { + char resolved[PATH_MAX] = {0}; + const char* p = ::realpath(path.c_str(), resolved); + const std::string full = (p != nullptr) ? std::string(resolved) : path; + const size_t slash = full.find_last_of('/'); + return slash == std::string::npos ? full : full.substr(slash + 1); +} + +bool file_exists(const std::string& path) { + struct stat st {}; + return ::stat(path.c_str(), &st) == 0; +} + +bool write_text_file(const std::string& path, const std::string& value) { + std::ofstream f(path); + if (!f.is_open()) { + return false; + } + f << value; + return f.good(); +} + +void apply_usb_serial_latency_timer(const std::string& port_path) { +#if defined(__linux__) + const std::string tty = tty_name_from_path(port_path); + if (tty.empty()) { + return; + } + + const std::string candidates[] = { + "/sys/class/tty/" + tty + "/device/latency_timer", + "/sys/bus/usb-serial/devices/" + tty + "/latency_timer", + }; + bool saw_latency_timer = false; + for (const std::string& latency_path : candidates) { + if (!file_exists(latency_path)) { + continue; + } + saw_latency_timer = true; + // FTDI latency_timer accepts 1..255 ms; 0 is not a valid sysfs value on + // normal Linux ftdi_sio, so 1 ms is the practical minimum here. + if (write_text_file(latency_path, "1\n")) { + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "DriverSerial: set USB serial latency_timer=1 at %s", + latency_path.c_str()); + } else { + PI_WARN("DriverSerial: failed to set USB serial latency_timer at %s: %s", + latency_path.c_str(), std::strerror(errno)); + } + } + if (!saw_latency_timer) { + PI_INFO("Driver", InfoLevel::FREQUENT_3, + "DriverSerial: no USB serial latency_timer sysfs entry for %s (%s)", + port_path.c_str(), tty.c_str()); + } +#else + // macOS uses IOSSDATALAT (per-fd ioctl) applied below in apply_low_latency_flag(). + (void)port_path; +#endif +} + +void apply_low_latency_flag(int fd, const std::string& port_path) { +#if defined(__linux__) + serial_struct serial {}; + if (::ioctl(fd, TIOCGSERIAL, &serial) != 0) { + PI_INFO("Driver", InfoLevel::FREQUENT_3, + "DriverSerial: TIOCGSERIAL low-latency query unsupported for '%s': %s", + port_path.c_str(), std::strerror(errno)); + return; + } + if ((serial.flags & ASYNC_LOW_LATENCY) != 0) { + return; + } + serial.flags |= ASYNC_LOW_LATENCY; + if (::ioctl(fd, TIOCSSERIAL, &serial) != 0) { + PI_WARN("DriverSerial: failed to set ASYNC_LOW_LATENCY for '%s': %s", + port_path.c_str(), std::strerror(errno)); + return; + } + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "DriverSerial: enabled ASYNC_LOW_LATENCY for '%s'", port_path.c_str()); +#elif defined(__APPLE__) + // IOSSDATALAT: per-fd minimum latency in microseconds. 1 us is the + // documented minimum and the FTDI driver clamps low values internally. + // This is the macOS counterpart of Linux's ASYNC_LOW_LATENCY. + unsigned long latency_us = 1; + if (::ioctl(fd, IOSSDATALAT, &latency_us) != 0) { + PI_INFO("Driver", InfoLevel::FREQUENT_3, + "DriverSerial: IOSSDATALAT low-latency query unsupported for '%s': %s", + port_path.c_str(), std::strerror(errno)); + return; + } + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "DriverSerial: enabled IOSSDATALAT low latency for '%s'", port_path.c_str()); +#else + (void)fd; + (void)port_path; +#endif +} + +#if defined(__APPLE__) +// Apply an arbitrary baud rate on macOS via IOSSIOSPEED. This is the same +// mechanism pyserial uses. Required because Apple termios B macros +// stop at B230400; our Dynamixel servos need 1 Mbps / 3 Mbps. +bool apply_mac_custom_baud(int fd, int baud_rate) { + speed_t speed = static_cast(baud_rate); + return ::ioctl(fd, IOSSIOSPEED, &speed) == 0; +} +#endif + +} // namespace + +DriverSerial::DriverSerial(Device* p_device, const CommandLineArgs& cla) + : Driver(p_device, cla) {} + +DriverSerial::~DriverSerial() { + stop_reception(); + DriverSerial::close(); +} + +speed_t DriverSerial::baud_constant(int baud_rate) { + // Apple only defines B-prefixed macros up to B230400. Higher + // rates (460800 / 500000 / 921600 / 1 Mbps) ship as Linux extensions in + // and are absent on macOS. Each high-rate entry is + // therefore guarded by its own #if defined(B...) so the same source builds + // on both platforms; macOS falls back to IOSSIOSPEED for those values + // (see DriverSerial::open() — the speed==0 branch on Apple). + static const std::map kMap = { + {9600, B9600}, {19200, B19200}, {38400, B38400}, {57600, B57600}, + {115200, B115200}, {230400, B230400}, +#if defined(B460800) + {460800, B460800}, +#endif +#if defined(B500000) + {500000, B500000}, +#endif +#if defined(B576000) + {576000, B576000}, +#endif +#if defined(B921600) + {921600, B921600}, +#endif +#if defined(B1000000) + {1000000, B1000000}, +#endif + }; + auto it = kMap.find(baud_rate); + return (it == kMap.end()) ? static_cast(0) : it->second; +} + +ReturnCode DriverSerial::open(int baud_rate) { + if (port_handler_ >= 0) { + // Already open -- treat as success so callers can safely retry. + return ReturnCode::SUCCESS; + } + + if (control_port_name_.empty()) { + PI_ERROR("DriverSerial::open: empty control_port_name_ (set --control_port)"); + return ReturnCode::INVALID_PARAM; + } + +#if defined(__APPLE__) + // macOS exposes every USB-serial under two synonyms: ``/dev/tty.`` + // (POSIX dial-in — blocks on DCD until the modem asserts carrier) and + // ``/dev/cu.`` (call-up — opens immediately). USB-serial adapters + // never assert DCD, so opening ``tty.usbserial-*`` or ``tty.usbmodem*`` + // hangs ``open()`` forever. Fail fast with a hint instead of letting + // the next O_NONBLOCK ``::open()`` call block the entire process. + if (control_port_name_.rfind("/dev/tty.usbserial", 0) == 0 || + control_port_name_.rfind("/dev/tty.usbmodem", 0) == 0 || + control_port_name_.rfind("/dev/tty.SLAB_USBtoUART", 0) == 0) { + std::string cu_hint = control_port_name_; + cu_hint.replace(0, 9, "/dev/cu."); // "/dev/tty." -> "/dev/cu." + PI_ERROR("DriverSerial::open: refusing macOS DCD-blocking path '%s'. " + "Use the call-up variant '%s' instead.", + control_port_name_.c_str(), cu_hint.c_str()); + return ReturnCode::INVALID_PARAM; + } +#endif + + const speed_t speed = baud_constant(baud_rate); +#if defined(__APPLE__) + // macOS termios maxes out at B230400 (B460800 / B921600 / B1000000 are + // absent from ). For anything above that we run cfset*speed() + // with B9600 as a placeholder and then apply the real speed via the + // IOSSIOSPEED ioctl. baud_constant() returning 0 is therefore expected + // for 1 Mbps / 3 Mbps / etc. on macOS and is handled below. + const bool use_custom_baud_macos = (speed == 0); + const speed_t termios_speed = use_custom_baud_macos ? B9600 : speed; +#else + if (speed == 0) { + PI_ERROR("DriverSerial::open: unsupported baud rate %d for port '%s'", + baud_rate, control_port_name_.c_str()); + return ReturnCode::NOT_SUPPORTED; + } + const speed_t termios_speed = speed; +#endif + + apply_usb_serial_latency_timer(control_port_name_); + + const int fd = ::open(control_port_name_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fd < 0) { + PI_ERROR("DriverSerial::open: failed to open '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + return ReturnCode::FAIL; + } + apply_low_latency_flag(fd, control_port_name_); + + struct termios tty; + std::memset(&tty, 0, sizeof(tty)); + if (::tcgetattr(fd, &tty) != 0) { + PI_ERROR("DriverSerial::open: tcgetattr failed for '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + ::close(fd); + return ReturnCode::FAIL; + } + + ::cfsetispeed(&tty, termios_speed); + ::cfsetospeed(&tty, termios_speed); + + // 8N1, no flow control, raw I/O. + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; + tty.c_cflag |= (CLOCAL | CREAD); + tty.c_cflag &= ~(PARENB | PARODD); // no parity + tty.c_cflag &= ~CSTOPB; // 1 stop bit + tty.c_cflag &= ~CRTSCTS; // no HW flow control + + tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXOFF | IXANY); + tty.c_lflag = 0; // raw, no echo, no canonical processing + tty.c_oflag = 0; // raw output + + // Non-blocking semantics on read -- the reception thread polls. + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 0; + + if (::tcsetattr(fd, TCSANOW, &tty) != 0) { + PI_ERROR("DriverSerial::open: tcsetattr failed for '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + ::close(fd); + return ReturnCode::FAIL; + } + +#if defined(__APPLE__) + if (use_custom_baud_macos) { + if (!apply_mac_custom_baud(fd, baud_rate)) { + PI_ERROR("DriverSerial::open: IOSSIOSPEED failed for '%s' @ %d bps: %s", + control_port_name_.c_str(), baud_rate, std::strerror(errno)); + ::close(fd); + return ReturnCode::FAIL; + } + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "DriverSerial: applied macOS custom baud %d via IOSSIOSPEED on '%s'", + baud_rate, control_port_name_.c_str()); + } +#endif + + // Some USB CDC devices do not start streaming replies until the host + // asserts DTR/RTS. Harmless for plain UARTs. + int modem_bits = 0; + if (::ioctl(fd, TIOCMGET, &modem_bits) == 0) { + modem_bits |= (TIOCM_DTR | TIOCM_RTS); + if (::ioctl(fd, TIOCMSET, &modem_bits) != 0) { + PI_WARN("DriverSerial::open: failed to set DTR/RTS for '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } + } else { + PI_WARN("DriverSerial::open: failed to read modem bits for '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + } + + // Flush any boot-time noise the device may have queued. + ::tcflush(fd, TCIOFLUSH); + + port_handler_ = fd; + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "Serial port opened: port=%s baud=%d", control_port_name_.c_str(), baud_rate); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverSerial::close() { + stop_reception(); + if (port_handler_ >= 0) { + ::close(port_handler_); + port_handler_ = -1; + } + return ReturnCode::SUCCESS; +} + +ReturnCode DriverSerial::write_bytes(const uint8_t* p_data, size_t size) { + if (port_handler_ < 0) { + return ReturnCode::NOT_INITIALIZED; + } + if (p_data == nullptr || size == 0) { + return ReturnCode::INVALID_PARAM; + } + + size_t written_total = 0; + while (written_total < size) { + const ssize_t n = ::write(port_handler_, p_data + written_total, size - written_total); + if (n < 0) { + if (errno == EINTR) { + continue; + } + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // The CDC kernel buffer is full -- yield briefly and retry so + // we do not silently drop bytes mid-frame. + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + PI_ERROR("DriverSerial::write_bytes: write failed on '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + return ReturnCode::FAIL; + } + written_total += static_cast(n); + } + return ReturnCode::SUCCESS; +} + +ReturnCode DriverSerial::write_text(const char* text) { + if (text == nullptr) { + return ReturnCode::INVALID_PARAM; + } + return write_bytes(reinterpret_cast(text), std::strlen(text)); +} + +ReturnCode DriverSerial::start_reception(const callback_t& callback) { + stop_reception(); + + if (port_handler_ < 0) { + PI_ERROR("DriverSerial::start_reception: port not open"); + return ReturnCode::NOT_INITIALIZED; + } + if (!callback) { + PI_ERROR("DriverSerial::start_reception: empty callback"); + return ReturnCode::INVALID_PARAM; + } + + is_running_.store(true, std::memory_order_release); + reception_thread_ = std::thread(&DriverSerial::receive_loop, this, callback); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverSerial::stop_reception() { + if (!is_running_.exchange(false, std::memory_order_acq_rel)) { + if (reception_thread_.joinable()) { + reception_thread_.join(); + } + return ReturnCode::SUCCESS; + } + if (reception_thread_.joinable()) { + reception_thread_.join(); + } + return ReturnCode::SUCCESS; +} + +void DriverSerial::receive_loop(callback_t callback) { + uint8_t buffer[256]; + while (is_running_.load(std::memory_order_acquire)) { + const int fd = port_handler_; + if (fd < 0) { + break; + } + const ssize_t n = ::read(fd, buffer, sizeof(buffer)); + if (n > 0) { + callback(buffer, static_cast(n)); + continue; + } + if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR) { + PI_WARN("DriverSerial::receive_loop: read error on '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + // Stop on hard errors so the device class can react (e.g. mark not-ready). + break; + } + // Cooperative idle so we do not pin a CPU on an empty CDC pipe. + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } +} diff --git a/native/pi_control/src/pi_servo.cpp b/native/pi_control/src/pi_servo.cpp index 7f22822..11ab556 100644 --- a/native/pi_control/src/pi_servo.cpp +++ b/native/pi_control/src/pi_servo.cpp @@ -12,6 +12,7 @@ #include "pi_servo_can_encoder.hpp" #include "pi_servo_controller.hpp" #include "pi_servo_dm.hpp" +#include "pi_servo_ft.hpp" #define MAX_CNT_POS_EXCEED \ 100 ///< Threshold count for position limit exceed detection @@ -365,16 +366,19 @@ ReturnCode Servo::new_servos(const json& joint_config, p_servo = std::make_unique(p_device, p_joint, p_driver); PI_INFO("Servo", InfoLevel::HELPFUL_1, "Created ServoDm instance"); - } else if (servo_model == p_config_model->val_servo_model_trossen_wxai) { - p_servo = std::make_unique(p_device, p_joint, p_driver); - PI_INFO("Servo", InfoLevel::HELPFUL_1, - "Created ServoController instance"); } else if (servo_model == p_config_model->val_servo_model_can_passive_encoder) { p_servo = std::make_unique(p_device, p_joint, p_driver); PI_INFO("Servo", InfoLevel::HELPFUL_1, "Created ServoCanPassiveEncoder instance"); + } else if (servo_model == p_config_model->val_servo_model_trossen_wxai) { + p_servo = std::make_unique(p_device, p_joint, p_driver); + PI_INFO("Servo", InfoLevel::HELPFUL_1, + "Created ServoController instance"); + } else if (servo_model == p_config_model->val_servo_model_ft_sts3215) { + p_servo = std::make_unique(p_device, p_joint, p_driver); + PI_INFO("Servo", InfoLevel::HELPFUL_1, "Created ServoFt instance"); } else { PI_ERROR("Unsupported servo model: %s", servo_model.c_str()); return ReturnCode::NOT_SUPPORTED; diff --git a/native/pi_control/src/pi_servo_ft.cpp b/native/pi_control/src/pi_servo_ft.cpp new file mode 100644 index 0000000..d3c00c9 --- /dev/null +++ b/native/pi_control/src/pi_servo_ft.cpp @@ -0,0 +1,395 @@ +/*! + * @file pi_servo_ft.cpp + * @brief Implementation of the ServoFt class for FeeTech SMS/STS serial bus servos. + */ + +#include +#include + +#include "pi_driver_ft.hpp" +#include "pi_joint.hpp" +#include "pi_servo_ft.hpp" + +// SMS/STS Operating Mode register values (addr 33). +#define FT_MODE_POSITION 0 + +// Torque Limit register full scale (0.1% of stall torque). +#define FT_TORQUE_LIMIT_FULL_SCALE 1000 + +// Acceleration register bounds (addr 41, unit 100 steps/s^2). +#define FT_ACCELERATION_MAX 254 + +// Verified torque disable: attempts and delay between attempts. The HX +// firmware on SO-ARM101 kits ACKs but ignores a torque-off that arrives right +// after a torque-on / goal write, so the disable is verified via readback and +// retried until the register actually reads 0. +#define FT_TORQUE_DISABLE_MAX_ATTEMPTS 5 +#define FT_TORQUE_DISABLE_RETRY_DELAY_US 10000 + +const ServoFtParam g_servo_ft_param(DEFAULT_TOLERABLE_POS_DIFFERENCE_RAD, MAX_POS_DIFFERENCE_RAD, + DEFAULT_VELOCITY_THRESHOLD_RAD_SEC); + +ServoFt::ServoFt(Device* p_device, Joint* p_joint, Driver* p_driver) : Servo(p_device, p_joint, p_driver) { + p_driver_ft_ = (DriverFt*)p_driver; +} + +ServoFt::~ServoFt() {} + +ReturnCode ServoFt::init_config_model(const json& servo_config, const DeviceConfig* p_config) { + p_servo_param_ = &g_servo_ft_param; + + ReturnCode return_code = Servo::init_config_model(servo_config, p_config); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + if (servo_model_ == p_config->val_servo_model_ft_sts3215) { + type_ = ServoType::FT_STS3215; + } else { + PI_ERROR("Unsupported servo model '%s' (servo ID %d)", servo_model_.c_str(), id_); + return ReturnCode::NOT_SUPPORTED; + } + + return_code = p_config->get_field_value(servo_config, p_config->fn_servo_resolution, servo_resolution_); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + PI_INFO("ServoFt", InfoLevel::HELPFUL_1, "Servo ID %d: servo_resolution=%d", id_, servo_resolution_); + // STS3215 operates single-turn with the calibrated middle at + // resolution / 2 (step 2048), which maps to 0 rad. + center_offset_ = servo_resolution_ / 2; + + // Optional: Acceleration register value (unit 100 steps/s^2). Reuses the + // generic prof_accel field; 0 (default) leaves the servo's own ramp off. + return_code = p_config->get_field_value_optional(servo_config, p_config->fn_servo_prof_accel, prof_accel_); + if (return_code == ReturnCode::SUCCESS) { + PI_INFO("ServoFt", InfoLevel::HELPFUL_1, "Servo ID %d: prof_accel=%d", id_, prof_accel_); + if (prof_accel_ < 0 || prof_accel_ > FT_ACCELERATION_MAX) { + PI_ERROR("Servo ID %d: prof_accel must be within 0..%d for FeeTech servos, but found %d", id_, + FT_ACCELERATION_MAX, prof_accel_); + return ReturnCode::INVALID_PARAM; + } + } + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoFt::start_hardware() { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_device_ == nullptr) { + PI_ERROR("Device pointer is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping start hardware", id_); + return ReturnCode::SUCCESS; + } + + ReturnCode return_code = p_driver_ft_->enable_torque(this, false); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to disable torque (servo ID %d)", id_); + return return_code; + } + usleep(200); + + return_code = p_driver_ft_->set_operating_mode(this, FT_MODE_POSITION); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set position operating mode (servo ID %d)", id_); + return return_code; + } + usleep(200); + + // Position PID gains are written only when a P gain is configured; the + // factory defaults (P=32, D=32, I=0) are otherwise kept, matching the + // LeRobot SO-ARM bring-up which does not touch these registers. + if (pos_kp_ > 0) { + return_code = p_driver_ft_->set_position_pid(this, pos_kp_, pos_ki_, pos_kd_); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set position PID gain (servo ID %d)", id_); + return return_code; + } + usleep(200); + } + + if (prof_accel_ > 0) { + return_code = p_driver_ft_->set_acceleration(this, static_cast(prof_accel_)); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set acceleration (servo ID %d)", id_); + return return_code; + } + usleep(200); + } + + // Align the goal with the present position BEFORE engaging torque so the + // servo holds its pose instead of jumping to a stale goal register (same + // rationale as ServoDxl::start_hardware()). + return_code = p_driver_ft_->sync_goal_position_to_present(this); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to sync goal position to present position before enabling torque (servo ID %d)", id_); + return return_code; + } + usleep(200); + + return_code = p_driver_ft_->enable_torque(this, true); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to enable torque (servo ID %d)", id_); + return return_code; + } + usleep(200); + + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo %d started hardware", id_); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoFt::park_safely() { + if (parked_ == true) { + return ReturnCode::SUCCESS; + } + + if (p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping park safely", id_); + return ReturnCode::SUCCESS; + } + + if (p_driver_ft_ == nullptr) { + PI_ERROR("FeeTech driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + ReturnCode return_code = read_hardware_values(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to read current hardware values (servo ID %d)", id_); + return return_code; + } + usleep(200); + + // Keep the goal register aligned with the resting pose so the next + // torque-on (e.g. a restart without a full re-init) does not jump. This + // must happen BEFORE the torque disable: a goal position write arriving + // after the disable re-engages torque on SO-ARM101 kits (Hiwonder HX + // firmware) and the arm stays locked after shutdown. + return_code = p_driver_ft_->set_goal_position_direct(this, curr_pos_steps_); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set goal position to current position (servo ID %d)", id_); + return return_code; + } + usleep(200); + + return_code = p_driver_ft_->enable_torque(this, false); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to disable torque (servo ID %d)", id_); + return return_code; + } + usleep(200); + + parked_ = true; + + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d parked safely", id_); + + return return_code; +} + +ReturnCode ServoFt::move(float target_pos) { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + float clipped_pos = clipping(target_pos, pos_min_rel_, pos_max_rel_); + float target_pos_absolute = get_pos_rad_absolute(clipped_pos); + int32_t target_pos_steps = rad_to_steps(target_pos_absolute); + + ReturnCode return_code = p_driver_ft_->set_goal_position_group(this, target_pos_steps); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set goal position (servo ID %d)", id_); + return return_code; + } + + PI_INFO("Servo", InfoLevel::FREQUENT_3, "Joint ID %d, Servo ID %d: move target_pos=%.3f rad, target_steps=%d", + p_joint_->id_, id_, target_pos, target_pos_steps); + + return return_code; +} + +ReturnCode ServoFt::move(float target_pos, float target_vel, float target_tor) { + // target_vel is part of the Servo interface but the FeeTech position mode + // does not consume an explicit goal velocity here. + (void)target_vel; + if (p_driver_ft_ == nullptr || p_joint_ == nullptr || p_device_ == nullptr) { + PI_ERROR("Required pointers are not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + float clipped_pos = clipping(target_pos, pos_min_rel_, pos_max_rel_); + float target_pos_absolute = get_pos_rad_absolute(clipped_pos); + int32_t target_pos_steps = rad_to_steps(target_pos_absolute); + + float rescaled_torque = target_tor * p_joint_->torq_rescale_; + float clipped_torque = clipping(rescaled_torque, p_joint_->torq_min_, p_joint_->torq_max_); + int32_t torque_limit = torque_to_torque_limit(clipped_torque); + + PI_INFO("ServoFt", InfoLevel::FREQUENT_3, + "Joint ID %d, Servo ID %d: move target_pos=%.3f rad, target_steps=%d, requested_torque=%.3f Nm, " + "torque_limit=%d", + p_joint_->id_, id_, target_pos, target_pos_steps, target_tor, torque_limit); + + ReturnCode return_code = p_driver_ft_->set_torque_limit_group(this, torque_limit); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set torque limit (servo ID %d)", id_); + return return_code; + } + + return_code = p_driver_ft_->set_goal_position_group(this, target_pos_steps); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set goal position (servo ID %d)", id_); + return return_code; + } + + return return_code; +} + +ReturnCode ServoFt::apply_torque(float torque) { + (void)torque; + PI_ERROR( + "Servo ID %d: apply_torque is not supported for %s (no current control loop); " + "use position control with a torque limit instead", + id_, servo_model_.c_str()); + return ReturnCode::NOT_SUPPORTED; +} + +ReturnCode ServoFt::enable_torque(bool enable) { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping enable_torque", id_); + return ReturnCode::SUCCESS; + } + return p_driver_ft_->enable_torque(this, enable); +} + +int32_t ServoFt::torque_to_torque_limit(float torque) { + if (kt_ <= 0) { + PI_ERROR("Servo ID %d: kt is not configured; cannot convert torque to a torque limit", id_); + return 0; + } + + // The Torque Limit register is an unsigned magnitude (the servo applies it + // in both directions), so only the requested torque magnitude matters. + int32_t torque_limit = (int32_t)(fabs(torque) / kt_); + if (torque_limit > FT_TORQUE_LIMIT_FULL_SCALE) { + torque_limit = FT_TORQUE_LIMIT_FULL_SCALE; + } + return torque_limit; +} + +ReturnCode ServoFt::change_control_mode_for_leader() { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: device is read only, skipping change_control_mode_for_leader", id_); + return ReturnCode::SUCCESS; + } + + if (p_device_ != nullptr && p_device_->is_force_feedback_enabled()) { + PI_ERROR( + "Servo ID %d: force feedback is not supported for %s (no current control loop); " + "run the leader without --force_feedback", + id_, servo_model_.c_str()); + return ReturnCode::NOT_SUPPORTED; + } + + // SO-ARM leader convention (same as LeRobot): fully passive, torque off so + // the operator back-drives the arm. The lightweight leader hardware does + // not need gravity compensation. + // + // Same-cycle goal entries queued by the final move-to-ready step must be + // dropped BEFORE disabling torque: flushing a goal position after the + // disable re-engages torque on SO-ARM101 kits (Hiwonder HX firmware) and + // the leader ends up locked at home. + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: leader mode, disabling torque (passive)", id_); + p_driver_ft_->discard_pending_group_writes(this); + + // Disable-and-verify loop: the write is ACKed even when the firmware ignores + // it (observed when the disable lands right after a torque-on / goal write on + // the leader gripper), so read Torque Enable back and retry until it is 0. + int32_t torque_enabled = -1; + for (int attempt = 1; attempt <= FT_TORQUE_DISABLE_MAX_ATTEMPTS; attempt++) { + ReturnCode return_code = p_driver_ft_->enable_torque(this, false); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to disable torque for passive leader (servo ID %d)", id_); + return return_code; + } + usleep(200); + + return_code = p_driver_ft_->get_torque_enabled(this, torque_enabled); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to read back Torque Enable after passive-leader disable (servo ID %d)", id_); + return return_code; + } + if (torque_enabled == 0) { + return ReturnCode::SUCCESS; + } + + PI_WARN("Servo ID %d: Torque Enable reads %d after passive-leader disable (attempt %d/%d), retrying", id_, + torque_enabled, attempt, FT_TORQUE_DISABLE_MAX_ATTEMPTS); + usleep(FT_TORQUE_DISABLE_RETRY_DELAY_US); + } + + PI_ERROR("Servo ID %d: Torque Enable still reads %d after %d passive-leader disable attempts (expected 0)", id_, + torque_enabled, FT_TORQUE_DISABLE_MAX_ATTEMPTS); + return ReturnCode::FAIL; +} + +ReturnCode ServoFt::change_control_mode_for_follower() { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: device is read only, skipping change_control_mode_for_follower", id_); + return ReturnCode::SUCCESS; + } + + ReturnCode return_code = p_driver_ft_->enable_torque(this, false); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to disable torque (servo ID %d)", id_); + return return_code; + } + usleep(10000); + + return_code = p_driver_ft_->set_operating_mode(this, FT_MODE_POSITION); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set position operating mode (servo ID %d)", id_); + return return_code; + } + + // Align the goal with the present position before re-enabling torque to + // avoid a stale-goal jump after the mode switch. + return_code = p_driver_ft_->sync_goal_position_to_present(this); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to sync goal position to present (servo ID %d)", id_); + return return_code; + } + + return_code = p_driver_ft_->enable_torque(this, true); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to enable torque (servo ID %d)", id_); + return return_code; + } + + return return_code; +} diff --git a/src/openpi_control/arm_commands/movements_test_so101.csv b/src/openpi_control/arm_commands/movements_test_so101.csv new file mode 100644 index 0000000..8739a16 --- /dev/null +++ b/src/openpi_control/arm_commands/movements_test_so101.csv @@ -0,0 +1,1352 @@ +timestamp, message_id, arm_joint0_pos, arm_joint1_pos, arm_joint2_pos, arm_joint3_pos, arm_joint4_pos, effector_joint0_pos, arm_joint0_vel, arm_joint1_vel, arm_joint2_vel, arm_joint3_vel, arm_joint4_vel, effector_joint0_vel, arm_joint0_tor, arm_joint1_tor, arm_joint2_tor, arm_joint3_tor, arm_joint4_tor, effector_joint0_tor +0,0,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20,1,0.000000,0.000000,0.000000,0.000000,0.000000,0.004000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +40,2,0.000000,0.000000,0.000000,0.000000,0.000000,0.008000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +60,3,0.000000,0.000000,0.000000,0.000000,0.000000,0.012000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +80,4,0.000000,0.000000,0.000000,0.000000,0.000000,0.016000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +100,5,0.000000,0.000000,0.000000,0.000000,0.000000,0.020000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +120,6,0.000000,0.000000,0.000000,0.000000,0.000000,0.024000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +140,7,0.000000,0.000000,0.000000,0.000000,0.000000,0.028000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +160,8,0.000000,0.000000,0.000000,0.000000,0.000000,0.032000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +180,9,0.000000,0.000000,0.000000,0.000000,0.000000,0.036000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +200,10,0.000000,0.000000,0.000000,0.000000,0.000000,0.040000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +220,11,0.000000,0.000000,0.000000,0.000000,0.000000,0.044000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +240,12,0.000000,0.000000,0.000000,0.000000,0.000000,0.048000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +260,13,0.000000,0.000000,0.000000,0.000000,0.000000,0.052000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +280,14,0.000000,0.000000,0.000000,0.000000,0.000000,0.056000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +300,15,0.000000,0.000000,0.000000,0.000000,0.000000,0.060000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +320,16,0.000000,0.000000,0.000000,0.000000,0.000000,0.064000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +340,17,0.000000,0.000000,0.000000,0.000000,0.000000,0.068000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +360,18,0.000000,0.000000,0.000000,0.000000,0.000000,0.072000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +380,19,0.000000,0.000000,0.000000,0.000000,0.000000,0.076000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +400,20,0.000000,0.000000,0.000000,0.000000,0.000000,0.080000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +420,21,0.000000,0.000000,0.000000,0.000000,0.000000,0.084000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +440,22,0.000000,0.000000,0.000000,0.000000,0.000000,0.088000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +460,23,0.000000,0.000000,0.000000,0.000000,0.000000,0.092000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +480,24,0.000000,0.000000,0.000000,0.000000,0.000000,0.096000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +500,25,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +520,26,0.015705,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +540,27,0.031395,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +560,28,0.047054,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +580,29,0.062667,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +600,30,0.078217,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +620,31,0.093691,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +640,32,0.109072,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +660,33,0.124345,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +680,34,0.139496,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +700,35,0.154508,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +720,36,0.169369,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +740,37,0.184062,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +760,38,0.198574,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +780,39,0.212890,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +800,40,0.226995,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +820,41,0.240877,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +840,42,0.254521,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +860,43,0.267913,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +880,44,0.281042,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +900,45,0.293893,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +920,46,0.306454,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +940,47,0.318712,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +960,48,0.330656,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +980,49,0.342274,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1000,50,0.353553,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1020,51,0.364484,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1040,52,0.375056,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1060,53,0.385257,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1080,54,0.395078,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1100,55,0.404508,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1120,56,0.413540,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1140,57,0.422164,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1160,58,0.430371,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1180,59,0.438153,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1200,60,0.445503,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1220,61,0.452414,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1240,62,0.458877,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1260,63,0.464888,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1280,64,0.470440,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1300,65,0.475528,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1320,66,0.480147,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1340,67,0.484292,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1360,68,0.487958,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1380,69,0.491144,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1400,70,0.493844,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1420,71,0.496057,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1440,72,0.497781,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1460,73,0.499013,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1480,74,0.499753,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1500,75,0.500000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1520,76,0.499753,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1540,77,0.499013,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1560,78,0.497781,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1580,79,0.496057,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1600,80,0.493844,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1620,81,0.491144,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1640,82,0.487958,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1660,83,0.484292,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1680,84,0.480147,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1700,85,0.475528,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1720,86,0.470440,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1740,87,0.464888,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1760,88,0.458877,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1780,89,0.452414,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1800,90,0.445503,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1820,91,0.438153,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1840,92,0.430371,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1860,93,0.422164,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1880,94,0.413540,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1900,95,0.404508,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1920,96,0.395078,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1940,97,0.385257,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1960,98,0.375056,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +1980,99,0.364484,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2000,100,0.353553,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2020,101,0.342274,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2040,102,0.330656,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2060,103,0.318712,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2080,104,0.306454,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2100,105,0.293893,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2120,106,0.281042,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2140,107,0.267913,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2160,108,0.254521,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2180,109,0.240877,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2200,110,0.226995,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2220,111,0.212890,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2240,112,0.198574,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2260,113,0.184062,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2280,114,0.169369,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2300,115,0.154508,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2320,116,0.139496,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2340,117,0.124345,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2360,118,0.109072,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2380,119,0.093691,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2400,120,0.078217,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2420,121,0.062667,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2440,122,0.047054,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2460,123,0.031395,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2480,124,0.015705,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2500,125,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2520,126,-0.015705,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2540,127,-0.031395,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2560,128,-0.047054,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2580,129,-0.062667,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2600,130,-0.078217,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2620,131,-0.093691,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2640,132,-0.109072,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2660,133,-0.124345,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2680,134,-0.139496,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2700,135,-0.154508,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2720,136,-0.169369,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2740,137,-0.184062,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2760,138,-0.198574,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2780,139,-0.212890,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2800,140,-0.226995,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2820,141,-0.240877,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2840,142,-0.254521,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2860,143,-0.267913,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2880,144,-0.281042,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2900,145,-0.293893,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2920,146,-0.306454,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2940,147,-0.318712,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2960,148,-0.330656,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +2980,149,-0.342274,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3000,150,-0.353553,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3020,151,-0.364484,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3040,152,-0.375056,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3060,153,-0.385257,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3080,154,-0.395078,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3100,155,-0.404508,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3120,156,-0.413540,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3140,157,-0.422164,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3160,158,-0.430371,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3180,159,-0.438153,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3200,160,-0.445503,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3220,161,-0.452414,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3240,162,-0.458877,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3260,163,-0.464888,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3280,164,-0.470440,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3300,165,-0.475528,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3320,166,-0.480147,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3340,167,-0.484292,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3360,168,-0.487958,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3380,169,-0.491144,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3400,170,-0.493844,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3420,171,-0.496057,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3440,172,-0.497781,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3460,173,-0.499013,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3480,174,-0.499753,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3500,175,-0.500000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3520,176,-0.499753,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3540,177,-0.499013,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3560,178,-0.497781,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3580,179,-0.496057,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3600,180,-0.493844,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3620,181,-0.491144,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3640,182,-0.487958,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3660,183,-0.484292,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3680,184,-0.480147,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3700,185,-0.475528,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3720,186,-0.470440,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3740,187,-0.464888,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3760,188,-0.458877,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3780,189,-0.452414,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3800,190,-0.445503,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3820,191,-0.438153,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3840,192,-0.430371,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3860,193,-0.422164,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3880,194,-0.413540,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3900,195,-0.404508,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3920,196,-0.395078,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3940,197,-0.385257,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3960,198,-0.375056,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +3980,199,-0.364484,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4000,200,-0.353553,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4020,201,-0.342274,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4040,202,-0.330656,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4060,203,-0.318712,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4080,204,-0.306454,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4100,205,-0.293893,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4120,206,-0.281042,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4140,207,-0.267913,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4160,208,-0.254521,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4180,209,-0.240877,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4200,210,-0.226995,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4220,211,-0.212890,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4240,212,-0.198574,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4260,213,-0.184062,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4280,214,-0.169369,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4300,215,-0.154508,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4320,216,-0.139496,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4340,217,-0.124345,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4360,218,-0.109072,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4380,219,-0.093691,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4400,220,-0.078217,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4420,221,-0.062667,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4440,222,-0.047054,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4460,223,-0.031395,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4480,224,-0.015705,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4500,225,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4520,226,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4540,227,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4560,228,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4580,229,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4600,230,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4620,231,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4640,232,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4660,233,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4680,234,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4700,235,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4720,236,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4740,237,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4760,238,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4780,239,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4800,240,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4820,241,0.000000,0.004712,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4840,242,0.000000,0.009423,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4860,243,0.000000,0.014132,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4880,244,0.000000,0.018837,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4900,245,0.000000,0.023538,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4920,246,0.000000,0.028232,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4940,247,0.000000,0.032920,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4960,248,0.000000,0.037600,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +4980,249,0.000000,0.042270,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5000,250,0.000000,0.046930,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5020,251,0.000000,0.051579,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5040,252,0.000000,0.056214,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5060,253,0.000000,0.060836,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5080,254,0.000000,0.065443,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5100,255,0.000000,0.070034,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5120,256,0.000000,0.074607,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5140,257,0.000000,0.079162,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5160,258,0.000000,0.083697,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5180,259,0.000000,0.088212,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5200,260,0.000000,0.092705,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5220,261,0.000000,0.097175,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5240,262,0.000000,0.101621,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5260,263,0.000000,0.106042,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5280,264,0.000000,0.110437,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5300,265,0.000000,0.114805,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5320,266,0.000000,0.119144,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5340,267,0.000000,0.123454,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5360,268,0.000000,0.127734,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5380,269,0.000000,0.131982,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5400,270,0.000000,0.136197,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5420,271,0.000000,0.140379,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5440,272,0.000000,0.144526,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5460,273,0.000000,0.148638,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5480,274,0.000000,0.152712,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5500,275,0.000000,0.156750,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5520,276,0.000000,0.160748,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5540,277,0.000000,0.164707,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5560,278,0.000000,0.168625,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5580,279,0.000000,0.172502,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5600,280,0.000000,0.176336,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5620,281,0.000000,0.180126,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5640,282,0.000000,0.183872,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5660,283,0.000000,0.187573,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5680,284,0.000000,0.191227,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5700,285,0.000000,0.194834,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5720,286,0.000000,0.198394,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5740,287,0.000000,0.201904,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5760,288,0.000000,0.205364,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5780,289,0.000000,0.208774,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5800,290,0.000000,0.212132,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5820,291,0.000000,0.215438,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5840,292,0.000000,0.218691,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5860,293,0.000000,0.221889,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5880,294,0.000000,0.225033,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5900,295,0.000000,0.228122,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5920,296,0.000000,0.231154,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5940,297,0.000000,0.234129,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5960,298,0.000000,0.237047,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +5980,299,0.000000,0.239905,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6000,300,0.000000,0.242705,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6020,301,0.000000,0.245445,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6040,302,0.000000,0.248124,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6060,303,0.000000,0.250742,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6080,304,0.000000,0.253298,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6100,305,0.000000,0.255792,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6120,306,0.000000,0.258223,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6140,307,0.000000,0.260589,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6160,308,0.000000,0.262892,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6180,309,0.000000,0.265130,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6200,310,0.000000,0.267302,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6220,311,0.000000,0.269408,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6240,312,0.000000,0.271448,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6260,313,0.000000,0.273421,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6280,314,0.000000,0.275326,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6300,315,0.000000,0.277164,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6320,316,0.000000,0.278933,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6340,317,0.000000,0.280633,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6360,318,0.000000,0.282264,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6380,319,0.000000,0.283826,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6400,320,0.000000,0.285317,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6420,321,0.000000,0.286738,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6440,322,0.000000,0.288088,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6460,323,0.000000,0.289367,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6480,324,0.000000,0.290575,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6500,325,0.000000,0.291711,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6520,326,0.000000,0.292775,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6540,327,0.000000,0.293767,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6560,328,0.000000,0.294686,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6580,329,0.000000,0.295533,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6600,330,0.000000,0.296307,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6620,331,0.000000,0.297007,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6640,332,0.000000,0.297634,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6660,333,0.000000,0.298188,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6680,334,0.000000,0.298669,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6700,335,0.000000,0.299075,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6720,336,0.000000,0.299408,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6740,337,0.000000,0.299667,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6760,338,0.000000,0.299852,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6780,339,0.000000,0.299963,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6800,340,0.000000,0.300000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6820,341,0.000000,0.299963,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6840,342,0.000000,0.299852,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6860,343,0.000000,0.299667,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6880,344,0.000000,0.299408,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6900,345,0.000000,0.299075,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6920,346,0.000000,0.298669,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6940,347,0.000000,0.298188,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6960,348,0.000000,0.297634,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +6980,349,0.000000,0.297007,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7000,350,0.000000,0.296307,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7020,351,0.000000,0.295533,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7040,352,0.000000,0.294686,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7060,353,0.000000,0.293767,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7080,354,0.000000,0.292775,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7100,355,0.000000,0.291711,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7120,356,0.000000,0.290575,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7140,357,0.000000,0.289367,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7160,358,0.000000,0.288088,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7180,359,0.000000,0.286738,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7200,360,0.000000,0.285317,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7220,361,0.000000,0.283826,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7240,362,0.000000,0.282264,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7260,363,0.000000,0.280633,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7280,364,0.000000,0.278933,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7300,365,0.000000,0.277164,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7320,366,0.000000,0.275326,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7340,367,0.000000,0.273421,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7360,368,0.000000,0.271448,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7380,369,0.000000,0.269408,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7400,370,0.000000,0.267302,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7420,371,0.000000,0.265130,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7440,372,0.000000,0.262892,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7460,373,0.000000,0.260589,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7480,374,0.000000,0.258223,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7500,375,0.000000,0.255792,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7520,376,0.000000,0.253298,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7540,377,0.000000,0.250742,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7560,378,0.000000,0.248124,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7580,379,0.000000,0.245445,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7600,380,0.000000,0.242705,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7620,381,0.000000,0.239905,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7640,382,0.000000,0.237047,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7660,383,0.000000,0.234129,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7680,384,0.000000,0.231154,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7700,385,0.000000,0.228122,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7720,386,0.000000,0.225033,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7740,387,0.000000,0.221889,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7760,388,0.000000,0.218691,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7780,389,0.000000,0.215438,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7800,390,0.000000,0.212132,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7820,391,0.000000,0.208774,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7840,392,0.000000,0.205364,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7860,393,0.000000,0.201904,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7880,394,0.000000,0.198394,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7900,395,0.000000,0.194834,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7920,396,0.000000,0.191227,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7940,397,0.000000,0.187573,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7960,398,0.000000,0.183872,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +7980,399,0.000000,0.180126,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8000,400,0.000000,0.176336,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8020,401,0.000000,0.172502,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8040,402,0.000000,0.168625,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8060,403,0.000000,0.164707,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8080,404,0.000000,0.160748,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8100,405,0.000000,0.156750,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8120,406,0.000000,0.152712,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8140,407,0.000000,0.148638,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8160,408,0.000000,0.144526,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8180,409,0.000000,0.140379,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8200,410,0.000000,0.136197,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8220,411,0.000000,0.131982,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8240,412,0.000000,0.127734,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8260,413,0.000000,0.123454,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8280,414,0.000000,0.119144,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8300,415,0.000000,0.114805,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8320,416,0.000000,0.110437,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8340,417,0.000000,0.106042,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8360,418,0.000000,0.101621,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8380,419,0.000000,0.097175,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8400,420,0.000000,0.092705,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8420,421,0.000000,0.088212,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8440,422,0.000000,0.083697,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8460,423,0.000000,0.079162,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8480,424,0.000000,0.074607,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8500,425,0.000000,0.070034,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8520,426,0.000000,0.065443,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8540,427,0.000000,0.060836,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8560,428,0.000000,0.056214,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8580,429,0.000000,0.051579,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8600,430,0.000000,0.046930,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8620,431,0.000000,0.042270,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8640,432,0.000000,0.037600,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8660,433,0.000000,0.032920,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8680,434,0.000000,0.028232,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8700,435,0.000000,0.023538,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8720,436,0.000000,0.018837,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8740,437,0.000000,0.014132,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8760,438,0.000000,0.009423,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8780,439,0.000000,0.004712,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8800,440,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8820,441,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8840,442,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8860,443,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8880,444,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8900,445,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8920,446,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8940,447,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8960,448,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +8980,449,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9000,450,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9020,451,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9040,452,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9060,453,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9080,454,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9100,455,0.000000,0.000000,-0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9120,456,0.000000,0.000000,-0.006283,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9140,457,0.000000,0.000000,-0.012564,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9160,458,0.000000,0.000000,-0.018843,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9180,459,0.000000,0.000000,-0.025116,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9200,460,0.000000,0.000000,-0.031384,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9220,461,0.000000,0.000000,-0.037643,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9240,462,0.000000,0.000000,-0.043894,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9260,463,0.000000,0.000000,-0.050133,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9280,464,0.000000,0.000000,-0.056360,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9300,465,0.000000,0.000000,-0.062574,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9320,466,0.000000,0.000000,-0.068772,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9340,467,0.000000,0.000000,-0.074953,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9360,468,0.000000,0.000000,-0.081115,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9380,469,0.000000,0.000000,-0.087257,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9400,470,0.000000,0.000000,-0.093378,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9420,471,0.000000,0.000000,-0.099476,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9440,472,0.000000,0.000000,-0.105549,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9460,473,0.000000,0.000000,-0.111596,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9480,474,0.000000,0.000000,-0.117616,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9500,475,0.000000,0.000000,-0.123607,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9520,476,0.000000,0.000000,-0.129567,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9540,477,0.000000,0.000000,-0.135495,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9560,478,0.000000,0.000000,-0.141390,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9580,479,0.000000,0.000000,-0.147250,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9600,480,0.000000,0.000000,-0.153073,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9620,481,0.000000,0.000000,-0.158859,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9640,482,0.000000,0.000000,-0.164606,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9660,483,0.000000,0.000000,-0.170312,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9680,484,0.000000,0.000000,-0.175976,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9700,485,0.000000,0.000000,-0.181596,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9720,486,0.000000,0.000000,-0.187172,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9740,487,0.000000,0.000000,-0.192701,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9760,488,0.000000,0.000000,-0.198183,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9780,489,0.000000,0.000000,-0.203617,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9800,490,0.000000,0.000000,-0.208999,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9820,491,0.000000,0.000000,-0.214331,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9840,492,0.000000,0.000000,-0.219609,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9860,493,0.000000,0.000000,-0.224833,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9880,494,0.000000,0.000000,-0.230002,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9900,495,0.000000,0.000000,-0.235114,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9920,496,0.000000,0.000000,-0.240168,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9940,497,0.000000,0.000000,-0.245163,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9960,498,0.000000,0.000000,-0.250097,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +9980,499,0.000000,0.000000,-0.254970,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10000,500,0.000000,0.000000,-0.259779,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10020,501,0.000000,0.000000,-0.264525,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10040,502,0.000000,0.000000,-0.269205,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10060,503,0.000000,0.000000,-0.273819,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10080,504,0.000000,0.000000,-0.278365,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10100,505,0.000000,0.000000,-0.282843,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10120,506,0.000000,0.000000,-0.287251,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10140,507,0.000000,0.000000,-0.291587,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10160,508,0.000000,0.000000,-0.295852,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10180,509,0.000000,0.000000,-0.300044,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10200,510,0.000000,0.000000,-0.304162,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10220,511,0.000000,0.000000,-0.308205,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10240,512,0.000000,0.000000,-0.312172,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10260,513,0.000000,0.000000,-0.316062,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10280,514,0.000000,0.000000,-0.319874,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10300,515,0.000000,0.000000,-0.323607,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10320,516,0.000000,0.000000,-0.327260,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10340,517,0.000000,0.000000,-0.330832,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10360,518,0.000000,0.000000,-0.334323,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10380,519,0.000000,0.000000,-0.337731,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10400,520,0.000000,0.000000,-0.341056,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10420,521,0.000000,0.000000,-0.344297,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10440,522,0.000000,0.000000,-0.347453,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10460,523,0.000000,0.000000,-0.350523,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10480,524,0.000000,0.000000,-0.353506,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10500,525,0.000000,0.000000,-0.356403,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10520,526,0.000000,0.000000,-0.359211,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10540,527,0.000000,0.000000,-0.361931,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10560,528,0.000000,0.000000,-0.364561,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10580,529,0.000000,0.000000,-0.367102,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10600,530,0.000000,0.000000,-0.369552,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10620,531,0.000000,0.000000,-0.371911,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10640,532,0.000000,0.000000,-0.374178,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10660,533,0.000000,0.000000,-0.376352,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10680,534,0.000000,0.000000,-0.378434,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10700,535,0.000000,0.000000,-0.380423,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10720,536,0.000000,0.000000,-0.382317,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10740,537,0.000000,0.000000,-0.384117,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10760,538,0.000000,0.000000,-0.385823,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10780,539,0.000000,0.000000,-0.387433,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10800,540,0.000000,0.000000,-0.388948,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10820,541,0.000000,0.000000,-0.390367,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10840,542,0.000000,0.000000,-0.391689,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10860,543,0.000000,0.000000,-0.392915,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10880,544,0.000000,0.000000,-0.394044,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10900,545,0.000000,0.000000,-0.395075,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10920,546,0.000000,0.000000,-0.396009,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10940,547,0.000000,0.000000,-0.396846,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10960,548,0.000000,0.000000,-0.397584,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +10980,549,0.000000,0.000000,-0.398225,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11000,550,0.000000,0.000000,-0.398767,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11020,551,0.000000,0.000000,-0.399211,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11040,552,0.000000,0.000000,-0.399556,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11060,553,0.000000,0.000000,-0.399803,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11080,554,0.000000,0.000000,-0.399951,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11100,555,0.000000,0.000000,-0.400000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11120,556,0.000000,0.000000,-0.399951,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11140,557,0.000000,0.000000,-0.399803,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11160,558,0.000000,0.000000,-0.399556,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11180,559,0.000000,0.000000,-0.399211,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11200,560,0.000000,0.000000,-0.398767,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11220,561,0.000000,0.000000,-0.398225,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11240,562,0.000000,0.000000,-0.397584,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11260,563,0.000000,0.000000,-0.396846,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11280,564,0.000000,0.000000,-0.396009,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11300,565,0.000000,0.000000,-0.395075,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11320,566,0.000000,0.000000,-0.394044,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11340,567,0.000000,0.000000,-0.392915,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11360,568,0.000000,0.000000,-0.391689,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11380,569,0.000000,0.000000,-0.390367,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11400,570,0.000000,0.000000,-0.388948,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11420,571,0.000000,0.000000,-0.387433,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11440,572,0.000000,0.000000,-0.385823,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11460,573,0.000000,0.000000,-0.384117,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11480,574,0.000000,0.000000,-0.382317,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11500,575,0.000000,0.000000,-0.380423,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11520,576,0.000000,0.000000,-0.378434,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11540,577,0.000000,0.000000,-0.376352,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11560,578,0.000000,0.000000,-0.374178,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11580,579,0.000000,0.000000,-0.371911,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11600,580,0.000000,0.000000,-0.369552,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11620,581,0.000000,0.000000,-0.367102,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11640,582,0.000000,0.000000,-0.364561,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11660,583,0.000000,0.000000,-0.361931,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11680,584,0.000000,0.000000,-0.359211,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11700,585,0.000000,0.000000,-0.356403,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11720,586,0.000000,0.000000,-0.353506,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11740,587,0.000000,0.000000,-0.350523,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11760,588,0.000000,0.000000,-0.347453,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11780,589,0.000000,0.000000,-0.344297,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11800,590,0.000000,0.000000,-0.341056,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11820,591,0.000000,0.000000,-0.337731,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11840,592,0.000000,0.000000,-0.334323,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11860,593,0.000000,0.000000,-0.330832,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11880,594,0.000000,0.000000,-0.327260,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11900,595,0.000000,0.000000,-0.323607,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11920,596,0.000000,0.000000,-0.319874,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11940,597,0.000000,0.000000,-0.316062,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11960,598,0.000000,0.000000,-0.312172,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +11980,599,0.000000,0.000000,-0.308205,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12000,600,0.000000,0.000000,-0.304162,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12020,601,0.000000,0.000000,-0.300044,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12040,602,0.000000,0.000000,-0.295852,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12060,603,0.000000,0.000000,-0.291587,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12080,604,0.000000,0.000000,-0.287251,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12100,605,0.000000,0.000000,-0.282843,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12120,606,0.000000,0.000000,-0.278365,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12140,607,0.000000,0.000000,-0.273819,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12160,608,0.000000,0.000000,-0.269205,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12180,609,0.000000,0.000000,-0.264525,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12200,610,0.000000,0.000000,-0.259779,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12220,611,0.000000,0.000000,-0.254970,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12240,612,0.000000,0.000000,-0.250097,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12260,613,0.000000,0.000000,-0.245163,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12280,614,0.000000,0.000000,-0.240168,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12300,615,0.000000,0.000000,-0.235114,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12320,616,0.000000,0.000000,-0.230002,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12340,617,0.000000,0.000000,-0.224833,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12360,618,0.000000,0.000000,-0.219609,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12380,619,0.000000,0.000000,-0.214331,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12400,620,0.000000,0.000000,-0.208999,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12420,621,0.000000,0.000000,-0.203617,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12440,622,0.000000,0.000000,-0.198183,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12460,623,0.000000,0.000000,-0.192701,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12480,624,0.000000,0.000000,-0.187172,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12500,625,0.000000,0.000000,-0.181596,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12520,626,0.000000,0.000000,-0.175976,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12540,627,0.000000,0.000000,-0.170312,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12560,628,0.000000,0.000000,-0.164606,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12580,629,0.000000,0.000000,-0.158859,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12600,630,0.000000,0.000000,-0.153073,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12620,631,0.000000,0.000000,-0.147250,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12640,632,0.000000,0.000000,-0.141390,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12660,633,0.000000,0.000000,-0.135495,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12680,634,0.000000,0.000000,-0.129567,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12700,635,0.000000,0.000000,-0.123607,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12720,636,0.000000,0.000000,-0.117616,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12740,637,0.000000,0.000000,-0.111596,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12760,638,0.000000,0.000000,-0.105549,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12780,639,0.000000,0.000000,-0.099476,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12800,640,0.000000,0.000000,-0.093378,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12820,641,0.000000,0.000000,-0.087257,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12840,642,0.000000,0.000000,-0.081115,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12860,643,0.000000,0.000000,-0.074953,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12880,644,0.000000,0.000000,-0.068772,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12900,645,0.000000,0.000000,-0.062574,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12920,646,0.000000,0.000000,-0.056360,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12940,647,0.000000,0.000000,-0.050133,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12960,648,0.000000,0.000000,-0.043894,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +12980,649,0.000000,0.000000,-0.037643,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13000,650,0.000000,0.000000,-0.031384,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13020,651,0.000000,0.000000,-0.025116,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13040,652,0.000000,0.000000,-0.018843,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13060,653,0.000000,0.000000,-0.012564,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13080,654,0.000000,0.000000,-0.006283,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13100,655,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13120,656,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13140,657,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13160,658,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13180,659,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13200,660,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13220,661,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13240,662,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13260,663,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13280,664,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13300,665,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13320,666,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13340,667,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13360,668,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13380,669,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13400,670,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13420,671,0.000000,0.000000,0.000000,0.015705,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13440,672,0.000000,0.000000,0.000000,0.031395,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13460,673,0.000000,0.000000,0.000000,0.047054,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13480,674,0.000000,0.000000,0.000000,0.062667,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13500,675,0.000000,0.000000,0.000000,0.078217,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13520,676,0.000000,0.000000,0.000000,0.093691,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13540,677,0.000000,0.000000,0.000000,0.109072,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13560,678,0.000000,0.000000,0.000000,0.124345,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13580,679,0.000000,0.000000,0.000000,0.139496,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13600,680,0.000000,0.000000,0.000000,0.154508,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13620,681,0.000000,0.000000,0.000000,0.169369,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13640,682,0.000000,0.000000,0.000000,0.184062,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13660,683,0.000000,0.000000,0.000000,0.198574,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13680,684,0.000000,0.000000,0.000000,0.212890,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13700,685,0.000000,0.000000,0.000000,0.226995,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13720,686,0.000000,0.000000,0.000000,0.240877,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13740,687,0.000000,0.000000,0.000000,0.254521,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13760,688,0.000000,0.000000,0.000000,0.267913,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13780,689,0.000000,0.000000,0.000000,0.281042,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13800,690,0.000000,0.000000,0.000000,0.293893,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13820,691,0.000000,0.000000,0.000000,0.306454,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13840,692,0.000000,0.000000,0.000000,0.318712,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13860,693,0.000000,0.000000,0.000000,0.330656,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13880,694,0.000000,0.000000,0.000000,0.342274,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13900,695,0.000000,0.000000,0.000000,0.353553,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13920,696,0.000000,0.000000,0.000000,0.364484,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13940,697,0.000000,0.000000,0.000000,0.375056,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13960,698,0.000000,0.000000,0.000000,0.385257,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +13980,699,0.000000,0.000000,0.000000,0.395078,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14000,700,0.000000,0.000000,0.000000,0.404508,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14020,701,0.000000,0.000000,0.000000,0.413540,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14040,702,0.000000,0.000000,0.000000,0.422164,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14060,703,0.000000,0.000000,0.000000,0.430371,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14080,704,0.000000,0.000000,0.000000,0.438153,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14100,705,0.000000,0.000000,0.000000,0.445503,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14120,706,0.000000,0.000000,0.000000,0.452414,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14140,707,0.000000,0.000000,0.000000,0.458877,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14160,708,0.000000,0.000000,0.000000,0.464888,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14180,709,0.000000,0.000000,0.000000,0.470440,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14200,710,0.000000,0.000000,0.000000,0.475528,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14220,711,0.000000,0.000000,0.000000,0.480147,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14240,712,0.000000,0.000000,0.000000,0.484292,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14260,713,0.000000,0.000000,0.000000,0.487958,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14280,714,0.000000,0.000000,0.000000,0.491144,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14300,715,0.000000,0.000000,0.000000,0.493844,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14320,716,0.000000,0.000000,0.000000,0.496057,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14340,717,0.000000,0.000000,0.000000,0.497781,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14360,718,0.000000,0.000000,0.000000,0.499013,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14380,719,0.000000,0.000000,0.000000,0.499753,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14400,720,0.000000,0.000000,0.000000,0.500000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14420,721,0.000000,0.000000,0.000000,0.499753,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14440,722,0.000000,0.000000,0.000000,0.499013,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14460,723,0.000000,0.000000,0.000000,0.497781,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14480,724,0.000000,0.000000,0.000000,0.496057,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14500,725,0.000000,0.000000,0.000000,0.493844,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14520,726,0.000000,0.000000,0.000000,0.491144,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14540,727,0.000000,0.000000,0.000000,0.487958,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14560,728,0.000000,0.000000,0.000000,0.484292,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14580,729,0.000000,0.000000,0.000000,0.480147,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14600,730,0.000000,0.000000,0.000000,0.475528,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14620,731,0.000000,0.000000,0.000000,0.470440,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14640,732,0.000000,0.000000,0.000000,0.464888,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14660,733,0.000000,0.000000,0.000000,0.458877,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14680,734,0.000000,0.000000,0.000000,0.452414,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14700,735,0.000000,0.000000,0.000000,0.445503,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14720,736,0.000000,0.000000,0.000000,0.438153,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14740,737,0.000000,0.000000,0.000000,0.430371,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14760,738,0.000000,0.000000,0.000000,0.422164,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14780,739,0.000000,0.000000,0.000000,0.413540,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14800,740,0.000000,0.000000,0.000000,0.404508,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14820,741,0.000000,0.000000,0.000000,0.395078,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14840,742,0.000000,0.000000,0.000000,0.385257,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14860,743,0.000000,0.000000,0.000000,0.375056,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14880,744,0.000000,0.000000,0.000000,0.364484,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14900,745,0.000000,0.000000,0.000000,0.353553,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14920,746,0.000000,0.000000,0.000000,0.342274,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14940,747,0.000000,0.000000,0.000000,0.330656,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14960,748,0.000000,0.000000,0.000000,0.318712,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +14980,749,0.000000,0.000000,0.000000,0.306454,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15000,750,0.000000,0.000000,0.000000,0.293893,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15020,751,0.000000,0.000000,0.000000,0.281042,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15040,752,0.000000,0.000000,0.000000,0.267913,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15060,753,0.000000,0.000000,0.000000,0.254521,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15080,754,0.000000,0.000000,0.000000,0.240877,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15100,755,0.000000,0.000000,0.000000,0.226995,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15120,756,0.000000,0.000000,0.000000,0.212890,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15140,757,0.000000,0.000000,0.000000,0.198574,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15160,758,0.000000,0.000000,0.000000,0.184062,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15180,759,0.000000,0.000000,0.000000,0.169369,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15200,760,0.000000,0.000000,0.000000,0.154508,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15220,761,0.000000,0.000000,0.000000,0.139496,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15240,762,0.000000,0.000000,0.000000,0.124345,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15260,763,0.000000,0.000000,0.000000,0.109072,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15280,764,0.000000,0.000000,0.000000,0.093691,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15300,765,0.000000,0.000000,0.000000,0.078217,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15320,766,0.000000,0.000000,0.000000,0.062667,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15340,767,0.000000,0.000000,0.000000,0.047054,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15360,768,0.000000,0.000000,0.000000,0.031395,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15380,769,0.000000,0.000000,0.000000,0.015705,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15400,770,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15420,771,0.000000,0.000000,0.000000,-0.015705,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15440,772,0.000000,0.000000,0.000000,-0.031395,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15460,773,0.000000,0.000000,0.000000,-0.047054,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15480,774,0.000000,0.000000,0.000000,-0.062667,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15500,775,0.000000,0.000000,0.000000,-0.078217,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15520,776,0.000000,0.000000,0.000000,-0.093691,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15540,777,0.000000,0.000000,0.000000,-0.109072,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15560,778,0.000000,0.000000,0.000000,-0.124345,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15580,779,0.000000,0.000000,0.000000,-0.139496,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15600,780,0.000000,0.000000,0.000000,-0.154508,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15620,781,0.000000,0.000000,0.000000,-0.169369,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15640,782,0.000000,0.000000,0.000000,-0.184062,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15660,783,0.000000,0.000000,0.000000,-0.198574,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15680,784,0.000000,0.000000,0.000000,-0.212890,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15700,785,0.000000,0.000000,0.000000,-0.226995,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15720,786,0.000000,0.000000,0.000000,-0.240877,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15740,787,0.000000,0.000000,0.000000,-0.254521,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15760,788,0.000000,0.000000,0.000000,-0.267913,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15780,789,0.000000,0.000000,0.000000,-0.281042,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15800,790,0.000000,0.000000,0.000000,-0.293893,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15820,791,0.000000,0.000000,0.000000,-0.306454,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15840,792,0.000000,0.000000,0.000000,-0.318712,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15860,793,0.000000,0.000000,0.000000,-0.330656,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15880,794,0.000000,0.000000,0.000000,-0.342274,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15900,795,0.000000,0.000000,0.000000,-0.353553,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15920,796,0.000000,0.000000,0.000000,-0.364484,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15940,797,0.000000,0.000000,0.000000,-0.375056,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15960,798,0.000000,0.000000,0.000000,-0.385257,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +15980,799,0.000000,0.000000,0.000000,-0.395078,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16000,800,0.000000,0.000000,0.000000,-0.404508,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16020,801,0.000000,0.000000,0.000000,-0.413540,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16040,802,0.000000,0.000000,0.000000,-0.422164,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16060,803,0.000000,0.000000,0.000000,-0.430371,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16080,804,0.000000,0.000000,0.000000,-0.438153,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16100,805,0.000000,0.000000,0.000000,-0.445503,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16120,806,0.000000,0.000000,0.000000,-0.452414,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16140,807,0.000000,0.000000,0.000000,-0.458877,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16160,808,0.000000,0.000000,0.000000,-0.464888,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16180,809,0.000000,0.000000,0.000000,-0.470440,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16200,810,0.000000,0.000000,0.000000,-0.475528,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16220,811,0.000000,0.000000,0.000000,-0.480147,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16240,812,0.000000,0.000000,0.000000,-0.484292,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16260,813,0.000000,0.000000,0.000000,-0.487958,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16280,814,0.000000,0.000000,0.000000,-0.491144,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16300,815,0.000000,0.000000,0.000000,-0.493844,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16320,816,0.000000,0.000000,0.000000,-0.496057,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16340,817,0.000000,0.000000,0.000000,-0.497781,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16360,818,0.000000,0.000000,0.000000,-0.499013,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16380,819,0.000000,0.000000,0.000000,-0.499753,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16400,820,0.000000,0.000000,0.000000,-0.500000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16420,821,0.000000,0.000000,0.000000,-0.499753,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16440,822,0.000000,0.000000,0.000000,-0.499013,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16460,823,0.000000,0.000000,0.000000,-0.497781,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16480,824,0.000000,0.000000,0.000000,-0.496057,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16500,825,0.000000,0.000000,0.000000,-0.493844,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16520,826,0.000000,0.000000,0.000000,-0.491144,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16540,827,0.000000,0.000000,0.000000,-0.487958,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16560,828,0.000000,0.000000,0.000000,-0.484292,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16580,829,0.000000,0.000000,0.000000,-0.480147,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16600,830,0.000000,0.000000,0.000000,-0.475528,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16620,831,0.000000,0.000000,0.000000,-0.470440,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16640,832,0.000000,0.000000,0.000000,-0.464888,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16660,833,0.000000,0.000000,0.000000,-0.458877,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16680,834,0.000000,0.000000,0.000000,-0.452414,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16700,835,0.000000,0.000000,0.000000,-0.445503,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16720,836,0.000000,0.000000,0.000000,-0.438153,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16740,837,0.000000,0.000000,0.000000,-0.430371,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16760,838,0.000000,0.000000,0.000000,-0.422164,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16780,839,0.000000,0.000000,0.000000,-0.413540,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16800,840,0.000000,0.000000,0.000000,-0.404508,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16820,841,0.000000,0.000000,0.000000,-0.395078,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16840,842,0.000000,0.000000,0.000000,-0.385257,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16860,843,0.000000,0.000000,0.000000,-0.375056,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16880,844,0.000000,0.000000,0.000000,-0.364484,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16900,845,0.000000,0.000000,0.000000,-0.353553,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16920,846,0.000000,0.000000,0.000000,-0.342274,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16940,847,0.000000,0.000000,0.000000,-0.330656,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16960,848,0.000000,0.000000,0.000000,-0.318712,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +16980,849,0.000000,0.000000,0.000000,-0.306454,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17000,850,0.000000,0.000000,0.000000,-0.293893,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17020,851,0.000000,0.000000,0.000000,-0.281042,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17040,852,0.000000,0.000000,0.000000,-0.267913,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17060,853,0.000000,0.000000,0.000000,-0.254521,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17080,854,0.000000,0.000000,0.000000,-0.240877,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17100,855,0.000000,0.000000,0.000000,-0.226995,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17120,856,0.000000,0.000000,0.000000,-0.212890,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17140,857,0.000000,0.000000,0.000000,-0.198574,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17160,858,0.000000,0.000000,0.000000,-0.184062,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17180,859,0.000000,0.000000,0.000000,-0.169369,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17200,860,0.000000,0.000000,0.000000,-0.154508,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17220,861,0.000000,0.000000,0.000000,-0.139496,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17240,862,0.000000,0.000000,0.000000,-0.124345,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17260,863,0.000000,0.000000,0.000000,-0.109072,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17280,864,0.000000,0.000000,0.000000,-0.093691,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17300,865,0.000000,0.000000,0.000000,-0.078217,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17320,866,0.000000,0.000000,0.000000,-0.062667,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17340,867,0.000000,0.000000,0.000000,-0.047054,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17360,868,0.000000,0.000000,0.000000,-0.031395,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17380,869,0.000000,0.000000,0.000000,-0.015705,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17400,870,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17420,871,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17440,872,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17460,873,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17480,874,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17500,875,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17520,876,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17540,877,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17560,878,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17580,879,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17600,880,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17620,881,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17640,882,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17660,883,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17680,884,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17700,885,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17720,886,0.000000,0.000000,0.000000,0.000000,0.025129,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17740,887,0.000000,0.000000,0.000000,0.000000,0.050232,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17760,888,0.000000,0.000000,0.000000,0.000000,0.075287,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17780,889,0.000000,0.000000,0.000000,0.000000,0.100267,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17800,890,0.000000,0.000000,0.000000,0.000000,0.125148,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17820,891,0.000000,0.000000,0.000000,0.000000,0.149905,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17840,892,0.000000,0.000000,0.000000,0.000000,0.174515,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17860,893,0.000000,0.000000,0.000000,0.000000,0.198952,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17880,894,0.000000,0.000000,0.000000,0.000000,0.223193,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17900,895,0.000000,0.000000,0.000000,0.000000,0.247214,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17920,896,0.000000,0.000000,0.000000,0.000000,0.270990,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17940,897,0.000000,0.000000,0.000000,0.000000,0.294500,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17960,898,0.000000,0.000000,0.000000,0.000000,0.317718,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +17980,899,0.000000,0.000000,0.000000,0.000000,0.340623,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18000,900,0.000000,0.000000,0.000000,0.000000,0.363192,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18020,901,0.000000,0.000000,0.000000,0.000000,0.385403,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18040,902,0.000000,0.000000,0.000000,0.000000,0.407233,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18060,903,0.000000,0.000000,0.000000,0.000000,0.428661,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18080,904,0.000000,0.000000,0.000000,0.000000,0.449667,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18100,905,0.000000,0.000000,0.000000,0.000000,0.470228,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18120,906,0.000000,0.000000,0.000000,0.000000,0.490326,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18140,907,0.000000,0.000000,0.000000,0.000000,0.509939,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18160,908,0.000000,0.000000,0.000000,0.000000,0.529049,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18180,909,0.000000,0.000000,0.000000,0.000000,0.547638,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18200,910,0.000000,0.000000,0.000000,0.000000,0.565685,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18220,911,0.000000,0.000000,0.000000,0.000000,0.583175,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18240,912,0.000000,0.000000,0.000000,0.000000,0.600089,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18260,913,0.000000,0.000000,0.000000,0.000000,0.616411,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18280,914,0.000000,0.000000,0.000000,0.000000,0.632124,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18300,915,0.000000,0.000000,0.000000,0.000000,0.647214,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18320,916,0.000000,0.000000,0.000000,0.000000,0.661664,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18340,917,0.000000,0.000000,0.000000,0.000000,0.675462,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18360,918,0.000000,0.000000,0.000000,0.000000,0.688594,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18380,919,0.000000,0.000000,0.000000,0.000000,0.701045,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18400,920,0.000000,0.000000,0.000000,0.000000,0.712805,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18420,921,0.000000,0.000000,0.000000,0.000000,0.723862,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18440,922,0.000000,0.000000,0.000000,0.000000,0.734204,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18460,923,0.000000,0.000000,0.000000,0.000000,0.743821,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18480,924,0.000000,0.000000,0.000000,0.000000,0.752705,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18500,925,0.000000,0.000000,0.000000,0.000000,0.760845,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18520,926,0.000000,0.000000,0.000000,0.000000,0.768235,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18540,927,0.000000,0.000000,0.000000,0.000000,0.774867,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18560,928,0.000000,0.000000,0.000000,0.000000,0.780733,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18580,929,0.000000,0.000000,0.000000,0.000000,0.785830,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18600,930,0.000000,0.000000,0.000000,0.000000,0.790151,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18620,931,0.000000,0.000000,0.000000,0.000000,0.793692,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18640,932,0.000000,0.000000,0.000000,0.000000,0.796450,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18660,933,0.000000,0.000000,0.000000,0.000000,0.798421,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18680,934,0.000000,0.000000,0.000000,0.000000,0.799605,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18700,935,0.000000,0.000000,0.000000,0.000000,0.800000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18720,936,0.000000,0.000000,0.000000,0.000000,0.799605,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18740,937,0.000000,0.000000,0.000000,0.000000,0.798421,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18760,938,0.000000,0.000000,0.000000,0.000000,0.796450,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18780,939,0.000000,0.000000,0.000000,0.000000,0.793692,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18800,940,0.000000,0.000000,0.000000,0.000000,0.790151,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18820,941,0.000000,0.000000,0.000000,0.000000,0.785830,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18840,942,0.000000,0.000000,0.000000,0.000000,0.780733,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18860,943,0.000000,0.000000,0.000000,0.000000,0.774867,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18880,944,0.000000,0.000000,0.000000,0.000000,0.768235,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18900,945,0.000000,0.000000,0.000000,0.000000,0.760845,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18920,946,0.000000,0.000000,0.000000,0.000000,0.752705,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18940,947,0.000000,0.000000,0.000000,0.000000,0.743821,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18960,948,0.000000,0.000000,0.000000,0.000000,0.734204,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +18980,949,0.000000,0.000000,0.000000,0.000000,0.723862,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19000,950,0.000000,0.000000,0.000000,0.000000,0.712805,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19020,951,0.000000,0.000000,0.000000,0.000000,0.701045,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19040,952,0.000000,0.000000,0.000000,0.000000,0.688594,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19060,953,0.000000,0.000000,0.000000,0.000000,0.675462,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19080,954,0.000000,0.000000,0.000000,0.000000,0.661664,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19100,955,0.000000,0.000000,0.000000,0.000000,0.647214,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19120,956,0.000000,0.000000,0.000000,0.000000,0.632124,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19140,957,0.000000,0.000000,0.000000,0.000000,0.616411,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19160,958,0.000000,0.000000,0.000000,0.000000,0.600089,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19180,959,0.000000,0.000000,0.000000,0.000000,0.583175,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19200,960,0.000000,0.000000,0.000000,0.000000,0.565685,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19220,961,0.000000,0.000000,0.000000,0.000000,0.547638,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19240,962,0.000000,0.000000,0.000000,0.000000,0.529049,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19260,963,0.000000,0.000000,0.000000,0.000000,0.509939,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19280,964,0.000000,0.000000,0.000000,0.000000,0.490326,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19300,965,0.000000,0.000000,0.000000,0.000000,0.470228,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19320,966,0.000000,0.000000,0.000000,0.000000,0.449667,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19340,967,0.000000,0.000000,0.000000,0.000000,0.428661,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19360,968,0.000000,0.000000,0.000000,0.000000,0.407233,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19380,969,0.000000,0.000000,0.000000,0.000000,0.385403,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19400,970,0.000000,0.000000,0.000000,0.000000,0.363192,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19420,971,0.000000,0.000000,0.000000,0.000000,0.340623,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19440,972,0.000000,0.000000,0.000000,0.000000,0.317718,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19460,973,0.000000,0.000000,0.000000,0.000000,0.294500,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19480,974,0.000000,0.000000,0.000000,0.000000,0.270990,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19500,975,0.000000,0.000000,0.000000,0.000000,0.247214,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19520,976,0.000000,0.000000,0.000000,0.000000,0.223193,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19540,977,0.000000,0.000000,0.000000,0.000000,0.198952,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19560,978,0.000000,0.000000,0.000000,0.000000,0.174515,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19580,979,0.000000,0.000000,0.000000,0.000000,0.149905,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19600,980,0.000000,0.000000,0.000000,0.000000,0.125148,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19620,981,0.000000,0.000000,0.000000,0.000000,0.100267,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19640,982,0.000000,0.000000,0.000000,0.000000,0.075287,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19660,983,0.000000,0.000000,0.000000,0.000000,0.050232,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19680,984,0.000000,0.000000,0.000000,0.000000,0.025129,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19700,985,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19720,986,0.000000,0.000000,0.000000,0.000000,-0.025129,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19740,987,0.000000,0.000000,0.000000,0.000000,-0.050232,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19760,988,0.000000,0.000000,0.000000,0.000000,-0.075287,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19780,989,0.000000,0.000000,0.000000,0.000000,-0.100267,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19800,990,0.000000,0.000000,0.000000,0.000000,-0.125148,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19820,991,0.000000,0.000000,0.000000,0.000000,-0.149905,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19840,992,0.000000,0.000000,0.000000,0.000000,-0.174515,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19860,993,0.000000,0.000000,0.000000,0.000000,-0.198952,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19880,994,0.000000,0.000000,0.000000,0.000000,-0.223193,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19900,995,0.000000,0.000000,0.000000,0.000000,-0.247214,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19920,996,0.000000,0.000000,0.000000,0.000000,-0.270990,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19940,997,0.000000,0.000000,0.000000,0.000000,-0.294500,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19960,998,0.000000,0.000000,0.000000,0.000000,-0.317718,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +19980,999,0.000000,0.000000,0.000000,0.000000,-0.340623,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20000,1000,0.000000,0.000000,0.000000,0.000000,-0.363192,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20020,1001,0.000000,0.000000,0.000000,0.000000,-0.385403,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20040,1002,0.000000,0.000000,0.000000,0.000000,-0.407233,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20060,1003,0.000000,0.000000,0.000000,0.000000,-0.428661,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20080,1004,0.000000,0.000000,0.000000,0.000000,-0.449667,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20100,1005,0.000000,0.000000,0.000000,0.000000,-0.470228,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20120,1006,0.000000,0.000000,0.000000,0.000000,-0.490326,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20140,1007,0.000000,0.000000,0.000000,0.000000,-0.509939,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20160,1008,0.000000,0.000000,0.000000,0.000000,-0.529049,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20180,1009,0.000000,0.000000,0.000000,0.000000,-0.547638,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20200,1010,0.000000,0.000000,0.000000,0.000000,-0.565685,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20220,1011,0.000000,0.000000,0.000000,0.000000,-0.583175,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20240,1012,0.000000,0.000000,0.000000,0.000000,-0.600089,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20260,1013,0.000000,0.000000,0.000000,0.000000,-0.616411,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20280,1014,0.000000,0.000000,0.000000,0.000000,-0.632124,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20300,1015,0.000000,0.000000,0.000000,0.000000,-0.647214,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20320,1016,0.000000,0.000000,0.000000,0.000000,-0.661664,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20340,1017,0.000000,0.000000,0.000000,0.000000,-0.675462,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20360,1018,0.000000,0.000000,0.000000,0.000000,-0.688594,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20380,1019,0.000000,0.000000,0.000000,0.000000,-0.701045,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20400,1020,0.000000,0.000000,0.000000,0.000000,-0.712805,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20420,1021,0.000000,0.000000,0.000000,0.000000,-0.723862,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20440,1022,0.000000,0.000000,0.000000,0.000000,-0.734204,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20460,1023,0.000000,0.000000,0.000000,0.000000,-0.743821,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20480,1024,0.000000,0.000000,0.000000,0.000000,-0.752705,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20500,1025,0.000000,0.000000,0.000000,0.000000,-0.760845,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20520,1026,0.000000,0.000000,0.000000,0.000000,-0.768235,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20540,1027,0.000000,0.000000,0.000000,0.000000,-0.774867,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20560,1028,0.000000,0.000000,0.000000,0.000000,-0.780733,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20580,1029,0.000000,0.000000,0.000000,0.000000,-0.785830,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20600,1030,0.000000,0.000000,0.000000,0.000000,-0.790151,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20620,1031,0.000000,0.000000,0.000000,0.000000,-0.793692,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20640,1032,0.000000,0.000000,0.000000,0.000000,-0.796450,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20660,1033,0.000000,0.000000,0.000000,0.000000,-0.798421,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20680,1034,0.000000,0.000000,0.000000,0.000000,-0.799605,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20700,1035,0.000000,0.000000,0.000000,0.000000,-0.800000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20720,1036,0.000000,0.000000,0.000000,0.000000,-0.799605,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20740,1037,0.000000,0.000000,0.000000,0.000000,-0.798421,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20760,1038,0.000000,0.000000,0.000000,0.000000,-0.796450,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20780,1039,0.000000,0.000000,0.000000,0.000000,-0.793692,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20800,1040,0.000000,0.000000,0.000000,0.000000,-0.790151,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20820,1041,0.000000,0.000000,0.000000,0.000000,-0.785830,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20840,1042,0.000000,0.000000,0.000000,0.000000,-0.780733,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20860,1043,0.000000,0.000000,0.000000,0.000000,-0.774867,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20880,1044,0.000000,0.000000,0.000000,0.000000,-0.768235,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20900,1045,0.000000,0.000000,0.000000,0.000000,-0.760845,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20920,1046,0.000000,0.000000,0.000000,0.000000,-0.752705,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20940,1047,0.000000,0.000000,0.000000,0.000000,-0.743821,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20960,1048,0.000000,0.000000,0.000000,0.000000,-0.734204,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +20980,1049,0.000000,0.000000,0.000000,0.000000,-0.723862,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21000,1050,0.000000,0.000000,0.000000,0.000000,-0.712805,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21020,1051,0.000000,0.000000,0.000000,0.000000,-0.701045,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21040,1052,0.000000,0.000000,0.000000,0.000000,-0.688594,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21060,1053,0.000000,0.000000,0.000000,0.000000,-0.675462,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21080,1054,0.000000,0.000000,0.000000,0.000000,-0.661664,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21100,1055,0.000000,0.000000,0.000000,0.000000,-0.647214,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21120,1056,0.000000,0.000000,0.000000,0.000000,-0.632124,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21140,1057,0.000000,0.000000,0.000000,0.000000,-0.616411,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21160,1058,0.000000,0.000000,0.000000,0.000000,-0.600089,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21180,1059,0.000000,0.000000,0.000000,0.000000,-0.583175,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21200,1060,0.000000,0.000000,0.000000,0.000000,-0.565685,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21220,1061,0.000000,0.000000,0.000000,0.000000,-0.547638,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21240,1062,0.000000,0.000000,0.000000,0.000000,-0.529049,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21260,1063,0.000000,0.000000,0.000000,0.000000,-0.509939,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21280,1064,0.000000,0.000000,0.000000,0.000000,-0.490326,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21300,1065,0.000000,0.000000,0.000000,0.000000,-0.470228,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21320,1066,0.000000,0.000000,0.000000,0.000000,-0.449667,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21340,1067,0.000000,0.000000,0.000000,0.000000,-0.428661,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21360,1068,0.000000,0.000000,0.000000,0.000000,-0.407233,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21380,1069,0.000000,0.000000,0.000000,0.000000,-0.385403,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21400,1070,0.000000,0.000000,0.000000,0.000000,-0.363192,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21420,1071,0.000000,0.000000,0.000000,0.000000,-0.340623,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21440,1072,0.000000,0.000000,0.000000,0.000000,-0.317718,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21460,1073,0.000000,0.000000,0.000000,0.000000,-0.294500,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21480,1074,0.000000,0.000000,0.000000,0.000000,-0.270990,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21500,1075,0.000000,0.000000,0.000000,0.000000,-0.247214,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21520,1076,0.000000,0.000000,0.000000,0.000000,-0.223193,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21540,1077,0.000000,0.000000,0.000000,0.000000,-0.198952,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21560,1078,0.000000,0.000000,0.000000,0.000000,-0.174515,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21580,1079,0.000000,0.000000,0.000000,0.000000,-0.149905,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21600,1080,0.000000,0.000000,0.000000,0.000000,-0.125148,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21620,1081,0.000000,0.000000,0.000000,0.000000,-0.100267,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21640,1082,0.000000,0.000000,0.000000,0.000000,-0.075287,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21660,1083,0.000000,0.000000,0.000000,0.000000,-0.050232,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21680,1084,0.000000,0.000000,0.000000,0.000000,-0.025129,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21700,1085,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21720,1086,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21740,1087,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21760,1088,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21780,1089,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21800,1090,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21820,1091,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21840,1092,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21860,1093,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21880,1094,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21900,1095,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21920,1096,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21940,1097,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21960,1098,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +21980,1099,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22000,1100,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22020,1101,0.000000,0.000000,0.000000,0.000000,0.000000,0.123559,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22040,1102,0.000000,0.000000,0.000000,0.000000,0.000000,0.147102,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22060,1103,0.000000,0.000000,0.000000,0.000000,0.000000,0.170613,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22080,1104,0.000000,0.000000,0.000000,0.000000,0.000000,0.194076,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22100,1105,0.000000,0.000000,0.000000,0.000000,0.000000,0.217474,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22120,1106,0.000000,0.000000,0.000000,0.000000,0.000000,0.240791,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22140,1107,0.000000,0.000000,0.000000,0.000000,0.000000,0.264012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22160,1108,0.000000,0.000000,0.000000,0.000000,0.000000,0.287121,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22180,1109,0.000000,0.000000,0.000000,0.000000,0.000000,0.310101,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22200,1110,0.000000,0.000000,0.000000,0.000000,0.000000,0.332937,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22220,1111,0.000000,0.000000,0.000000,0.000000,0.000000,0.355614,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22240,1112,0.000000,0.000000,0.000000,0.000000,0.000000,0.378115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22260,1113,0.000000,0.000000,0.000000,0.000000,0.000000,0.400426,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22280,1114,0.000000,0.000000,0.000000,0.000000,0.000000,0.422531,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22300,1115,0.000000,0.000000,0.000000,0.000000,0.000000,0.444415,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22320,1116,0.000000,0.000000,0.000000,0.000000,0.000000,0.466063,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22340,1117,0.000000,0.000000,0.000000,0.000000,0.000000,0.487460,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22360,1118,0.000000,0.000000,0.000000,0.000000,0.000000,0.508591,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22380,1119,0.000000,0.000000,0.000000,0.000000,0.000000,0.529443,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22400,1120,0.000000,0.000000,0.000000,0.000000,0.000000,0.550000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22420,1121,0.000000,0.000000,0.000000,0.000000,0.000000,0.570249,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22440,1122,0.000000,0.000000,0.000000,0.000000,0.000000,0.590175,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22460,1123,0.000000,0.000000,0.000000,0.000000,0.000000,0.609766,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22480,1124,0.000000,0.000000,0.000000,0.000000,0.000000,0.629007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22500,1125,0.000000,0.000000,0.000000,0.000000,0.000000,0.647885,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22520,1126,0.000000,0.000000,0.000000,0.000000,0.000000,0.666388,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22540,1127,0.000000,0.000000,0.000000,0.000000,0.000000,0.684503,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22560,1128,0.000000,0.000000,0.000000,0.000000,0.000000,0.702218,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22580,1129,0.000000,0.000000,0.000000,0.000000,0.000000,0.719519,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22600,1130,0.000000,0.000000,0.000000,0.000000,0.000000,0.736396,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22620,1131,0.000000,0.000000,0.000000,0.000000,0.000000,0.752837,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22640,1132,0.000000,0.000000,0.000000,0.000000,0.000000,0.768830,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22660,1133,0.000000,0.000000,0.000000,0.000000,0.000000,0.784365,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22680,1134,0.000000,0.000000,0.000000,0.000000,0.000000,0.799431,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22700,1135,0.000000,0.000000,0.000000,0.000000,0.000000,0.814018,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22720,1136,0.000000,0.000000,0.000000,0.000000,0.000000,0.828115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22740,1137,0.000000,0.000000,0.000000,0.000000,0.000000,0.841714,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22760,1138,0.000000,0.000000,0.000000,0.000000,0.000000,0.854804,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22780,1139,0.000000,0.000000,0.000000,0.000000,0.000000,0.867376,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22800,1140,0.000000,0.000000,0.000000,0.000000,0.000000,0.879423,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22820,1141,0.000000,0.000000,0.000000,0.000000,0.000000,0.890935,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22840,1142,0.000000,0.000000,0.000000,0.000000,0.000000,0.901906,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22860,1143,0.000000,0.000000,0.000000,0.000000,0.000000,0.912327,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22880,1144,0.000000,0.000000,0.000000,0.000000,0.000000,0.922191,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22900,1145,0.000000,0.000000,0.000000,0.000000,0.000000,0.931492,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22920,1146,0.000000,0.000000,0.000000,0.000000,0.000000,0.940222,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22940,1147,0.000000,0.000000,0.000000,0.000000,0.000000,0.948377,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22960,1148,0.000000,0.000000,0.000000,0.000000,0.000000,0.955951,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +22980,1149,0.000000,0.000000,0.000000,0.000000,0.000000,0.962938,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23000,1150,0.000000,0.000000,0.000000,0.000000,0.000000,0.969333,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23020,1151,0.000000,0.000000,0.000000,0.000000,0.000000,0.975133,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23040,1152,0.000000,0.000000,0.000000,0.000000,0.000000,0.980333,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23060,1153,0.000000,0.000000,0.000000,0.000000,0.000000,0.984929,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23080,1154,0.000000,0.000000,0.000000,0.000000,0.000000,0.988920,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23100,1155,0.000000,0.000000,0.000000,0.000000,0.000000,0.992300,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23120,1156,0.000000,0.000000,0.000000,0.000000,0.000000,0.995070,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23140,1157,0.000000,0.000000,0.000000,0.000000,0.000000,0.997226,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23160,1158,0.000000,0.000000,0.000000,0.000000,0.000000,0.998767,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23180,1159,0.000000,0.000000,0.000000,0.000000,0.000000,0.999692,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23200,1160,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23220,1161,0.000000,0.000000,0.000000,0.000000,0.000000,0.999692,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23240,1162,0.000000,0.000000,0.000000,0.000000,0.000000,0.998767,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23260,1163,0.000000,0.000000,0.000000,0.000000,0.000000,0.997226,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23280,1164,0.000000,0.000000,0.000000,0.000000,0.000000,0.995070,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23300,1165,0.000000,0.000000,0.000000,0.000000,0.000000,0.992300,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23320,1166,0.000000,0.000000,0.000000,0.000000,0.000000,0.988920,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23340,1167,0.000000,0.000000,0.000000,0.000000,0.000000,0.984929,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23360,1168,0.000000,0.000000,0.000000,0.000000,0.000000,0.980333,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23380,1169,0.000000,0.000000,0.000000,0.000000,0.000000,0.975133,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23400,1170,0.000000,0.000000,0.000000,0.000000,0.000000,0.969333,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23420,1171,0.000000,0.000000,0.000000,0.000000,0.000000,0.962938,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23440,1172,0.000000,0.000000,0.000000,0.000000,0.000000,0.955951,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23460,1173,0.000000,0.000000,0.000000,0.000000,0.000000,0.948377,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23480,1174,0.000000,0.000000,0.000000,0.000000,0.000000,0.940222,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23500,1175,0.000000,0.000000,0.000000,0.000000,0.000000,0.931492,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23520,1176,0.000000,0.000000,0.000000,0.000000,0.000000,0.922191,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23540,1177,0.000000,0.000000,0.000000,0.000000,0.000000,0.912327,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23560,1178,0.000000,0.000000,0.000000,0.000000,0.000000,0.901906,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23580,1179,0.000000,0.000000,0.000000,0.000000,0.000000,0.890935,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23600,1180,0.000000,0.000000,0.000000,0.000000,0.000000,0.879423,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23620,1181,0.000000,0.000000,0.000000,0.000000,0.000000,0.867376,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23640,1182,0.000000,0.000000,0.000000,0.000000,0.000000,0.854804,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23660,1183,0.000000,0.000000,0.000000,0.000000,0.000000,0.841714,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23680,1184,0.000000,0.000000,0.000000,0.000000,0.000000,0.828115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23700,1185,0.000000,0.000000,0.000000,0.000000,0.000000,0.814018,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23720,1186,0.000000,0.000000,0.000000,0.000000,0.000000,0.799431,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23740,1187,0.000000,0.000000,0.000000,0.000000,0.000000,0.784365,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23760,1188,0.000000,0.000000,0.000000,0.000000,0.000000,0.768830,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23780,1189,0.000000,0.000000,0.000000,0.000000,0.000000,0.752837,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23800,1190,0.000000,0.000000,0.000000,0.000000,0.000000,0.736396,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23820,1191,0.000000,0.000000,0.000000,0.000000,0.000000,0.719519,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23840,1192,0.000000,0.000000,0.000000,0.000000,0.000000,0.702218,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23860,1193,0.000000,0.000000,0.000000,0.000000,0.000000,0.684503,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23880,1194,0.000000,0.000000,0.000000,0.000000,0.000000,0.666388,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23900,1195,0.000000,0.000000,0.000000,0.000000,0.000000,0.647885,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23920,1196,0.000000,0.000000,0.000000,0.000000,0.000000,0.629007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23940,1197,0.000000,0.000000,0.000000,0.000000,0.000000,0.609766,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23960,1198,0.000000,0.000000,0.000000,0.000000,0.000000,0.590175,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +23980,1199,0.000000,0.000000,0.000000,0.000000,0.000000,0.570249,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24000,1200,0.000000,0.000000,0.000000,0.000000,0.000000,0.550000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24020,1201,0.000000,0.000000,0.000000,0.000000,0.000000,0.529443,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24040,1202,0.000000,0.000000,0.000000,0.000000,0.000000,0.508591,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24060,1203,0.000000,0.000000,0.000000,0.000000,0.000000,0.487460,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24080,1204,0.000000,0.000000,0.000000,0.000000,0.000000,0.466063,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24100,1205,0.000000,0.000000,0.000000,0.000000,0.000000,0.444415,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24120,1206,0.000000,0.000000,0.000000,0.000000,0.000000,0.422531,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24140,1207,0.000000,0.000000,0.000000,0.000000,0.000000,0.400426,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24160,1208,0.000000,0.000000,0.000000,0.000000,0.000000,0.378115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24180,1209,0.000000,0.000000,0.000000,0.000000,0.000000,0.355614,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24200,1210,0.000000,0.000000,0.000000,0.000000,0.000000,0.332937,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24220,1211,0.000000,0.000000,0.000000,0.000000,0.000000,0.310101,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24240,1212,0.000000,0.000000,0.000000,0.000000,0.000000,0.287121,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24260,1213,0.000000,0.000000,0.000000,0.000000,0.000000,0.264012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24280,1214,0.000000,0.000000,0.000000,0.000000,0.000000,0.240791,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24300,1215,0.000000,0.000000,0.000000,0.000000,0.000000,0.217474,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24320,1216,0.000000,0.000000,0.000000,0.000000,0.000000,0.194076,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24340,1217,0.000000,0.000000,0.000000,0.000000,0.000000,0.170613,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24360,1218,0.000000,0.000000,0.000000,0.000000,0.000000,0.147102,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24380,1219,0.000000,0.000000,0.000000,0.000000,0.000000,0.123559,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24400,1220,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24420,1221,0.000000,0.000000,0.000000,0.000000,0.000000,0.123559,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24440,1222,0.000000,0.000000,0.000000,0.000000,0.000000,0.147102,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24460,1223,0.000000,0.000000,0.000000,0.000000,0.000000,0.170613,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24480,1224,0.000000,0.000000,0.000000,0.000000,0.000000,0.194076,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24500,1225,0.000000,0.000000,0.000000,0.000000,0.000000,0.217474,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24520,1226,0.000000,0.000000,0.000000,0.000000,0.000000,0.240791,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24540,1227,0.000000,0.000000,0.000000,0.000000,0.000000,0.264012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24560,1228,0.000000,0.000000,0.000000,0.000000,0.000000,0.287121,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24580,1229,0.000000,0.000000,0.000000,0.000000,0.000000,0.310101,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24600,1230,0.000000,0.000000,0.000000,0.000000,0.000000,0.332937,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24620,1231,0.000000,0.000000,0.000000,0.000000,0.000000,0.355614,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24640,1232,0.000000,0.000000,0.000000,0.000000,0.000000,0.378115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24660,1233,0.000000,0.000000,0.000000,0.000000,0.000000,0.400426,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24680,1234,0.000000,0.000000,0.000000,0.000000,0.000000,0.422531,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24700,1235,0.000000,0.000000,0.000000,0.000000,0.000000,0.444415,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24720,1236,0.000000,0.000000,0.000000,0.000000,0.000000,0.466063,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24740,1237,0.000000,0.000000,0.000000,0.000000,0.000000,0.487460,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24760,1238,0.000000,0.000000,0.000000,0.000000,0.000000,0.508591,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24780,1239,0.000000,0.000000,0.000000,0.000000,0.000000,0.529443,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24800,1240,0.000000,0.000000,0.000000,0.000000,0.000000,0.550000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24820,1241,0.000000,0.000000,0.000000,0.000000,0.000000,0.570249,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24840,1242,0.000000,0.000000,0.000000,0.000000,0.000000,0.590175,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24860,1243,0.000000,0.000000,0.000000,0.000000,0.000000,0.609766,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24880,1244,0.000000,0.000000,0.000000,0.000000,0.000000,0.629007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24900,1245,0.000000,0.000000,0.000000,0.000000,0.000000,0.647885,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24920,1246,0.000000,0.000000,0.000000,0.000000,0.000000,0.666388,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24940,1247,0.000000,0.000000,0.000000,0.000000,0.000000,0.684503,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24960,1248,0.000000,0.000000,0.000000,0.000000,0.000000,0.702218,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +24980,1249,0.000000,0.000000,0.000000,0.000000,0.000000,0.719519,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25000,1250,0.000000,0.000000,0.000000,0.000000,0.000000,0.736396,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25020,1251,0.000000,0.000000,0.000000,0.000000,0.000000,0.752837,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25040,1252,0.000000,0.000000,0.000000,0.000000,0.000000,0.768830,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25060,1253,0.000000,0.000000,0.000000,0.000000,0.000000,0.784365,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25080,1254,0.000000,0.000000,0.000000,0.000000,0.000000,0.799431,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25100,1255,0.000000,0.000000,0.000000,0.000000,0.000000,0.814018,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25120,1256,0.000000,0.000000,0.000000,0.000000,0.000000,0.828115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25140,1257,0.000000,0.000000,0.000000,0.000000,0.000000,0.841714,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25160,1258,0.000000,0.000000,0.000000,0.000000,0.000000,0.854804,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25180,1259,0.000000,0.000000,0.000000,0.000000,0.000000,0.867376,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25200,1260,0.000000,0.000000,0.000000,0.000000,0.000000,0.879423,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25220,1261,0.000000,0.000000,0.000000,0.000000,0.000000,0.890935,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25240,1262,0.000000,0.000000,0.000000,0.000000,0.000000,0.901906,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25260,1263,0.000000,0.000000,0.000000,0.000000,0.000000,0.912327,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25280,1264,0.000000,0.000000,0.000000,0.000000,0.000000,0.922191,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25300,1265,0.000000,0.000000,0.000000,0.000000,0.000000,0.931492,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25320,1266,0.000000,0.000000,0.000000,0.000000,0.000000,0.940222,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25340,1267,0.000000,0.000000,0.000000,0.000000,0.000000,0.948377,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25360,1268,0.000000,0.000000,0.000000,0.000000,0.000000,0.955951,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25380,1269,0.000000,0.000000,0.000000,0.000000,0.000000,0.962938,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25400,1270,0.000000,0.000000,0.000000,0.000000,0.000000,0.969333,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25420,1271,0.000000,0.000000,0.000000,0.000000,0.000000,0.975133,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25440,1272,0.000000,0.000000,0.000000,0.000000,0.000000,0.980333,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25460,1273,0.000000,0.000000,0.000000,0.000000,0.000000,0.984929,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25480,1274,0.000000,0.000000,0.000000,0.000000,0.000000,0.988920,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25500,1275,0.000000,0.000000,0.000000,0.000000,0.000000,0.992300,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25520,1276,0.000000,0.000000,0.000000,0.000000,0.000000,0.995070,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25540,1277,0.000000,0.000000,0.000000,0.000000,0.000000,0.997226,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25560,1278,0.000000,0.000000,0.000000,0.000000,0.000000,0.998767,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25580,1279,0.000000,0.000000,0.000000,0.000000,0.000000,0.999692,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25600,1280,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25620,1281,0.000000,0.000000,0.000000,0.000000,0.000000,0.999692,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25640,1282,0.000000,0.000000,0.000000,0.000000,0.000000,0.998767,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25660,1283,0.000000,0.000000,0.000000,0.000000,0.000000,0.997226,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25680,1284,0.000000,0.000000,0.000000,0.000000,0.000000,0.995070,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25700,1285,0.000000,0.000000,0.000000,0.000000,0.000000,0.992300,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25720,1286,0.000000,0.000000,0.000000,0.000000,0.000000,0.988920,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25740,1287,0.000000,0.000000,0.000000,0.000000,0.000000,0.984929,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25760,1288,0.000000,0.000000,0.000000,0.000000,0.000000,0.980333,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25780,1289,0.000000,0.000000,0.000000,0.000000,0.000000,0.975133,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25800,1290,0.000000,0.000000,0.000000,0.000000,0.000000,0.969333,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25820,1291,0.000000,0.000000,0.000000,0.000000,0.000000,0.962938,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25840,1292,0.000000,0.000000,0.000000,0.000000,0.000000,0.955951,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25860,1293,0.000000,0.000000,0.000000,0.000000,0.000000,0.948377,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25880,1294,0.000000,0.000000,0.000000,0.000000,0.000000,0.940222,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25900,1295,0.000000,0.000000,0.000000,0.000000,0.000000,0.931492,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25920,1296,0.000000,0.000000,0.000000,0.000000,0.000000,0.922191,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25940,1297,0.000000,0.000000,0.000000,0.000000,0.000000,0.912327,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25960,1298,0.000000,0.000000,0.000000,0.000000,0.000000,0.901906,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +25980,1299,0.000000,0.000000,0.000000,0.000000,0.000000,0.890935,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26000,1300,0.000000,0.000000,0.000000,0.000000,0.000000,0.879423,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26020,1301,0.000000,0.000000,0.000000,0.000000,0.000000,0.867376,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26040,1302,0.000000,0.000000,0.000000,0.000000,0.000000,0.854804,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26060,1303,0.000000,0.000000,0.000000,0.000000,0.000000,0.841714,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26080,1304,0.000000,0.000000,0.000000,0.000000,0.000000,0.828115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26100,1305,0.000000,0.000000,0.000000,0.000000,0.000000,0.814018,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26120,1306,0.000000,0.000000,0.000000,0.000000,0.000000,0.799431,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26140,1307,0.000000,0.000000,0.000000,0.000000,0.000000,0.784365,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26160,1308,0.000000,0.000000,0.000000,0.000000,0.000000,0.768830,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26180,1309,0.000000,0.000000,0.000000,0.000000,0.000000,0.752837,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26200,1310,0.000000,0.000000,0.000000,0.000000,0.000000,0.736396,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26220,1311,0.000000,0.000000,0.000000,0.000000,0.000000,0.719519,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26240,1312,0.000000,0.000000,0.000000,0.000000,0.000000,0.702218,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26260,1313,0.000000,0.000000,0.000000,0.000000,0.000000,0.684503,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26280,1314,0.000000,0.000000,0.000000,0.000000,0.000000,0.666388,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26300,1315,0.000000,0.000000,0.000000,0.000000,0.000000,0.647885,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26320,1316,0.000000,0.000000,0.000000,0.000000,0.000000,0.629007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26340,1317,0.000000,0.000000,0.000000,0.000000,0.000000,0.609766,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26360,1318,0.000000,0.000000,0.000000,0.000000,0.000000,0.590175,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26380,1319,0.000000,0.000000,0.000000,0.000000,0.000000,0.570249,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26400,1320,0.000000,0.000000,0.000000,0.000000,0.000000,0.550000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26420,1321,0.000000,0.000000,0.000000,0.000000,0.000000,0.529443,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26440,1322,0.000000,0.000000,0.000000,0.000000,0.000000,0.508591,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26460,1323,0.000000,0.000000,0.000000,0.000000,0.000000,0.487460,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26480,1324,0.000000,0.000000,0.000000,0.000000,0.000000,0.466063,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26500,1325,0.000000,0.000000,0.000000,0.000000,0.000000,0.444415,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26520,1326,0.000000,0.000000,0.000000,0.000000,0.000000,0.422531,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26540,1327,0.000000,0.000000,0.000000,0.000000,0.000000,0.400426,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26560,1328,0.000000,0.000000,0.000000,0.000000,0.000000,0.378115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26580,1329,0.000000,0.000000,0.000000,0.000000,0.000000,0.355614,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26600,1330,0.000000,0.000000,0.000000,0.000000,0.000000,0.332937,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26620,1331,0.000000,0.000000,0.000000,0.000000,0.000000,0.310101,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26640,1332,0.000000,0.000000,0.000000,0.000000,0.000000,0.287121,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26660,1333,0.000000,0.000000,0.000000,0.000000,0.000000,0.264012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26680,1334,0.000000,0.000000,0.000000,0.000000,0.000000,0.240791,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26700,1335,0.000000,0.000000,0.000000,0.000000,0.000000,0.217474,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26720,1336,0.000000,0.000000,0.000000,0.000000,0.000000,0.194076,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26740,1337,0.000000,0.000000,0.000000,0.000000,0.000000,0.170613,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26760,1338,0.000000,0.000000,0.000000,0.000000,0.000000,0.147102,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26780,1339,0.000000,0.000000,0.000000,0.000000,0.000000,0.123559,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26800,1340,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26820,1341,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26840,1342,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26860,1343,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26880,1344,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26900,1345,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26920,1346,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26940,1347,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26960,1348,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +26980,1349,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 +27000,1350,0.000000,0.000000,0.000000,0.000000,0.000000,0.100000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 diff --git a/src/openpi_control/config.py b/src/openpi_control/config.py index 073cdd8..145ec0d 100644 --- a/src/openpi_control/config.py +++ b/src/openpi_control/config.py @@ -22,12 +22,14 @@ "ARX_ENC", "ARX_L5", "ARX_X5", + "SO101", "Trossen_wai_ctrl", "Yam", ) SUPPORTED_EFFECTORS = ( "E_ARX", "E_ARX_ENC", + "E_SO101", "E_Trossen_ctrl", "E_Yam", "E_Yam_Handle", diff --git a/src/openpi_control/models/arms/SO101/SO101.json b/src/openpi_control/models/arms/SO101/SO101.json new file mode 100644 index 0000000..4f8e3e3 --- /dev/null +++ b/src/openpi_control/models/arms/SO101/SO101.json @@ -0,0 +1,221 @@ +{ + "config_version": "1.1.1", + "device_model": "SO101", + "device_type": "arm", + "arm_type": "nello", + "topic_type": "ROS", + "driver_type": "FEETECH", + "algo_type": "Pinocchio", + "planning_type": "None", + "catalog": { + "display_label": "SO101: SO-ARM100/101 5 DOF Arm", + "order": 11, + "baudrate": 1000000, + "default_effector_model": "E_SO101", + "port_type": "Serial" + }, + "joint_init_sequence": [ + 2, + 1, + 3, + 4, + 0 + ], + "joints": [ + { + "joint_id": 0, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 1, + "data_index": 0, + "dir_invert": 1, + "pos_min": -1.91986, + "pos_max": 1.91986, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + }, + { + "joint_id": 1, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 2, + "data_index": 1, + "dir_invert": 1, + "pos_min": -1.74533, + "pos_max": 1.74533, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + }, + { + "joint_id": 2, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 3, + "data_index": 2, + "dir_invert": 1, + "pos_min": -1.69, + "pos_max": 1.69, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + }, + { + "joint_id": 3, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 4, + "data_index": 3, + "dir_invert": 1, + "pos_min": -1.65806, + "pos_max": 1.65806, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + }, + { + "joint_id": 4, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 5, + "data_index": 4, + "dir_invert": 1, + "pos_min": -2.74385, + "pos_max": 2.84121, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + } + ] +} diff --git a/src/openpi_control/models/arms/SO101/SO101.urdf b/src/openpi_control/models/arms/SO101/SO101.urdf new file mode 100644 index 0000000..3b7ec05 --- /dev/null +++ b/src/openpi_control/models/arms/SO101/SO101.urdf @@ -0,0 +1,414 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + \ No newline at end of file diff --git a/src/openpi_control/models/arms/SO101/SO101_01.json b/src/openpi_control/models/arms/SO101/SO101_01.json new file mode 100644 index 0000000..d236cc7 --- /dev/null +++ b/src/openpi_control/models/arms/SO101/SO101_01.json @@ -0,0 +1,75 @@ +{ + "config_version": "1.1.1", + "device_model": "SO101", + "device_type": "arm", + "arm_type": "nello", + "spring_effect": false, + "gravity_compensation": false, + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 0, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 1, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 2, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 2, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 3, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 3, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 4, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 4, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 5, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_SO101/E_SO101.json b/src/openpi_control/models/effectors/E_SO101/E_SO101.json new file mode 100644 index 0000000..9810016 --- /dev/null +++ b/src/openpi_control/models/effectors/E_SO101/E_SO101.json @@ -0,0 +1,58 @@ +{ + "config_version": "1.1.1", + "device_model": "E_SO101", + "device_type": "effector", + "effector_type": "nello", + "topic_type": "ROS", + "driver_type": "FEETECH", + "algo_type": "Algo", + "planning_type": "None", + "catalog": { + "display_label": "E_SO101: SO-ARM100/101 Gripper", + "order": 10, + "baudrate": 1000000, + "need_repeated_command": false + }, + "joints": [ + { + "joint_id": 5, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "max_length": 0.06, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 6, + "data_index": 5, + "dir_invert": 1, + "pos_min": -0.174533, + "pos_max": 1.74533, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json b/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json new file mode 100644 index 0000000..9fa6d12 --- /dev/null +++ b/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json @@ -0,0 +1,32 @@ +{ + "config_version": "1.1.1", + "device_model": "E_SO101", + "device_type": "effector", + "effector_type": "nello", + "spring_effect": false, + "control_mode": "position", + "base_rpy": [ + 0, + 0, + 0 + ], + "rpy_invert": [ + 1, + 1, + 1 + ], + "joints": [ + { + "joint_id": 5, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 6, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_SO101/E_SO101_mass.json b/src/openpi_control/models/effectors/E_SO101/E_SO101_mass.json new file mode 100644 index 0000000..d70ec4b --- /dev/null +++ b/src/openpi_control/models/effectors/E_SO101/E_SO101_mass.json @@ -0,0 +1,16 @@ +{ + "mass": 0.012000, + "center_of_mass": [ + -0.00157495, + -0.0300244, + 0.0192755 + ], + "inertia": { + "ixx": 0.00001, + "iyy": 0.00000, + "izz": 0.00001, + "ixy": 0.00000, + "ixz": 0.00000, + "iyz": 0.00000 + } +} diff --git a/src/openpi_control/native.py b/src/openpi_control/native.py index 08603f6..9ac9b70 100644 --- a/src/openpi_control/native.py +++ b/src/openpi_control/native.py @@ -27,6 +27,7 @@ EthernetConnection, InputLayout, ResolvedArmAssets, + SerialConnection, ) from .exceptions import ( CommandRejectedError, @@ -122,6 +123,14 @@ def validate_connection(connection: ArmConnection) -> None: "power, and that the host has an address on the controller's subnet" ) return + if isinstance(connection, SerialConnection): + device = Path(connection.device) + if not device.exists(): + raise ConnectionUnavailableError( + f"serial device {connection.device!r} does not exist; " + "check the USB cable and adapter" + ) + return path = Path("/sys/class/net") / connection.interface if not path.exists(): raise ConnectionUnavailableError( @@ -375,9 +384,17 @@ def _connect_prepared( if role is ArmRole.FOLLOWER: self._direct_pub = _Publisher(self._context, topics.direct_command) # The native node takes the bus identity as an opaque string: a SocketCAN - # interface name, or the controller IPv4 address for Ethernet drivers. + # interface name, the controller IPv4 address for Ethernet drivers, or + # the tty device path for serial buses (which also need the catalog baud). if isinstance(config.connection, EthernetConnection): connection_args = ["--control_port", config.connection.ip] + elif isinstance(config.connection, SerialConnection): + connection_args = [ + "--control_port", + config.connection.device, + "--baud_rate", + str(config.catalog_baudrate()), + ] else: connection_args = ["--control_port", config.connection.interface] args = [ diff --git a/tests/test_config.py b/tests/test_config.py index 8b95e20..2a358de 100644 --- a/tests/test_config.py +++ b/tests/test_config.py @@ -23,7 +23,8 @@ def test_physical_model_catalog_is_complete(model: str) -> None: assert assets.model_config.is_file() assert assets.instance_config.is_file() assert assets.urdf.is_file() - assert len(config.joint_names()) == 6 + # SO101 (SO-ARM100/101) is the catalog's only 5-DOF arm; everything else is 6-DOF. + assert len(config.joint_names()) == (5 if model == "SO101" else 6) # The native node is always launched with --algo_type Pinocchio, which overrides # any config value except "Algo" (config keeps priority for "Algo"). Arm configs # must therefore never declare "Algo": robot-test 1.1.1 configs say "KDL" @@ -44,6 +45,13 @@ def test_serial_connection_requires_a_dev_path() -> None: SerialConnection("ttyUSB0") +def test_so101_catalog_declares_serial_port_type_and_baudrate() -> None: + config = ArmConfig("arm", "SO101", SocketCanConnection("test"), effector_model="E_SO101") + catalog = json.loads(config.resolve_assets().model_config.read_text())["catalog"] + assert catalog["port_type"] == "Serial" + assert config.catalog_baudrate() == 1000000 + + def test_logical_identity_is_separate_from_instance_config(tmp_path: Path) -> None: canonical = ( ArmConfig("source", "Yam", SocketCanConnection("test")).resolve_assets().instance_config diff --git a/uv.lock b/uv.lock index 33eec5d..6f319f7 100644 --- a/uv.lock +++ b/uv.lock @@ -11,7 +11,7 @@ name = "cffi" version = "2.0.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "pycparser" }, + { name = "pycparser", marker = "implementation_name != 'PyPy'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/eb/56/b1ba7935a17738ae8453301356628e8147c79dbb825bcbc73dc7401f9846/cffi-2.0.0.tar.gz", hash = "sha256:44d1b5909021139fe36001ae048dbdde8214afa20200eda0f64c068cac5d5529", size = 523588, upload-time = "2025-09-08T23:24:04.541Z" } wheels = [ @@ -333,13 +333,15 @@ wheels = [ [[package]] name = "openpi-control" -version = "0.1.0" +version = "0.1.1" source = { editable = "." } dependencies = [ { name = "numpy", version = "2.4.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.12'" }, { name = "numpy", version = "2.5.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.12'" }, + { name = "pyserial" }, { name = "python-can" }, { name = "pyzmq" }, + { name = "trossen-arm" }, ] [package.optional-dependencies] @@ -354,11 +356,13 @@ dev = [ requires-dist = [ { name = "hypothesis", marker = "extra == 'dev'", specifier = ">=6" }, { name = "numpy", specifier = ">=1.26" }, + { name = "pyserial", specifier = ">=3.5" }, { name = "pytest", marker = "extra == 'dev'", specifier = ">=8" }, { name = "pytest-cov", marker = "extra == 'dev'", specifier = ">=5" }, { name = "python-can", specifier = ">=4.3" }, { name = "pyzmq", specifier = ">=25" }, { name = "ruff", marker = "extra == 'dev'", specifier = ">=0.6" }, + { name = "trossen-arm", specifier = "==1.10.0" }, ] provides-extras = ["dev"] @@ -398,6 +402,15 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/f4/7e/a72dd26f3b0f4f2bf1dd8923c85f7ceb43172af56d63c7383eb62b332364/pygments-2.20.0-py3-none-any.whl", hash = "sha256:81a9e26dd42fd28a23a2d169d86d7ac03b46e2f8b59ed4698fb4785f946d0176", size = 1231151, upload-time = "2026-03-29T13:29:30.038Z" }, ] +[[package]] +name = "pyserial" +version = "3.5" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/1e/7d/ae3f0a63f41e4d2f6cb66a5b57197850f919f59e558159a4dd3a818f5082/pyserial-3.5.tar.gz", hash = "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb", size = 159125, upload-time = "2020-11-23T03:59:15.045Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/07/bc/587a445451b253b285629263eb51c2d8e9bcea4fc97826266d186f96f558/pyserial-3.5-py2.py3-none-any.whl", hash = "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0", size = 90585, upload-time = "2020-11-23T03:59:13.41Z" }, +] + [[package]] name = "pytest" version = "9.1.1" @@ -588,6 +601,26 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/7b/61/cceae43728b7de99d9b847560c262873a1f6c98202171fd5ed62640b494b/tomli-2.4.1-py3-none-any.whl", hash = "sha256:0d85819802132122da43cb86656f8d1f8c6587d54ae7dcaf30e90533028b49fe", size = 14583, upload-time = "2026-03-25T20:22:03.012Z" }, ] +[[package]] +name = "trossen-arm" +version = "1.10.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy", version = "2.4.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.12'" }, + { name = "numpy", version = "2.5.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.12'" }, +] +wheels = [ + { url = "https://files.pythonhosted.org/packages/0a/32/13307c7eef88cc656a00d41ea1925b749e7762fac6da7252ea7938c1b7d2/trossen_arm-1.10.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:76dbfae7841fad136e747be3921c2101801d25ac2f4ca176eec3027a9b1a5e74", size = 729263, upload-time = "2026-05-05T21:13:26.789Z" }, + { url = "https://files.pythonhosted.org/packages/6c/a4/5e0f49b18d2533aee4536666d4b0117c9aeb5d3230f35b233e3c106a2b8a/trossen_arm-1.10.0-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:e145bd36ffb53cdf1c6e6de4d4d2e8e5302ea599db0d716e75f97f61925631b7", size = 1026542, upload-time = "2026-05-05T21:13:28.287Z" }, + { url = "https://files.pythonhosted.org/packages/7a/bb/e0f2c80a1018f507b4dd252603d62196acdc558104cb3a7349c0cb35e72c/trossen_arm-1.10.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:2ebd6775ca42dc75ebd718ea080450b335daedc2eedbd6223cbdfe47dbe35cb1", size = 1134241, upload-time = "2026-05-05T21:13:29.604Z" }, + { url = "https://files.pythonhosted.org/packages/bc/35/4b9e702dbe5a844171e76d9e20e57f7d2ebce539dc25866a7dc665e24277/trossen_arm-1.10.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:8c7653ebeb8a0240ba0f6de99cfb63ef470d8d2cff13844f9a994e48d4c68095", size = 698112, upload-time = "2026-05-05T21:13:31.316Z" }, + { url = "https://files.pythonhosted.org/packages/55/78/54758216c0b3fea040edabc452facf7e8363752e113e97e58bbc817b76f0/trossen_arm-1.10.0-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:1ecf68daf9c56829c6b8b0b8ea08b5d9ef8364424e847b32c321b31395d9fbb3", size = 1027422, upload-time = "2026-05-05T21:13:32.72Z" }, + { url = "https://files.pythonhosted.org/packages/cb/e7/d6b208b9f48d4549ca39f992a6ef8ea9175fa675ed1ba9a61dcfc3cb6782/trossen_arm-1.10.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:40c595fd535012f50e1e802cd23cde3daa4ad2c65a15907868cbe5b6969a3e51", size = 1137443, upload-time = "2026-05-05T21:13:34.122Z" }, + { url = "https://files.pythonhosted.org/packages/10/37/11a89f928de15bc79e8151579b7aff1347842175cd02c9cf26c76a3a6bbb/trossen_arm-1.10.0-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:d873626e07d2b734b4fa986911e19d7551379ce0b64167205abbbddb2d4de116", size = 698155, upload-time = "2026-05-05T21:13:35.287Z" }, + { url = "https://files.pythonhosted.org/packages/68/39/ee18ee48aa32878da42244365dbdb91904dc082422a947686ee8d0953e40/trossen_arm-1.10.0-cp313-cp313-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:396a01a85313b49c4456a7425528d46a4cc58520709c18312228a7c3f051cf19", size = 1027412, upload-time = "2026-05-05T21:13:36.261Z" }, + { url = "https://files.pythonhosted.org/packages/ed/fe/1f45d3f05b88b394df6974e660ae9479956d99e41f10d9859a1f675f66eb/trossen_arm-1.10.0-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8b4218b51eece5bfadaa54885bf182dc94325f7a1f8071f225a8e80fe2bbd31f", size = 1137637, upload-time = "2026-05-05T21:13:37.492Z" }, +] + [[package]] name = "typing-extensions" version = "4.16.0"