From 5b581db14acec416fa72dfce0489bc30bd1e4309 Mon Sep 17 00:00:00 2001 From: maxconway Date: Sat, 31 Oct 2026 11:49:57 -0600 Subject: [PATCH 001/179] golem_safety: drop the old safety layers' refusals, aliases and parity fixtures; numpy is a run-time dependency Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_01MezUeauFS9tKMgZKJxo1HC --- core_ws/src/golem_safety/CMakeLists.txt | 3 - core_ws/src/golem_safety/README.md | 13 +- core_ws/src/golem_safety/package.xml | 3 +- core_ws/src/golem_safety/src/config.cpp | 85 +---- .../src/golem_safety/src/dump_config_main.cpp | 3 +- .../src/golem_safety/test/fixtures/README.md | 11 +- .../test/fixtures/limits/g1/full.json | 316 ------------------ .../test/fixtures/limits/g1/sim_split.json | 316 ------------------ .../test/fixtures/limits/g1/split.json | 316 ------------------ .../limits/h12/default_safety_full.json | 263 --------------- .../limits/h12/default_safety_split.json | 263 --------------- .../limits/h12/locomotion_safety_full.json | 263 --------------- .../limits/h12/relax_safety_full.json | 263 --------------- .../limits/h12/relax_safety_split.json | 263 --------------- .../fixtures/limits/h12/sim_safety_split.json | 263 --------------- .../limits/h12/tight_safety_full.json | 263 --------------- .../limits/h12/tight_safety_split.json | 263 --------------- core_ws/src/golem_safety/test/test_config.cpp | 118 ------- .../golem_safety/test/test_limit_parity.py | 129 ------- core_ws/src/golem_safety/test/test_no_sdk.py | 2 +- .../src/golem_safety/test/test_robot_data.py | 8 - .../src/golem_safety/tools/g1_joint_limits.py | 1 - 22 files changed, 17 insertions(+), 3411 deletions(-) delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/g1/full.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/g1/sim_split.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/g1/split.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/h12/default_safety_full.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/h12/default_safety_split.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/h12/locomotion_safety_full.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/h12/relax_safety_full.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/h12/relax_safety_split.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/h12/sim_safety_split.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/h12/tight_safety_full.json delete mode 100644 core_ws/src/golem_safety/test/fixtures/limits/h12/tight_safety_split.json delete mode 100644 core_ws/src/golem_safety/test/test_limit_parity.py diff --git a/core_ws/src/golem_safety/CMakeLists.txt b/core_ws/src/golem_safety/CMakeLists.txt index 9be01dbd..0611c527 100644 --- a/core_ws/src/golem_safety/CMakeLists.txt +++ b/core_ws/src/golem_safety/CMakeLists.txt @@ -63,9 +63,6 @@ if(BUILD_TESTING) ament_add_gtest(test_config test/test_config.cpp) target_link_libraries(test_config golem_safety_core) - # Runs golem_safety_dump_config, so it needs the installed executable and configs. - ament_add_pytest_test(test_limit_parity test/test_limit_parity.py TIMEOUT 120) - # test/fixtures/lowcmd_crc.json holds the Unitree SDK's CRCs (test/fixtures/README.md). ament_add_gtest(test_lowcmd test/test_lowcmd.cpp) target_link_libraries(test_lowcmd golem_safety_core) diff --git a/core_ws/src/golem_safety/README.md b/core_ws/src/golem_safety/README.md index 3752f96b..e3937066 100644 --- a/core_ws/src/golem_safety/README.md +++ b/core_ws/src/golem_safety/README.md @@ -122,21 +122,19 @@ another pair (`golem_upper_ik.pairing`). | `estop_response.after` | required | `burst_then_silence` or `keep_writing` | | `estop_response.burst_count` | 50 | the commands of `burst_then_silence` | | `limits.clip.position_offset`, `velocity_ratio`, `torque_ratio` | 0.01, 0.5, 0.5 | one number, or one per body joint in slot order | -| `limits.clip.kp_max`, `kd_max` | 300, 30 | one number for every joint (`kp_abs`, `kd_abs` are aliases) | +| `limits.clip.kp_max`, `kd_max` | 300, 30 | one number for every joint | | `limits.estop.position_offset`, `velocity_ratio`, `torque_ratio` | 0.01, 0.5, 0.5 | as `limits.clip`, for the measured state | | `logging.enabled`, `base_dir`, `chunk_prefix` | `false`, `/tmp/golem_safety`, `chunk_` | the `.npz` sample log | -`limits.enforce` is an alias of `limits.clip`. The limits derive from the profile's joint +The limits derive from the profile's joint table: q within [low + `position_offset`, high - `position_offset`] (a negative offset widens the range), |dq| up to velocity limit x `velocity_ratio`, |tau| up to effort limit x `torque_ratio`. `golem_safety_dump_config ` (installed in `lib/golem_safety`) prints every derived and parsed value as JSON. The loader refuses, naming the robot, the file and the key: an unknown key at any level; a -key of an older schema (`network`, `robot`, `startup`, `engage`, `estop.enabled`, ...), -saying why it was removed or which key replaces it; a non-finite or out-of-range number; a -list whose length is not the profile's joint count; an alias given with the key it aliases; -a missing required key. +non-finite or out-of-range number; a list whose length is not the profile's joint count; a +missing required key. ## Behavior @@ -274,8 +272,7 @@ docker/scripts/docker_run.sh ros --restart /home/code/h12_sim_scripts/launch_ros - `test/test_node_ros.py` runs the installed `safety_node` against a fake robot over ROS on `ROS_DOMAIN_ID` 91 (`test/conftest.py`), for both robots. `test_spinner` (the executor thread) runs on 91 too. -- `test_limit_parity.py` compares every config's derived limits with fixtures from the - previous safety layers' loaders, `test_robot_data.py` the profiles with their sources, +- `test_robot_data.py` compares the profiles with their sources, `test_chunk_files.py` the chunks with numpy and `combine_chunks.py`, and `test_no_sdk.py` that nothing imports `unitree_sdk2py`. diff --git a/core_ws/src/golem_safety/package.xml b/core_ws/src/golem_safety/package.xml index eb94a17d..db1d817e 100644 --- a/core_ws/src/golem_safety/package.xml +++ b/core_ws/src/golem_safety/package.xml @@ -18,11 +18,12 @@ launch launch_ros + + python3-numpy ament_cmake_gtest ament_cmake_pytest ament_index_python - python3-numpy python3-pytest python3-yaml rclpy diff --git a/core_ws/src/golem_safety/src/config.cpp b/core_ws/src/golem_safety/src/config.cpp index bd344604..54dbace9 100644 --- a/core_ws/src/golem_safety/src/config.cpp +++ b/core_ws/src/golem_safety/src/config.cpp @@ -7,7 +7,6 @@ #include #include #include -#include #include #include #include @@ -29,33 +28,6 @@ constexpr double kDefaultTorqueRatio = 0.5; constexpr double kDefaultKpMax = 300.0; constexpr double kDefaultKdMax = 30.0; -// What the error says about a key an older config may still carry. A key inside a removed -// section (engage.mode_machine) is reported by its own reason when it has one. -const std::map & replaced_keys() -{ - static const std::string estop_box = "removed: the bringup's use_estop decides"; - static const std::string zero_stream = - "removed: every robot streams zero gains from boot and engages each input on its own " - "first valid command"; - static const std::map keys = { - {"network", "removed: ROS supplies the domain, and the G1's interface is pinned in " - "cyclonedds_local.xml"}, - {"robot", "removed: whether the robot is simulated comes from the bringup's sim argument"}, - {"engage.mode_machine", "removed: mode_machine comes from the robot profile"}, - {"estop.enabled", estop_box}, - {"estop.estop_topic", estop_box}, - {"estop.poll_hz", estop_box}, - {"estop.upper_stale_seconds", "renamed stale.upper_seconds"}, - {"estop.state_lag_ms", "moved to control.state_lag_ms"}, - {"control.lower_stale_estop_seconds", "renamed stale.lower_seconds"}, - {"control.state_stale_estop_seconds", "renamed stale.state_seconds"}, - {"arm_sdk", "arm_sdk mode was removed: the G1 runs split_mode or full_body_mode"}, - {"startup", zero_stream}, - {"engage", zero_stream}, - }; - return keys; -} - struct Where { const std::string & robot; @@ -133,13 +105,7 @@ class Section if (std::find(names.begin(), names.end(), name) != names.end()) { continue; } - fail_on_replaced_child(name, entry.second); - std::string what = "unknown key"; - const auto replaced = replaced_keys().find(key(name)); - if (replaced != replaced_keys().end()) { - what += " (" + replaced->second + ")"; - } - fail(where_, key(name), what); + fail(where_, key(name), "unknown key"); } } @@ -301,25 +267,6 @@ class Section return list; } - // Fails on the first key of the unknown section `name` that has a reason of its own in - // replaced_keys(); returns when it has none. - void fail_on_replaced_child(const std::string & name, const YAML::Node & section) const - { - if (!section.IsMap()) { - return; - } - for (const auto & child : section) { - if (!child.first.IsScalar()) { - continue; - } - const std::string child_key = key(name) + "." + child.first.Scalar(); - const auto replaced = replaced_keys().find(child_key); - if (replaced != replaced_keys().end()) { - fail(where_, child_key, "unknown key (" + replaced->second + ")"); - } - } - } - const Where & where_; YAML::Node node_; std::string path_; @@ -356,30 +303,10 @@ LimitSet derive_limits(const RobotProfile & profile, const Section & policy) return out; } -// The section holding the clip policy: limits.clip, or its alias limits.enforce. -Section clip_section(const Section & limits) -{ - if (limits.has("clip") && limits.has("enforce")) { - limits.fail_at("enforce", "is an alias of limits.clip; give only one of them"); - } - return limits.section(limits.has("enforce") ? "enforce" : "clip"); -} - -// kp_max and kd_max, each under its own name or its alias (kp_abs, kd_abs). -double gain_limit( - const Section & clip, const std::string & name, const std::string & alias, double fallback) -{ - if (clip.has(name) && clip.has(alias)) { - clip.fail_at(alias, "is an alias of " + name + "; give only one of them"); - } - return clip.single_number(clip.has(name) ? name : alias, fallback); -} - DerivedLimits derive_all(const RobotProfile & profile, const Section & limits) { - const Section clip = clip_section(limits); - clip.allow_only( - {"position_offset", "velocity_ratio", "torque_ratio", "kp_max", "kd_max", "kp_abs", "kd_abs"}); + const Section clip = limits.section("clip"); + clip.allow_only({"position_offset", "velocity_ratio", "torque_ratio", "kp_max", "kd_max"}); const Section estop = limits.section("estop"); estop.allow_only({"position_offset", "velocity_ratio", "torque_ratio"}); @@ -389,8 +316,8 @@ DerivedLimits derive_all(const RobotProfile & profile, const Section & limits) out.q_clip_high = clipped.q_high; out.dq_clip = clipped.dq; out.tau_clip = clipped.tau; - out.kp_max = gain_limit(clip, "kp_max", "kp_abs", kDefaultKpMax); - out.kd_max = gain_limit(clip, "kd_max", "kd_abs", kDefaultKdMax); + out.kp_max = clip.single_number("kp_max", kDefaultKpMax); + out.kd_max = clip.single_number("kd_max", kDefaultKdMax); const LimitSet estopped = derive_limits(profile, estop); out.q_estop_low = estopped.q_low; @@ -486,7 +413,7 @@ SafetyConfig parse_checked( config.estop.timeout_seconds = estop.positive("timeout_seconds", config.estop.timeout_seconds); const Section limits = root.section("limits"); - limits.allow_only({"clip", "enforce", "estop"}); + limits.allow_only({"clip", "estop"}); config.limits = derive_all(profile, limits); config.estop_response = parse_estop_response(root.section("estop_response"), config.limits); diff --git a/core_ws/src/golem_safety/src/dump_config_main.cpp b/core_ws/src/golem_safety/src/dump_config_main.cpp index fe87710e..012d3c42 100644 --- a/core_ws/src/golem_safety/src/dump_config_main.cpp +++ b/core_ws/src/golem_safety/src/dump_config_main.cpp @@ -1,8 +1,7 @@ // golem_safety_dump_config // // Loads a safety config with the node's own loader and prints what it derived as -// JSON: the derived limits and every parsed value, under the schema's keys. The -// limit parity test compares this output to what the old safety layers derive. +// JSON: the derived limits and every parsed value, under the schema's keys. #include #include #include diff --git a/core_ws/src/golem_safety/test/fixtures/README.md b/core_ws/src/golem_safety/test/fixtures/README.md index c5116853..583945f4 100644 --- a/core_ws/src/golem_safety/test/fixtures/README.md +++ b/core_ws/src/golem_safety/test/fixtures/README.md @@ -18,19 +18,12 @@ The tools that wrote them were deleted with it. |---|---|---| | `h12_joint_limits.json` | The H1-2's joint table as `h12_safety_layer/core/joint_limits.py` held it: `joint_names`, `position_limits` (`low`/`high`, rad), `velocity_limits` (rad/s), `torque_limits` (N*m), in slot order. It is the H1-2's table, not the URDF's: shoulder roll is +-0.38 rad here and +-0.19 rad in `h1_2_magpie.urdf`. | `test_robot_data.py`: `robots/h12.yaml` must hold the same values | | `g1_joint_limits.json` | The G1's body joint table as `g1_description.joints.BODY` held it, in `h12_joint_limits.json`'s format and slot order. These are Unitree's URDF values. `tools/g1_joint_limits.py` holds the same values, and `tools/generate_robot_data.py` writes `robots/g1.yaml` from it. | `test_robot_data.py`: `robots/g1.yaml` must hold the same values | -| `limits//.json` | One file per safety config of `h12_safety_layer` (`h12/`) and `g1_safety_layer` (`g1/`), from that package's own `load_config`. `limits`: the derived clip and estop arrays by body slot (`q_clip_low`, `q_clip_high`, `dq_clip`, `tau_clip`, `q_estop_low`, `q_estop_high`, `dq_estop`, `tau_estop`) and the single numbers `kp_max` and `kd_max`. `old`: the other values the old loader returned, under their old meaning: `mode`, `topics` (`rt/` names), `publish_hz`, the stale times, `state_lag_ms`, the engage gate, `estop_response` and `logging`. | `test_limit_parity.py`, against `golem_safety_dump_config` | | `lowcmd_crc.json` | `commands`: four `unitree_hg` `LowCmd` messages with known field values, one list per field over the 35 motor slots, and the CRC `unitree_sdk2py`'s `CRC().Crc` computed for each (field values rounded to float32 first). `old_commands`: the estop command each old layer wrote, with the header `golem_safety` stamps (`mode_machine` 6 for the H1-2, 5 for the G1) and the SDK's CRC for it: `h12_estop` is `make_estop_cmd(LowCmdDefault())` of `h12_safety_layer`, `g1_damp` is `make_estop_cmd(template, kd)` of `g1_safety_layer` with its input in `kd_input`. The `generator` field names `tools/crc_fixtures.py`, the tool that wrote the file and has been deleted. | `test_lowcmd.cpp` | ## What was changed after recording -- `limits/*/*.json`: `old.logging.base_dir` was `/tmp/h12_safety_layer` and - `/tmp/g1_safety_layer`; it is `/tmp/golem_safety` now, the value every config carries. -- `limits/*/*.json`: `old` held the old loaders' settings of the G1 mode that shared the - waist and arms with Unitree's controller through a blend weight in slot 29 (null on the - H1-2). `golem_safety` no longer has that mode: the entry is gone, and so are the fixtures - of its two G1 configs. The other values are unchanged. -- `lowcmd_crc.json`: `commands` held a fifth message, a G1 command with that blend weight - in slot 29; it is gone. The other four and `old_commands` are unchanged. +- `lowcmd_crc.json`: `commands` held a fifth message, a G1 command with a blend weight in + slot 29 for a mode `golem_safety` does not have; it is gone. The other four and `old_commands` are unchanged. - `h12_joint_limits.json`: `source` held `h12_safety_layer/core/joint_limits.py`; it points here. The table's values are unchanged. `tools/h12_joint_limits.py` is a copy of that file, and `tools/generate_robot_data.py` writes `robots/h12.yaml` from it. diff --git a/core_ws/src/golem_safety/test/fixtures/limits/g1/full.json b/core_ws/src/golem_safety/test/fixtures/limits/g1/full.json deleted file mode 100644 index 5530ae09..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/g1/full.json +++ /dev/null @@ -1,316 +0,0 @@ -{ - "robot": "g1", - "config": "full", - "limits": { - "q_clip_low": [ - -2.5207, - -0.5136, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.5207, - -2.9571, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.608, - -0.51, - -0.51, - -3.0792, - -1.5782, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558, - -3.0792, - -2.2415000000000003, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558 - ], - "q_clip_high": [ - 2.8698, - 2.9571, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.8698, - 0.5136, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.608, - 0.51, - 0.51, - 2.6604, - 2.2415000000000003, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558, - 2.6604, - 1.5782, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558 - ], - "dq_clip": [ - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 3.0, - 3.0, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2 - ], - "tau_clip": [ - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 21.0, - 21.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0 - ], - "q_estop_low": [ - -2.5307, - -0.5236, - -2.7576, - -0.087267, - -0.87267, - -0.2618, - -2.5307, - -2.9671, - -2.7576, - -0.087267, - -0.87267, - -0.2618, - -2.618, - -0.52, - -0.52, - -3.0892, - -1.5882, - -2.618, - -1.0472, - -1.972222054, - -1.614429558, - -1.614429558, - -3.0892, - -2.2515, - -2.618, - -1.0472, - -1.972222054, - -1.614429558, - -1.614429558 - ], - "q_estop_high": [ - 2.8798, - 2.9671, - 2.7576, - 2.8798, - 0.5236, - 0.2618, - 2.8798, - 0.5236, - 2.7576, - 2.8798, - 0.5236, - 0.2618, - 2.618, - 0.52, - 0.52, - 2.6704, - 2.2515, - 2.618, - 2.0944, - 1.972222054, - 1.614429558, - 1.614429558, - 2.6704, - 1.5882, - 2.618, - 2.0944, - 1.972222054, - 1.614429558, - 1.614429558 - ], - "dq_estop": [ - 16.0, - 10.0, - 16.0, - 10.0, - 15.0, - 15.0, - 16.0, - 10.0, - 16.0, - 10.0, - 15.0, - 15.0, - 16.0, - 15.0, - 15.0, - 18.5, - 18.5, - 18.5, - 18.5, - 18.5, - 11.0, - 11.0, - 18.5, - 18.5, - 18.5, - 18.5, - 18.5, - 11.0, - 11.0 - ], - "tau_estop": [ - 79.2, - 125.10000000000001, - 79.2, - 125.10000000000001, - 31.5, - 31.5, - 79.2, - 125.10000000000001, - 79.2, - 125.10000000000001, - 31.5, - 31.5, - 79.2, - 31.5, - 31.5, - 22.5, - 22.5, - 22.5, - 22.5, - 22.5, - 4.5, - 4.5, - 22.5, - 22.5, - 22.5, - 22.5, - 22.5, - 4.5, - 4.5 - ], - "kp_max": 200.0, - "kd_max": 10.0 - }, - "old": { - "mode": "full_body_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 2.0, - "upper_stale_active": false, - "lower_stale_seconds": 0.2, - "state_stale_seconds": 0.1, - "state_lag_ms": null, - "engage": { - "max_state_age_seconds": 0.1, - "mode_machine": 5, - "max_position_error": 0.15 - }, - "estop_response": { - "mode": "zero", - "kd": [ - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0 - ] - }, - "logging": { - "enabled": true, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/g1/sim_split.json b/core_ws/src/golem_safety/test/fixtures/limits/g1/sim_split.json deleted file mode 100644 index db2d8b0a..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/g1/sim_split.json +++ /dev/null @@ -1,316 +0,0 @@ -{ - "robot": "g1", - "config": "sim_split", - "limits": { - "q_clip_low": [ - -2.5207, - -0.5136, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.5207, - -2.9571, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.608, - -0.51, - -0.51, - -3.0792, - -1.5782, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558, - -3.0792, - -2.2415000000000003, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558 - ], - "q_clip_high": [ - 2.8698, - 2.9571, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.8698, - 0.5136, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.608, - 0.51, - 0.51, - 2.6604, - 2.2415000000000003, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558, - 2.6604, - 1.5782, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558 - ], - "dq_clip": [ - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 3.0, - 3.0, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2 - ], - "tau_clip": [ - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 21.0, - 21.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0 - ], - "q_estop_low": [ - -2.6306999999999996, - -0.6235999999999999, - -2.8575999999999997, - -0.187267, - -0.9726699999999999, - -0.3618, - -2.6306999999999996, - -3.0670999999999995, - -2.8575999999999997, - -0.187267, - -0.9726699999999999, - -0.3618, - -2.7179999999999995, - -0.62, - -0.62, - -3.1891999999999996, - -1.6882000000000001, - -2.7179999999999995, - -1.1472, - -2.072222054, - -1.7144295580000002, - -1.7144295580000002, - -3.1891999999999996, - -2.3514999999999997, - -2.7179999999999995, - -1.1472, - -2.072222054, - -1.7144295580000002, - -1.7144295580000002 - ], - "q_estop_high": [ - 2.9797999999999996, - 3.0670999999999995, - 2.8575999999999997, - 2.9797999999999996, - 0.6235999999999999, - 0.3618, - 2.9797999999999996, - 0.6235999999999999, - 2.8575999999999997, - 2.9797999999999996, - 0.6235999999999999, - 0.3618, - 2.7179999999999995, - 0.62, - 0.62, - 2.7703999999999995, - 2.3514999999999997, - 2.7179999999999995, - 2.1943999999999995, - 2.072222054, - 1.7144295580000002, - 1.7144295580000002, - 2.7703999999999995, - 1.6882000000000001, - 2.7179999999999995, - 2.1943999999999995, - 2.072222054, - 1.7144295580000002, - 1.7144295580000002 - ], - "dq_estop": [ - 64.0, - 40.0, - 64.0, - 40.0, - 60.0, - 60.0, - 64.0, - 40.0, - 64.0, - 40.0, - 60.0, - 60.0, - 64.0, - 60.0, - 60.0, - 74.0, - 74.0, - 74.0, - 74.0, - 74.0, - 44.0, - 44.0, - 74.0, - 74.0, - 74.0, - 74.0, - 74.0, - 44.0, - 44.0 - ], - "tau_estop": [ - 176.0, - 278.0, - 176.0, - 278.0, - 70.0, - 70.0, - 176.0, - 278.0, - 176.0, - 278.0, - 70.0, - 70.0, - 176.0, - 70.0, - 70.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 10.0, - 10.0, - 50.0, - 50.0, - 50.0, - 50.0, - 50.0, - 10.0, - 10.0 - ], - "kp_max": 200.0, - "kd_max": 10.0 - }, - "old": { - "mode": "split_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 2.0, - "upper_stale_active": true, - "lower_stale_seconds": 1.0, - "state_stale_seconds": 1.0, - "state_lag_ms": null, - "engage": { - "max_state_age_seconds": 1.0, - "mode_machine": 5, - "max_position_error": 0.15 - }, - "estop_response": { - "mode": "zero", - "kd": [ - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0 - ] - }, - "logging": { - "enabled": true, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/g1/split.json b/core_ws/src/golem_safety/test/fixtures/limits/g1/split.json deleted file mode 100644 index a323a228..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/g1/split.json +++ /dev/null @@ -1,316 +0,0 @@ -{ - "robot": "g1", - "config": "split", - "limits": { - "q_clip_low": [ - -2.5207, - -0.5136, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.5207, - -2.9571, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.608, - -0.51, - -0.51, - -3.0792, - -1.5782, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558, - -3.0792, - -2.2415000000000003, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558 - ], - "q_clip_high": [ - 2.8698, - 2.9571, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.8698, - 0.5136, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.608, - 0.51, - 0.51, - 2.6604, - 2.2415000000000003, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558, - 2.6604, - 1.5782, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558 - ], - "dq_clip": [ - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 3.0, - 3.0, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2 - ], - "tau_clip": [ - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 21.0, - 21.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0 - ], - "q_estop_low": [ - -2.5307, - -0.5236, - -2.7576, - -0.087267, - -0.87267, - -0.2618, - -2.5307, - -2.9671, - -2.7576, - -0.087267, - -0.87267, - -0.2618, - -2.618, - -0.52, - -0.52, - -3.0892, - -1.5882, - -2.618, - -1.0472, - -1.972222054, - -1.614429558, - -1.614429558, - -3.0892, - -2.2515, - -2.618, - -1.0472, - -1.972222054, - -1.614429558, - -1.614429558 - ], - "q_estop_high": [ - 2.8798, - 2.9671, - 2.7576, - 2.8798, - 0.5236, - 0.2618, - 2.8798, - 0.5236, - 2.7576, - 2.8798, - 0.5236, - 0.2618, - 2.618, - 0.52, - 0.52, - 2.6704, - 2.2515, - 2.618, - 2.0944, - 1.972222054, - 1.614429558, - 1.614429558, - 2.6704, - 1.5882, - 2.618, - 2.0944, - 1.972222054, - 1.614429558, - 1.614429558 - ], - "dq_estop": [ - 16.0, - 10.0, - 16.0, - 10.0, - 15.0, - 15.0, - 16.0, - 10.0, - 16.0, - 10.0, - 15.0, - 15.0, - 16.0, - 15.0, - 15.0, - 18.5, - 18.5, - 18.5, - 18.5, - 18.5, - 11.0, - 11.0, - 18.5, - 18.5, - 18.5, - 18.5, - 18.5, - 11.0, - 11.0 - ], - "tau_estop": [ - 79.2, - 125.10000000000001, - 79.2, - 125.10000000000001, - 31.5, - 31.5, - 79.2, - 125.10000000000001, - 79.2, - 125.10000000000001, - 31.5, - 31.5, - 79.2, - 31.5, - 31.5, - 22.5, - 22.5, - 22.5, - 22.5, - 22.5, - 4.5, - 4.5, - 22.5, - 22.5, - 22.5, - 22.5, - 22.5, - 4.5, - 4.5 - ], - "kp_max": 200.0, - "kd_max": 10.0 - }, - "old": { - "mode": "split_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 2.0, - "upper_stale_active": true, - "lower_stale_seconds": 0.2, - "state_stale_seconds": 0.1, - "state_lag_ms": null, - "engage": { - "max_state_age_seconds": 0.1, - "mode_machine": 5, - "max_position_error": 0.15 - }, - "estop_response": { - "mode": "zero", - "kd": [ - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0, - 8.0 - ] - }, - "logging": { - "enabled": true, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/h12/default_safety_full.json b/core_ws/src/golem_safety/test/fixtures/limits/h12/default_safety_full.json deleted file mode 100644 index 0786e716..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/h12/default_safety_full.json +++ /dev/null @@ -1,263 +0,0 @@ -{ - "robot": "h12", - "config": "default_safety_full", - "limits": { - "q_clip_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_clip_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq_clip": [ - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14 - ], - "tau_clip": [ - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4 - ], - "q_estop_low": [ - -0.4299, - -3.1399, - -0.4299, - -0.11989999999999999, - -0.897234, - -0.261699, - -0.4299, - -3.1399, - -3.1399, - -0.11989999999999999, - -0.897234, - -0.261699, - -2.3499, - -3.1399, - -0.3799, - -2.6599, - -0.9499, - -3.0098999999999996, - -0.46240000000000003, - -1.2699, - -3.1399, - -3.3998999999999997, - -3.0098999999999996, - -0.9499, - -2.7499, - -0.46240000000000003, - -1.2699 - ], - "q_estop_high": [ - 0.4299, - 2.4999, - 3.1399, - 2.1898999999999997, - 0.523498, - 0.261699, - 0.4299, - 2.4999, - 0.4299, - 2.1898999999999997, - 0.523498, - 0.261699, - 2.3499, - 1.5699, - 3.3998999999999997, - 3.0098999999999996, - 3.1799, - 2.7499, - 0.46240000000000003, - 1.2699, - 1.5699, - 0.3799, - 2.6599, - 3.1799, - 3.0098999999999996, - 0.46240000000000003, - 1.2699 - ], - "dq_estop": [ - 4.6000000000000005, - 5.75, - 4.6000000000000005, - 8.4, - 27.0, - 27.0, - 4.6000000000000005, - 5.75, - 4.6000000000000005, - 8.4, - 27.0, - 27.0, - 3.4499999999999997, - 4.5, - 4.5, - 10.0, - 6.0, - 9.42, - 9.42, - 9.42, - 4.5, - 4.5, - 10.0, - 6.0, - 9.42, - 9.42, - 9.42 - ], - "tau_estop": [ - 100.0, - 140.0, - 200.0, - 300.0, - 54.0, - 36.0, - 100.0, - 140.0, - 200.0, - 300.0, - 54.0, - 36.0, - 60.0, - 36.0, - 36.0, - 16.2, - 16.2, - 17.1, - 17.1, - 17.1, - 36.0, - 36.0, - 16.2, - 16.2, - 17.1, - 17.1, - 17.1 - ], - "kp_max": 500.0, - "kd_max": 30.0 - }, - "old": { - "mode": "full_body_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 2.0, - "upper_stale_active": false, - "lower_stale_seconds": null, - "state_stale_seconds": null, - "state_lag_ms": 100.0, - "engage": null, - "estop_response": null, - "logging": { - "enabled": false, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/h12/default_safety_split.json b/core_ws/src/golem_safety/test/fixtures/limits/h12/default_safety_split.json deleted file mode 100644 index eca29aad..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/h12/default_safety_split.json +++ /dev/null @@ -1,263 +0,0 @@ -{ - "robot": "h12", - "config": "default_safety_split", - "limits": { - "q_clip_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_clip_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq_clip": [ - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14 - ], - "tau_clip": [ - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4 - ], - "q_estop_low": [ - -1.44, - -4.15, - -1.44, - -1.13, - -1.907334, - -1.2717990000000001, - -1.44, - -4.15, - -4.15, - -1.13, - -1.907334, - -1.2717990000000001, - -3.3600000000000003, - -4.15, - -1.3900000000000001, - -3.67, - -1.96, - -4.02, - -1.4725000000000001, - -2.2800000000000002, - -4.15, - -4.41, - -4.02, - -1.96, - -3.76, - -1.4725000000000001, - -2.2800000000000002 - ], - "q_estop_high": [ - 1.44, - 3.51, - 4.15, - 3.2, - 1.533598, - 1.2717990000000001, - 1.44, - 3.51, - 1.44, - 3.2, - 1.533598, - 1.2717990000000001, - 3.3600000000000003, - 2.58, - 4.41, - 4.02, - 4.19, - 3.76, - 1.4725000000000001, - 2.2800000000000002, - 2.58, - 1.3900000000000001, - 3.67, - 4.19, - 4.02, - 1.4725000000000001, - 2.2800000000000002 - ], - "dq_estop": [ - 4.6000000000000005, - 5.75, - 4.6000000000000005, - 8.4, - 27.0, - 27.0, - 4.6000000000000005, - 5.75, - 4.6000000000000005, - 8.4, - 27.0, - 27.0, - 3.4499999999999997, - 4.5, - 4.5, - 10.0, - 6.0, - 9.42, - 9.42, - 9.42, - 4.5, - 4.5, - 10.0, - 6.0, - 9.42, - 9.42, - 9.42 - ], - "tau_estop": [ - 100.0, - 140.0, - 200.0, - 300.0, - 54.0, - 36.0, - 100.0, - 140.0, - 200.0, - 300.0, - 54.0, - 36.0, - 60.0, - 36.0, - 36.0, - 16.2, - 16.2, - 17.1, - 17.1, - 17.1, - 36.0, - 36.0, - 16.2, - 16.2, - 17.1, - 17.1, - 17.1 - ], - "kp_max": 500.0, - "kd_max": 30.0 - }, - "old": { - "mode": "split_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 2.0, - "upper_stale_active": true, - "lower_stale_seconds": null, - "state_stale_seconds": null, - "state_lag_ms": 100.0, - "engage": null, - "estop_response": null, - "logging": { - "enabled": false, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/h12/locomotion_safety_full.json b/core_ws/src/golem_safety/test/fixtures/limits/h12/locomotion_safety_full.json deleted file mode 100644 index 8380938f..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/h12/locomotion_safety_full.json +++ /dev/null @@ -1,263 +0,0 @@ -{ - "robot": "h12", - "config": "locomotion_safety_full", - "limits": { - "q_clip_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_clip_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq_clip": [ - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14 - ], - "tau_clip": [ - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4 - ], - "q_estop_low": [ - -0.4299, - -3.1399, - -0.4299, - -0.11989999999999999, - -0.897234, - -0.261699, - -0.4299, - -3.1399, - -3.1399, - -0.11989999999999999, - -0.897234, - -0.261699, - -2.3499, - -3.1399, - -0.3799, - -2.6599, - -0.9499, - -3.0098999999999996, - -0.46240000000000003, - -1.2699, - -3.1399, - -3.3998999999999997, - -3.0098999999999996, - -0.9499, - -2.7499, - -0.46240000000000003, - -1.2699 - ], - "q_estop_high": [ - 0.4299, - 2.4999, - 3.1399, - 2.1898999999999997, - 0.523498, - 0.261699, - 0.4299, - 2.4999, - 0.4299, - 2.1898999999999997, - 0.523498, - 0.261699, - 2.3499, - 1.5699, - 3.3998999999999997, - 3.0098999999999996, - 3.1799, - 2.7499, - 0.46240000000000003, - 1.2699, - 1.5699, - 0.3799, - 2.6599, - 3.1799, - 3.0098999999999996, - 0.46240000000000003, - 1.2699 - ], - "dq_estop": [ - 6.44, - 8.049999999999999, - 7.36, - 10.08, - 27.0, - 27.0, - 6.44, - 8.049999999999999, - 7.36, - 10.08, - 27.0, - 27.0, - 5.06, - 5.3999999999999995, - 5.3999999999999995, - 12.0, - 8.0, - 12.56, - 12.56, - 12.56, - 5.3999999999999995, - 5.3999999999999995, - 12.0, - 8.0, - 12.56, - 12.56, - 12.56 - ], - "tau_estop": [ - 60.0, - 130.0, - 200.0, - 300.0, - 54.0, - 36.0, - 60.0, - 130.0, - 200.0, - 300.0, - 54.0, - 36.0, - 40.0, - 32.0, - 32.0, - 14.4, - 14.4, - 9.5, - 9.5, - 9.5, - 32.0, - 32.0, - 14.4, - 14.4, - 9.5, - 9.5, - 9.5 - ], - "kp_max": 500.0, - "kd_max": 30.0 - }, - "old": { - "mode": "full_body_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 2.0, - "upper_stale_active": false, - "lower_stale_seconds": null, - "state_stale_seconds": null, - "state_lag_ms": 100.0, - "engage": null, - "estop_response": null, - "logging": { - "enabled": false, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/h12/relax_safety_full.json b/core_ws/src/golem_safety/test/fixtures/limits/h12/relax_safety_full.json deleted file mode 100644 index e5f106b4..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/h12/relax_safety_full.json +++ /dev/null @@ -1,263 +0,0 @@ -{ - "robot": "h12", - "config": "relax_safety_full", - "limits": { - "q_clip_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_clip_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq_clip": [ - 23.0, - 23.0, - 23.0, - 14.0, - 9.0, - 9.0, - 23.0, - 23.0, - 23.0, - 14.0, - 9.0, - 9.0, - 23.0, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4 - ], - "tau_clip": [ - 200.0, - 200.0, - 200.0, - 300.0, - 60.0, - 40.0, - 200.0, - 200.0, - 200.0, - 300.0, - 60.0, - 40.0, - 200.0, - 40.0, - 40.0, - 18.0, - 18.0, - 19.0, - 19.0, - 19.0, - 40.0, - 40.0, - 18.0, - 18.0, - 19.0, - 19.0, - 19.0 - ], - "q_estop_low": [ - -0.4299, - -3.1399, - -0.4299, - -0.11989999999999999, - -0.897234, - -0.261699, - -0.4299, - -3.1399, - -3.1399, - -0.11989999999999999, - -0.897234, - -0.261699, - -2.3499, - -3.1399, - -0.3799, - -2.6599, - -0.9499, - -3.0098999999999996, - -0.46240000000000003, - -1.2699, - -3.1399, - -3.3998999999999997, - -3.0098999999999996, - -0.9499, - -2.7499, - -0.46240000000000003, - -1.2699 - ], - "q_estop_high": [ - 0.4299, - 2.4999, - 3.1399, - 2.1898999999999997, - 0.523498, - 0.261699, - 0.4299, - 2.4999, - 0.4299, - 2.1898999999999997, - 0.523498, - 0.261699, - 2.3499, - 1.5699, - 3.3998999999999997, - 3.0098999999999996, - 3.1799, - 2.7499, - 0.46240000000000003, - 1.2699, - 1.5699, - 0.3799, - 2.6599, - 3.1799, - 3.0098999999999996, - 0.46240000000000003, - 1.2699 - ], - "dq_estop": [ - 23.0, - 23.0, - 23.0, - 14.0, - 27.0, - 27.0, - 23.0, - 23.0, - 23.0, - 14.0, - 27.0, - 27.0, - 23.0, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4 - ], - "tau_estop": [ - 200.0, - 200.0, - 200.0, - 300.0, - 60.0, - 40.0, - 200.0, - 200.0, - 200.0, - 300.0, - 60.0, - 40.0, - 200.0, - 40.0, - 40.0, - 18.0, - 18.0, - 19.0, - 19.0, - 19.0, - 40.0, - 40.0, - 18.0, - 18.0, - 19.0, - 19.0, - 19.0 - ], - "kp_max": 500.0, - "kd_max": 30.0 - }, - "old": { - "mode": "full_body_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 2.0, - "upper_stale_active": false, - "lower_stale_seconds": null, - "state_stale_seconds": null, - "state_lag_ms": 100.0, - "engage": null, - "estop_response": null, - "logging": { - "enabled": false, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/h12/relax_safety_split.json b/core_ws/src/golem_safety/test/fixtures/limits/h12/relax_safety_split.json deleted file mode 100644 index 21d81617..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/h12/relax_safety_split.json +++ /dev/null @@ -1,263 +0,0 @@ -{ - "robot": "h12", - "config": "relax_safety_split", - "limits": { - "q_clip_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_clip_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq_clip": [ - 23.0, - 23.0, - 23.0, - 14.0, - 9.0, - 9.0, - 23.0, - 23.0, - 23.0, - 14.0, - 9.0, - 9.0, - 23.0, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4 - ], - "tau_clip": [ - 200.0, - 200.0, - 200.0, - 300.0, - 60.0, - 40.0, - 200.0, - 200.0, - 200.0, - 300.0, - 60.0, - 40.0, - 200.0, - 40.0, - 40.0, - 18.0, - 19.9998, - 19.0, - 19.0, - 19.0, - 40.0, - 40.0, - 18.0, - 19.9998, - 19.0, - 19.0, - 19.0 - ], - "q_estop_low": [ - -0.4299, - -3.1399, - -0.4299, - -0.11989999999999999, - -0.897234, - -0.261699, - -0.4299, - -3.1399, - -3.1399, - -0.11989999999999999, - -0.897234, - -0.261699, - -2.3499, - -3.1399, - -0.3799, - -2.6599, - -0.9499, - -3.0098999999999996, - -0.5625, - -1.37, - -3.1399, - -3.3998999999999997, - -3.0098999999999996, - -0.9499, - -2.7499, - -0.5625, - -1.37 - ], - "q_estop_high": [ - 0.4299, - 2.4999, - 3.1399, - 2.1898999999999997, - 0.523498, - 0.261699, - 0.4299, - 2.4999, - 0.4299, - 2.1898999999999997, - 0.523498, - 0.261699, - 2.3499, - 1.5699, - 3.3998999999999997, - 3.0098999999999996, - 3.1799, - 2.7499, - 0.5625, - 1.37, - 1.5699, - 0.3799, - 2.6599, - 3.1799, - 3.0098999999999996, - 0.5625, - 1.37 - ], - "dq_estop": [ - 23.0, - 23.0, - 23.0, - 14.0, - 27.0, - 27.0, - 23.0, - 23.0, - 23.0, - 14.0, - 27.0, - 27.0, - 23.0, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4 - ], - "tau_estop": [ - 200.0, - 200.0, - 200.0, - 300.0, - 95.0004, - 40.0, - 200.0, - 200.0, - 200.0, - 300.0, - 95.0004, - 40.0, - 200.0, - 40.0, - 40.0, - 18.0, - 21.599999999999998, - 19.0, - 38.0, - 38.0, - 40.0, - 40.0, - 18.0, - 21.599999999999998, - 19.0, - 38.0, - 38.0 - ], - "kp_max": 500.0, - "kd_max": 30.0 - }, - "old": { - "mode": "split_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 0.0, - "upper_stale_active": true, - "lower_stale_seconds": null, - "state_stale_seconds": null, - "state_lag_ms": 100.0, - "engage": null, - "estop_response": null, - "logging": { - "enabled": false, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/h12/sim_safety_split.json b/core_ws/src/golem_safety/test/fixtures/limits/h12/sim_safety_split.json deleted file mode 100644 index 7cdaf43c..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/h12/sim_safety_split.json +++ /dev/null @@ -1,263 +0,0 @@ -{ - "robot": "h12", - "config": "sim_safety_split", - "limits": { - "q_clip_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_clip_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq_clip": [ - 69.0, - 69.0, - 69.0, - 42.0, - 27.0, - 27.0, - 69.0, - 69.0, - 69.0, - 42.0, - 27.0, - 27.0, - 69.0, - 27.0, - 27.0, - 60.0, - 60.0, - 94.19999999999999, - 94.19999999999999, - 94.19999999999999, - 27.0, - 27.0, - 60.0, - 60.0, - 94.19999999999999, - 94.19999999999999, - 94.19999999999999 - ], - "tau_clip": [ - 600.0, - 600.0, - 600.0, - 900.0, - 180.0, - 120.0, - 600.0, - 600.0, - 600.0, - 900.0, - 180.0, - 120.0, - 600.0, - 120.0, - 120.0, - 54.0, - 54.0, - 57.0, - 57.0, - 57.0, - 120.0, - 120.0, - 54.0, - 54.0, - 57.0, - 57.0, - 57.0 - ], - "q_estop_low": [ - -0.53, - -3.24, - -0.53, - -0.22, - -0.9973339999999999, - -0.361799, - -0.53, - -3.24, - -3.24, - -0.22, - -0.9973339999999999, - -0.361799, - -2.45, - -3.24, - -0.48, - -2.7600000000000002, - -1.05, - -3.11, - -0.5625, - -1.37, - -3.24, - -3.5, - -3.11, - -1.05, - -2.85, - -0.5625, - -1.37 - ], - "q_estop_high": [ - 0.53, - 2.6, - 3.24, - 2.29, - 0.623598, - 0.361799, - 0.53, - 2.6, - 0.53, - 2.29, - 0.623598, - 0.361799, - 2.45, - 1.6700000000000002, - 3.5, - 3.11, - 3.2800000000000002, - 2.85, - 0.5625, - 1.37, - 1.6700000000000002, - 0.48, - 2.7600000000000002, - 3.2800000000000002, - 3.11, - 0.5625, - 1.37 - ], - "dq_estop": [ - 69.0, - 69.0, - 69.0, - 42.0, - 27.0, - 27.0, - 69.0, - 69.0, - 69.0, - 42.0, - 27.0, - 27.0, - 69.0, - 27.0, - 27.0, - 60.0, - 60.0, - 94.19999999999999, - 94.19999999999999, - 94.19999999999999, - 27.0, - 27.0, - 60.0, - 60.0, - 94.19999999999999, - 94.19999999999999, - 94.19999999999999 - ], - "tau_estop": [ - 600.0, - 600.0, - 600.0, - 900.0, - 180.0, - 120.0, - 600.0, - 600.0, - 600.0, - 900.0, - 180.0, - 120.0, - 600.0, - 120.0, - 120.0, - 54.0, - 54.0, - 57.0, - 57.0, - 57.0, - 120.0, - 120.0, - 54.0, - 54.0, - 57.0, - 57.0, - 57.0 - ], - "kp_max": 500.0, - "kd_max": 30.0 - }, - "old": { - "mode": "split_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 0.0, - "upper_stale_active": true, - "lower_stale_seconds": null, - "state_stale_seconds": null, - "state_lag_ms": 100.0, - "engage": null, - "estop_response": null, - "logging": { - "enabled": false, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/h12/tight_safety_full.json b/core_ws/src/golem_safety/test/fixtures/limits/h12/tight_safety_full.json deleted file mode 100644 index ceaecba1..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/h12/tight_safety_full.json +++ /dev/null @@ -1,263 +0,0 @@ -{ - "robot": "h12", - "config": "tight_safety_full", - "limits": { - "q_clip_low": [ - -0.38, - -3.0900000000000003, - -0.38, - -0.06999999999999999, - -0.8473339999999999, - -0.21179900000000002, - -0.38, - -3.0900000000000003, - -3.0900000000000003, - -0.06999999999999999, - -0.8473339999999999, - -0.21179900000000002, - -2.3000000000000003, - -3.0900000000000003, - -0.33, - -2.6100000000000003, - -0.8999999999999999, - -2.96, - -0.41250000000000003, - -1.22, - -3.0900000000000003, - -3.35, - -2.96, - -0.8999999999999999, - -2.7, - -0.41250000000000003, - -1.22 - ], - "q_clip_high": [ - 0.38, - 2.45, - 3.0900000000000003, - 2.14, - 0.473598, - 0.21179900000000002, - 0.38, - 2.45, - 0.38, - 2.14, - 0.473598, - 0.21179900000000002, - 2.3000000000000003, - 1.52, - 3.35, - 2.96, - 3.1300000000000003, - 2.7, - 0.41250000000000003, - 1.22, - 1.52, - 0.33, - 2.6100000000000003, - 3.1300000000000003, - 2.96, - 0.41250000000000003, - 1.22 - ], - "dq_clip": [ - 1.84, - 1.84, - 1.84, - 1.12, - 0.72, - 0.72, - 1.84, - 1.84, - 1.84, - 1.12, - 0.72, - 0.72, - 1.84, - 0.72, - 0.72, - 1.6, - 1.6, - 2.512, - 2.512, - 2.512, - 0.72, - 0.72, - 1.6, - 1.6, - 2.512, - 2.512, - 2.512 - ], - "tau_clip": [ - 50.0, - 50.0, - 50.0, - 75.0, - 15.0, - 10.0, - 50.0, - 50.0, - 50.0, - 75.0, - 15.0, - 10.0, - 50.0, - 10.0, - 10.0, - 4.5, - 4.5, - 4.75, - 4.75, - 4.75, - 10.0, - 10.0, - 4.5, - 4.5, - 4.75, - 4.75, - 4.75 - ], - "q_estop_low": [ - -0.428, - -3.1380000000000003, - -0.428, - -0.118, - -0.895334, - -0.259799, - -0.428, - -3.1380000000000003, - -3.1380000000000003, - -0.118, - -0.895334, - -0.259799, - -2.3480000000000003, - -3.1380000000000003, - -0.378, - -2.6580000000000004, - -0.948, - -3.008, - -0.4605, - -1.268, - -3.1380000000000003, - -3.398, - -3.008, - -0.948, - -2.748, - -0.4605, - -1.268 - ], - "q_estop_high": [ - 0.428, - 2.498, - 3.1380000000000003, - 2.188, - 0.521598, - 0.259799, - 0.428, - 2.498, - 0.428, - 2.188, - 0.521598, - 0.259799, - 2.3480000000000003, - 1.568, - 3.398, - 3.008, - 3.1780000000000004, - 2.748, - 0.4605, - 1.268, - 1.568, - 0.378, - 2.6580000000000004, - 3.1780000000000004, - 3.008, - 0.4605, - 1.268 - ], - "dq_estop": [ - 3.4499999999999997, - 3.4499999999999997, - 4.6000000000000005, - 6.3, - 3.6, - 3.15, - 3.4499999999999997, - 3.4499999999999997, - 4.6000000000000005, - 6.3, - 3.6, - 3.15, - 2.3000000000000003, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14 - ], - "tau_estop": [ - 60.0, - 60.0, - 200.0, - 120.0, - 54.0, - 36.0, - 60.0, - 60.0, - 200.0, - 120.0, - 54.0, - 36.0, - 20.0, - 20.0, - 20.0, - 9.0, - 12.6, - 7.6000000000000005, - 5.7, - 5.7, - 20.0, - 20.0, - 9.0, - 12.6, - 7.6000000000000005, - 5.7, - 5.7 - ], - "kp_max": 500.0, - "kd_max": 30.0 - }, - "old": { - "mode": "full_body_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 2.0, - "upper_stale_active": false, - "lower_stale_seconds": null, - "state_stale_seconds": null, - "state_lag_ms": 100.0, - "engage": null, - "estop_response": null, - "logging": { - "enabled": false, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/fixtures/limits/h12/tight_safety_split.json b/core_ws/src/golem_safety/test/fixtures/limits/h12/tight_safety_split.json deleted file mode 100644 index ba6c893e..00000000 --- a/core_ws/src/golem_safety/test/fixtures/limits/h12/tight_safety_split.json +++ /dev/null @@ -1,263 +0,0 @@ -{ - "robot": "h12", - "config": "tight_safety_split", - "limits": { - "q_clip_low": [ - -0.38, - -3.0900000000000003, - -0.38, - -0.06999999999999999, - -0.8473339999999999, - -0.21179900000000002, - -0.38, - -3.0900000000000003, - -3.0900000000000003, - -0.06999999999999999, - -0.8473339999999999, - -0.21179900000000002, - -2.3000000000000003, - -3.0900000000000003, - -0.33, - -2.6100000000000003, - -0.8999999999999999, - -2.96, - -0.41250000000000003, - -1.22, - -3.0900000000000003, - -3.35, - -2.96, - -0.8999999999999999, - -2.7, - -0.41250000000000003, - -1.22 - ], - "q_clip_high": [ - 0.38, - 2.45, - 3.0900000000000003, - 2.14, - 0.473598, - 0.21179900000000002, - 0.38, - 2.45, - 0.38, - 2.14, - 0.473598, - 0.21179900000000002, - 2.3000000000000003, - 1.52, - 3.35, - 2.96, - 3.1300000000000003, - 2.7, - 0.41250000000000003, - 1.22, - 1.52, - 0.33, - 2.6100000000000003, - 3.1300000000000003, - 2.96, - 0.41250000000000003, - 1.22 - ], - "dq_clip": [ - 1.84, - 1.84, - 1.84, - 1.12, - 0.72, - 0.72, - 1.84, - 1.84, - 1.84, - 1.12, - 0.72, - 0.72, - 1.84, - 0.72, - 0.72, - 1.6, - 1.6, - 2.512, - 2.512, - 2.512, - 0.72, - 0.72, - 1.6, - 1.6, - 2.512, - 2.512, - 2.512 - ], - "tau_clip": [ - 50.0, - 50.0, - 50.0, - 75.0, - 15.0, - 10.0, - 50.0, - 50.0, - 50.0, - 75.0, - 15.0, - 10.0, - 50.0, - 10.0, - 10.0, - 4.5, - 4.5, - 4.75, - 4.75, - 4.75, - 10.0, - 10.0, - 4.5, - 4.5, - 4.75, - 4.75, - 4.75 - ], - "q_estop_low": [ - -0.428, - -3.1380000000000003, - -0.428, - -0.118, - -0.895334, - -0.259799, - -0.428, - -3.1380000000000003, - -3.1380000000000003, - -0.118, - -0.895334, - -0.259799, - -2.3480000000000003, - -3.1380000000000003, - -0.378, - -2.6580000000000004, - -0.948, - -3.008, - -0.4605, - -1.268, - -3.1380000000000003, - -3.398, - -3.008, - -0.948, - -2.748, - -0.4605, - -1.268 - ], - "q_estop_high": [ - 0.428, - 2.498, - 3.1380000000000003, - 2.188, - 0.521598, - 0.259799, - 0.428, - 2.498, - 0.428, - 2.188, - 0.521598, - 0.259799, - 2.3480000000000003, - 1.568, - 3.398, - 3.008, - 3.1780000000000004, - 2.748, - 0.4605, - 1.268, - 1.568, - 0.378, - 2.6580000000000004, - 3.1780000000000004, - 3.008, - 0.4605, - 1.268 - ], - "dq_estop": [ - 3.4499999999999997, - 3.4499999999999997, - 4.6000000000000005, - 6.3, - 3.6, - 3.15, - 3.4499999999999997, - 3.4499999999999997, - 4.6000000000000005, - 6.3, - 3.6, - 3.15, - 2.3000000000000003, - 0.9, - 1.3499999999999999, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14, - 0.9, - 1.3499999999999999, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14 - ], - "tau_estop": [ - 60.0, - 60.0, - 200.0, - 120.0, - 54.0, - 36.0, - 60.0, - 60.0, - 200.0, - 120.0, - 54.0, - 36.0, - 20.0, - 20.0, - 20.0, - 10.799999999999999, - 12.6, - 7.6000000000000005, - 7.6000000000000005, - 7.6000000000000005, - 20.0, - 20.0, - 10.799999999999999, - 12.6, - 7.6000000000000005, - 7.6000000000000005, - 7.6000000000000005 - ], - "kp_max": 500.0, - "kd_max": 30.0 - }, - "old": { - "mode": "split_mode", - "topics": { - "low_cmd_in": "rt/safety/lowcmd_in", - "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", - "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", - "low_cmd_out": "rt/lowcmd", - "low_state": "rt/lowstate" - }, - "publish_hz": 500.0, - "upper_stale_seconds": 2.0, - "upper_stale_active": true, - "lower_stale_seconds": null, - "state_stale_seconds": null, - "state_lag_ms": 100.0, - "engage": null, - "estop_response": null, - "logging": { - "enabled": false, - "base_dir": "/tmp/golem_safety", - "chunk_prefix": "chunk_" - } - } -} diff --git a/core_ws/src/golem_safety/test/test_config.cpp b/core_ws/src/golem_safety/test/test_config.cpp index 592d6c28..6eae6d5e 100644 --- a/core_ws/src/golem_safety/test/test_config.cpp +++ b/core_ws/src/golem_safety/test/test_config.cpp @@ -177,100 +177,6 @@ TEST(Config, UnknownKeyThrows) contains(parse_error("limits:\n estop:\n kp_max: 1.0\n", h12()), "'limits.estop.kp_max'")); } -TEST(Config, RemovedAndRenamedKeysNameTheirReplacement) -{ - EXPECT_TRUE(contains(parse_error("network: {domain_id: 1}\n", h12()), "removed")); - EXPECT_TRUE(contains(parse_error("robot: real\n", g1()), "sim")); - EXPECT_TRUE(contains(parse_error("estop: {enabled: true}\n", h12()), "use_estop")); - EXPECT_TRUE(contains(parse_error("estop: {poll_hz: 500.0}\n", h12()), "use_estop")); - EXPECT_TRUE( - contains(parse_error("estop: {upper_stale_seconds: 2.0}\n", h12()), "stale.upper_seconds")); - EXPECT_TRUE( - contains(parse_error("estop: {state_lag_ms: 100.0}\n", h12()), "control.state_lag_ms")); - EXPECT_TRUE( - contains( - parse_error("control: {lower_stale_estop_seconds: 0.2}\n", g1()), "stale.lower_seconds")); - EXPECT_TRUE( - contains( - parse_error("control: {state_stale_estop_seconds: 0.1}\n", g1()), "stale.state_seconds")); -} - -TEST(Config, ArmSdkModeRefused) -{ - // The G1 runs split_mode or full_body_mode: the message names the mode it was given and the - // robot's own. - const std::string message = parse_error("mode: arm_sdk_mode\n", g1()); - EXPECT_TRUE(contains(message, "(robot g1)")) << message; - EXPECT_TRUE(contains(message, "'mode'")) << message; - EXPECT_TRUE( - contains(message, "'arm_sdk_mode' is not a mode of robot g1 (split_mode, full_body_mode)")) << - message; -} - -TEST(Config, ArmSdkKeysRefused) -{ - const std::string reason = - "unknown key (arm_sdk mode was removed: the G1 runs split_mode or full_body_mode)"; - for (const char * section : { - "arm_sdk: {engage_seconds: 1.0, release_seconds: 3.0}\n", "arm_sdk: {waist_kp_min: 40.0}\n", - "arm_sdk: {}\n"}) - { - SCOPED_TRACE(section); - const std::string message = parse_error(std::string("mode: split_mode\n") + section, g1()); - EXPECT_TRUE( - contains(message, "safety config case.yaml (robot g1): key 'arm_sdk': " + reason)) << - message; - } - // The H1-2's configs are refused the same way. - const std::string h12_message = parse_error("arm_sdk: {}\n", h12()); - EXPECT_TRUE(contains(h12_message, "key 'arm_sdk': " + reason)) << h12_message; -} - -TEST(Config, StartupAndEngageKeysRefused) -{ - // Every robot streams zero gains from boot, so neither section has a choice left to make. - const std::string zero_stream = - "unknown key (removed: every robot streams zero gains from boot and engages each input " - "on its own first valid command)"; - // engage.mode_machine has a reason of its own, wherever it sits in the section. - const std::string profile = "unknown key (removed: mode_machine comes from the robot profile)"; - struct Case - { - const RobotProfile & profile; - std::string rest; // the required keys, with the robot's carried values - }; - const Case cases[] = { - {h12(), "control: {state_lag_ms: 100.0}\nestop_response: {after: burst_then_silence}\n"}, - {g1(), "control: {state_lag_ms: 0.0}\nestop_response: {after: keep_writing}\n"}, - }; - struct Section - { - const char * key; - const char * yaml; - const std::string & reason; - }; - const Section sections[] = { - {"startup", "startup: {rule: zero_stream}\n", zero_stream}, - {"startup", "startup: {rule: engage_gate}\n", zero_stream}, - {"startup", "startup: {}\n", zero_stream}, - {"engage", "engage: {max_state_age_seconds: 0.1, max_position_error: 0.15}\n", zero_stream}, - {"engage", "engage: {}\n", zero_stream}, - {"engage.mode_machine", "engage: {mode_machine: 5}\n", profile}, - {"engage.mode_machine", "engage: {max_position_error: 0.15, mode_machine: 6}\n", profile}, - }; - for (const Case & c : cases) { - for (const Section & section : sections) { - SCOPED_TRACE(c.profile.robot + ": " + section.yaml); - const std::string message = - parse_raw_error("mode: split_mode\n" + c.rest + section.yaml, c.profile); - EXPECT_TRUE( - contains( - message, "safety config case.yaml (robot " + c.profile.robot + "): key '" + - section.key + "': " + section.reason)) << message; - } - } -} - TEST(Config, ModeNotInProfileThrows) { const TempConfig file("mode: legs_only_mode\n"); @@ -521,30 +427,6 @@ TEST(Config, DerivedLimitsUseTheSchemaDefaults) EXPECT_EQ(parse("", g1()).limits.q_clip_low.size(), 29u); } -TEST(Config, EnforceAndKpAbsKdAbsAreAliases) -{ - const SafetyConfig c = parse( - "mode: full_body_mode\n" - "limits:\n" - " enforce: {position_offset: 0.02, velocity_ratio: 0.2, torque_ratio: 0.3," - " kp_abs: 111.0, kd_abs: 7.0}\n", h12()); - EXPECT_DOUBLE_EQ(c.limits.q_clip_low[0], h12().joints[0].q_low + 0.02); - EXPECT_DOUBLE_EQ(c.limits.dq_clip[0], h12().joints[0].dq_max * 0.2); - EXPECT_DOUBLE_EQ(c.limits.tau_clip[0], h12().joints[0].tau_max * 0.3); - EXPECT_DOUBLE_EQ(c.limits.kp_max, 111.0); - EXPECT_DOUBLE_EQ(c.limits.kd_max, 7.0); - // The estop limits keep their own defaults. - EXPECT_DOUBLE_EQ(c.limits.q_estop_low[0], h12().joints[0].q_low + 0.01); - - // An alias next to its own key is ambiguous. - EXPECT_TRUE( - contains( - parse_error("limits:\n clip: {kp_max: 1.0, kp_abs: 2.0}\n", h12()), "limits.clip.kp_abs")); - EXPECT_TRUE( - contains( - parse_error("limits:\n clip: {}\n enforce: {}\n", h12()), "limits.enforce")); -} - TEST(Config, NegativePositionOffsetWidensAndTooLargeThrows) { const SafetyConfig wide = diff --git a/core_ws/src/golem_safety/test/test_limit_parity.py b/core_ws/src/golem_safety/test/test_limit_parity.py deleted file mode 100644 index 27da90c6..00000000 --- a/core_ws/src/golem_safety/test/test_limit_parity.py +++ /dev/null @@ -1,129 +0,0 @@ -"""The new loader against what the old safety layers derive from the same configs. - -The fixtures under test/fixtures/limits//.json hold what the previous safety -layers derived from their configs (test/fixtures/README.md). This test imports neither -package: it runs golem_safety_dump_config and compares its output to the fixtures. -""" -import json -from pathlib import Path -import shutil -import subprocess - -from ament_index_python.packages import get_package_prefix, get_package_share_directory -import numpy as np -import pytest - -FIXTURES = Path(__file__).parent / 'fixtures' / 'limits' - -CONFIGS = { - 'h12': [ - 'default_safety_full', 'default_safety_split', 'locomotion_safety_full', - 'relax_safety_full', 'relax_safety_split', 'sim_safety_split', - 'tight_safety_full', 'tight_safety_split', - ], - 'g1': ['full', 'sim_split', 'split'], -} -CASES = [f'{robot}/{name}' for robot, names in CONFIGS.items() for name in names] - -LIMIT_ARRAYS = ( - 'q_clip_low', 'q_clip_high', 'dq_clip', 'tau_clip', - 'q_estop_low', 'q_estop_high', 'dq_estop', 'tau_estop', -) -JOINTS = {'h12': 27, 'g1': 29} -MODE_MACHINE = {'h12': 6, 'g1': 5} -# The five old topic keys and their new names; a DDS topic rt/x is the ROS topic /x. -TOPIC_KEYS = ('low_cmd_in', 'low_cmd_lower_in', 'low_cmd_upper_in', 'low_cmd_out', 'low_state') - - -def dump_executable(): - prefix = Path(get_package_prefix('golem_safety')) - installed = prefix / 'lib' / 'golem_safety' / 'golem_safety_dump_config' - if installed.is_file(): - return str(installed) - on_path = shutil.which('golem_safety_dump_config') - assert on_path, f'golem_safety_dump_config is neither on PATH nor at {installed}' - return on_path - - -def dump(robot, config): - result = subprocess.run( - [dump_executable(), robot, config], capture_output=True, text=True, timeout=30) - assert result.returncode == 0, result.stderr - return json.loads(result.stdout) - - -def fixture(case): - robot, name = case.split('/') - return json.loads((FIXTURES / robot / f'{name}.json').read_text()) - - -@pytest.mark.parametrize('case', CASES) -def test_limits_match_old_loaders(case): - robot, name = case.split('/') - old = fixture(case)['limits'] - new = dump(robot, name)['limits'] - for key in LIMIT_ARRAYS: - assert len(new[key]) == JOINTS[robot], key - np.testing.assert_allclose(new[key], old[key], rtol=0, atol=1e-12, err_msg=key) - assert new['kp_max'] == pytest.approx(old['kp_max'], rel=0, abs=1e-12) - assert new['kd_max'] == pytest.approx(old['kd_max'], rel=0, abs=1e-12) - - -@pytest.mark.parametrize('case', CASES) -def test_scalars_match_old_loaders(case): - robot, name = case.split('/') - old = fixture(case)['old'] - new = dump(robot, name) - - assert new['robot'] == robot - assert new['mode_machine'] == MODE_MACHINE[robot] - assert new['mode'] == old['mode'] - assert new['control']['publish_hz'] == old['publish_hz'] - for key in TOPIC_KEYS: - assert old['topics'][key].startswith('rt/'), key - assert new['topics'][key] == '/' + old['topics'][key][len('rt/'):], key - - # Logging keeps each file's own values; only the G1 enabled it. - assert new['logging'] == old['logging'] - - # The stale times. A key the old loader had carries its effective value; the others take - # 0.2 and 0.1. The old upper watchdog ran in split_mode only. - upper = old['upper_stale_seconds'] if old['upper_stale_active'] else 0.0 - assert new['stale']['upper_seconds'] == upper - assert new['stale']['lower_seconds'] == ( - old['lower_stale_seconds'] if old['lower_stale_seconds'] is not None else 0.2) - assert new['stale']['state_seconds'] == ( - old['state_stale_seconds'] if old['state_stale_seconds'] is not None else 0.1) - - # estop.state_lag_ms moved to control.state_lag_ms. The G1 had no lag check, so its configs - # carry 0.0, which disables it. - assert new['control']['state_lag_ms'] == ( - old['state_lag_ms'] if old['state_lag_ms'] is not None else 0.0) - - # Both robots stream zero gains from boot, a start-up the loader no longer configures. - # After an estop the H1-2 writes its burst and goes silent; the G1 keeps writing its - # estop command. - assert 'startup' not in new and 'engage' not in new - if robot == 'h12': - assert new['estop_response']['after'] == 'burst_then_silence' - assert new['estop_response']['burst_count'] == 50 - assert new['estop_response']['mode'] == 'zero' - assert new['estop_response']['kd'] == [0.0] * JOINTS[robot] - assert old['engage'] is None - else: - assert new['estop_response']['after'] == 'keep_writing' - assert new['estop_response']['mode'] == old['estop_response']['mode'] - kd = old['estop_response']['kd'] - assert new['estop_response']['kd'] == (kd if kd is not None else [0.0] * JOINTS[robot]) - # the old engage.mode_machine is the profile's - assert old['engage']['mode_machine'] == MODE_MACHINE[robot] - - -def test_every_installed_config_has_a_fixture(): - share = Path(get_package_share_directory('golem_safety')) / 'config' - for robot, names in CONFIGS.items(): - installed = sorted(path.stem for path in (share / robot).glob('*.yaml')) - assert installed == sorted(names), robot - fixtures = sorted(path.stem for path in (FIXTURES / robot).glob('*.json')) - assert fixtures == sorted(names), robot - diff --git a/core_ws/src/golem_safety/test/test_no_sdk.py b/core_ws/src/golem_safety/test/test_no_sdk.py index 3c7633ec..724b926c 100644 --- a/core_ws/src/golem_safety/test/test_no_sdk.py +++ b/core_ws/src/golem_safety/test/test_no_sdk.py @@ -1,6 +1,6 @@ ''' golem_safety's code talks ROS only: nothing in it imports the Unitree SDK. tools/ and -test/fixtures are not scanned: the offline fixture tools read the SDK on purpose. +test/ are not scanned: they are not installed, and test/fixtures records the SDK's CRCs. ''' from pathlib import Path diff --git a/core_ws/src/golem_safety/test/test_robot_data.py b/core_ws/src/golem_safety/test/test_robot_data.py index 50a6ef09..24d2ab9b 100644 --- a/core_ws/src/golem_safety/test/test_robot_data.py +++ b/core_ws/src/golem_safety/test/test_robot_data.py @@ -45,14 +45,6 @@ def test_robot_file_has_the_schema_and_robot_facts(robot): assert joint['velocity'] > 0 and joint['effort'] > 0, joint['name'] -def test_g1_profile_has_no_arm_sdk(): - '''The G1 runs split_mode or full_body_mode, and no slot past the body carries a value''' - data = load_robot('g1') - assert data['modes'] == ['split_mode', 'full_body_mode'] - assert set(data) == {'robot', 'mode_machine', 'mode_pr', 'motor_slots', 'modes', - 'lower_slots', 'upper_slots', 'joints'} - - def check_against_table(robot, fixture, joint_count): table = json.loads((FIXTURES / fixture).read_text()) data = load_robot(robot) diff --git a/core_ws/src/golem_safety/tools/g1_joint_limits.py b/core_ws/src/golem_safety/tools/g1_joint_limits.py index d2c67a99..af30c483 100644 --- a/core_ws/src/golem_safety/tools/g1_joint_limits.py +++ b/core_ws/src/golem_safety/tools/g1_joint_limits.py @@ -16,7 +16,6 @@ LEG_SLOTS = range(0, 12) WAIST_SLOTS = range(12, 15) -LEFT_ARM_SLOTS = range(15, 22) RIGHT_ARM_SLOTS = range(22, 29) JOINT_NAMES = [ From d3bf40bf37c14c286d4d6f533ce9ef732dc57bcd Mon Sep 17 00:00:00 2001 From: maxconway Date: Sat, 31 Oct 2026 11:49:57 -0600 Subject: [PATCH 002/179] golem_upper_ik: /lowcmd is the only safety output; remove the full-body IK path and the old controller's parity fixtures Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_01MezUeauFS9tKMgZKJxo1HC --- core_ws/src/golem_upper_ik/README.md | 24 +- .../golem_upper_ik/config/g1/g1_split.yaml | 5 +- .../golem_upper_ik/config/g1/sim_split.yaml | 5 +- .../src/golem_upper_ik/config/h12/debug.yaml | 4 - .../config/h12/safety_full.yaml | 4 - .../config/h12/safety_split.yaml | 4 - .../config/h12/sim_safety_split.yaml | 4 - .../golem_upper_ik/golem_upper_ik/config.py | 65 +- .../core/controller/upper_controller.py | 11 +- .../golem_upper_ik/core/ik_solver.py | 145 +- .../golem_upper_ik/core/low_cmd_handler.py | 22 +- .../golem_upper_ik/core/robot_model.py | 7 - .../golem_upper_ik/core/ros_io.py | 11 +- .../golem_upper_ik/golem_upper_ik/pairing.py | 5 +- .../golem_upper_ik/golem_upper_ik/profile.py | 8 +- .../golem_upper_ik/robots/g1.py | 11 +- .../golem_upper_ik/robots/h12.py | 13 +- .../golem_upper_ik/test/fixtures/README.md | 40 - .../golem_upper_ik/test/fixtures/fk_g1.json | 17 - .../golem_upper_ik/test/fixtures/fk_h12.json | 18 - .../test/fixtures/old_controller/g1.json | 916 ---------- .../fixtures/old_controller/g1_pairing.json | 1042 ----------- .../test/fixtures/old_controller/h12.json | 1540 ----------------- .../src/golem_upper_ik/test/test_config.py | 150 +- .../test/test_controller_loop.py | 24 +- .../test/test_frame_task_server.py | 40 +- .../test/test_low_cmd_handler.py | 22 +- .../src/golem_upper_ik/test/test_pairing.py | 48 +- .../src/golem_upper_ik/test/test_profile.py | 53 +- .../golem_upper_ik/test/test_robot_model.py | 102 +- .../tools/frame_task_server_rate.py | 7 +- docs/g1/safety.md | 2 +- 32 files changed, 150 insertions(+), 4219 deletions(-) delete mode 100644 core_ws/src/golem_upper_ik/test/fixtures/README.md delete mode 100644 core_ws/src/golem_upper_ik/test/fixtures/fk_g1.json delete mode 100644 core_ws/src/golem_upper_ik/test/fixtures/fk_h12.json delete mode 100644 core_ws/src/golem_upper_ik/test/fixtures/old_controller/g1.json delete mode 100644 core_ws/src/golem_upper_ik/test/fixtures/old_controller/g1_pairing.json delete mode 100644 core_ws/src/golem_upper_ik/test/fixtures/old_controller/h12.json diff --git a/core_ws/src/golem_upper_ik/README.md b/core_ws/src/golem_upper_ik/README.md index 03df1f40..bfdab324 100644 --- a/core_ws/src/golem_upper_ik/README.md +++ b/core_ws/src/golem_upper_ik/README.md @@ -51,7 +51,7 @@ they are read) and exits 1. On SIGINT or SIGTERM it shuts down (below) and exits | pub | `left_ee_target`, `right_ee_target` | `geometry_msgs/PoseStamped` | the current goal's targets for the end-effector frames, when it names them | | pub | `topics.low_cmd` | `unitree_hg/msg/LowCmd` | reliable, keep-last 1, at `frequency.pub_hz`; must be under `/safety/` | | sub | `topics.low_state` (`/lowstate`) | `unitree_hg/msg/LowState` | keep-last 1, best effort | -| sub | `topics.safety_out` | `LowCmd` | `golem_safety`'s output, read (never written) only until the follow gate opens | +| sub | `/lowcmd` | `LowCmd` | `golem_safety`'s output, read (never written) only until the follow gate opens | The actions, publishers and the `/frame_task` goal check exist once the start-up is done. @@ -72,7 +72,7 @@ nothing of another robot's packages: the H1-2 builds and runs without the G1's p |---|---|---| | IK joints | the 14 arm joints | the 14 arm joints | | Held joints | `torso_joint` | `waist_yaw_joint`, `waist_roll_joint`, `waist_pitch_joint` | -| Held joints at shutdown on `/lowcmd` (Shutdown, below) | off, with the arms | kept at their hold | +| Held joints at shutdown (Shutdown, below) | off, with the arms | kept at their hold | | End-effector frames | `left/right_wrist_yaw_link` | `left/right_hand_palm_link` | | Planner frames | `left/right_grasp_frame` | `left/right_hand_palm_link` | | IMU frame | `torso_link` | `imu_in_pelvis` (`controller.imu_frame` overrides) | @@ -80,7 +80,6 @@ nothing of another robot's packages: the H1-2 builds and runs without the G1's p | Self-collision model | `h1_2_magpie_sphere.urdf` + `h1_2_magpie_sphere_collision.srdf` | `g1_sphere.urdf` + `g1_sphere_collision.srdf`, packed on the rubber-hand G1: two spheres per hand at the Dex3-1 palm, none on the fingers, so the barrier and the planner do not see the Dex3-1 fingers. Its torso spheres stay inside the torso's flanks, where the hanging upper arms are 9 mm from the torso (`CL_Assets/README.md`) | | Start-up postures | `init_1`, `init_2`, `init_3`, `home` | `home` | | Named configurations | `NAMED_CONFIGS` in `robots/h12.py` (`home`, `t_pose`, `prep`, `staging_left`, ...) | `NAMED_CONFIGS` in `robots/g1.py` (`home`, `arms_down`, `t_pose`, `arms_front_45`) | -| `topics.safety_out` default | `/lowcmd` | `/lowcmd` | | `controller.max_state_lag_ms` default | 50 | 0 (off: the real G1's tick has not been measured) | ## Configs @@ -110,7 +109,6 @@ config outside the tables is not refused. |---|---|---| | `topics.low_cmd` | `/safety/lowcmd_upper_in` | the `golem_safety` input written; a topic outside `/safety/` is refused | | `topics.low_state` | `/lowstate` | the robot's state | -| `topics.safety_out` | per robot (above) | `golem_safety`'s output for the follow gate: `/lowcmd`, the output of every `golem_safety` mode; another topic is refused | | `controller.v_lim`, `w_lim` | 1.0, 2.0 | end-effector linear (m/s) and angular (rad/s) speed limits | | `controller.dq_lim` | 1.0 | IK joint speed limit, rad/s | | `controller.d_min` | 0.02 | self-collision distance the IK keeps between sphere pairs, m; every config of both robots keeps it (the G1's closest pairs at `home`, the upper arms against the torso, are 0.025 m apart) | @@ -125,16 +123,13 @@ config outside the tables is not refused. | `controller.max_state_lag_ms` | per robot (above) | the stale-state limit on domain 0, ms; <= 0 turns it off | | `frequency.ctrl_hz`, `pub_hz`, `check_hz` | 50, 500, 1000 | IK steps, command publishing, the follow gate's checks between steps, Hz | | `gains.kp`, `kd`, `ki` | | per upper-body joint | -| `limits.clip.position_offset`, `velocity_ratio`, `torque_ratio` | 0.001, 0.10, 0.25 | the command clip, as in `golem_safety` (`limits.enforce` is an alias) | +| `limits.clip.position_offset`, `velocity_ratio`, `torque_ratio` | 0.001, 0.10, 0.25 | the command clip, as in `golem_safety` | | `planner.*` | `enabled: true`, `frame_names` the profile's | the OMPL planner (`planner`, `timeout`, `moving_speed`, the frame bounds, ...); the rest take `PlannerConfig`'s defaults | Every config sets `ctrl_hz` 30 and `pub_hz` 500, except the H1-2's `sim_safety_split`, which leaves `frequency` out and so solves IK at 50 Hz. The loader refuses, naming the robot, the -file and the key: an unknown key at any level; a key of an older schema (`mode`, `network`, -`controller.torso_target`, `controller.waist_target`, `limits.estop`, -`planner.frame_z_min_margin`), saying why it was removed or which key replaces it; a -non-finite number, a number -out of its range or a value of the wrong kind; an alias given with its key. +file and the key: an unknown key at any level; a non-finite number, a number out of its +range or a value of the wrong kind. ## Behavior @@ -160,7 +155,7 @@ its first command once `/lowstate` reports the profile's `mode_machine` (and, on H1-2, `estop_node` has sent a first sample), so the gate opens at the first relay, on both robots whether or not the leg node commands. -**The follow gate.** Until a `topics.safety_out` sample carries the controller's command on +**The follow gate.** Until a `/lowcmd` sample (`golem_safety`'s output) carries the controller's command on every upper slot (kp > 0, each q within `controller.follow_tolerance`), every upper-body command is kept within `follow_tolerance` of the measured joints, and a thread re-checks at `frequency.check_hz` between control steps. Nothing executes the commands before @@ -236,7 +231,7 @@ later one is ignored. `frame_task_server` first sends its upper slots one comman publishing, then stops the planner and its `/lowstate` subscription, the last two even if an earlier step fails; the whole shutdown fits in launch's 5 s `sigterm_timeout`. The command gives the IK joints mode 0 and zero kp, kd and tau (q and dq as they were). The held joints -get the same, unless the profile's `shutdown_keeps_held_joints_on_lowcmd` is set (the G1): +get the same, unless the profile's `shutdown_keeps_held_joints` is set (the G1): then they keep their hold angle, gains and torque, since a walking policy on the legs needs a stiff waist. What `golem_safety` does with that command: @@ -265,10 +260,7 @@ robots with their collision models), the ROS I/O, command handler and follow gat (`test_ros_io.py`, `test_low_cmd_handler.py`), the controller and the nodes (`test_controller_loop.py`, `test_frame_task_server.py`), a start-up IK step with no solution (`test_startup_no_solution.py`), the publish rate -(`test_publish_rate.py`) and that nothing imports `unitree_sdk2py` (`test_no_sdk.py`). The -fixtures under `test/fixtures/` are recorded data: what the previous arm controllers -returned for the same configs and configurations (`test/fixtures/README.md` says which -package and commit); no test imports those controllers. +(`test_publish_rate.py`) and that nothing imports `unitree_sdk2py` (`test_no_sdk.py`). ## Tools diff --git a/core_ws/src/golem_upper_ik/config/g1/g1_split.yaml b/core_ws/src/golem_upper_ik/config/g1/g1_split.yaml index 9e27eaa8..1baa1d71 100644 --- a/core_ws/src/golem_upper_ik/config/g1/g1_split.yaml +++ b/core_ws/src/golem_upper_ik/config/g1/g1_split.yaml @@ -7,9 +7,6 @@ topics: low_cmd: /safety/lowcmd_upper_in low_state: /lowstate - # golem_safety's output, only read: the follow gate opens once a sample - # carries the commands (its split.yaml has engaged) - safety_out: /lowcmd controller: # deliberately conservative for the first sessions on the real robot (the @@ -31,7 +28,7 @@ controller: waist_pitch_joint: 0.0 # s to wait for a fresh /lowstate before enabling anything (then abort) state_timeout: 5.0 - # until golem_safety is seen relaying the commands (topics.safety_out), + # until golem_safety is seen relaying the commands (on /lowcmd), # every waist and arm command stays within this of its measured joint, rad. # Never after that. 0 is off follow_tolerance: 0.05 diff --git a/core_ws/src/golem_upper_ik/config/g1/sim_split.yaml b/core_ws/src/golem_upper_ik/config/g1/sim_split.yaml index 81e5bc8f..736b53dc 100644 --- a/core_ws/src/golem_upper_ik/config/g1/sim_split.yaml +++ b/core_ws/src/golem_upper_ik/config/g1/sim_split.yaml @@ -6,9 +6,6 @@ topics: low_cmd: /safety/lowcmd_upper_in low_state: /lowstate - # golem_safety's output, only read: the follow gate opens once a sample - # carries the commands (its sim_split.yaml has engaged) - safety_out: /lowcmd controller: v_lim: 0.5 # palm linear speed limit, m/s @@ -27,7 +24,7 @@ controller: waist_pitch_joint: 0.0 # s to wait for a fresh /lowstate before enabling anything (then abort) state_timeout: 5.0 - # until golem_safety is seen relaying the commands (topics.safety_out), + # until golem_safety is seen relaying the commands (on /lowcmd), # every waist and arm command stays within this of its measured joint, rad. # Never after that. 0 is off follow_tolerance: 0.05 diff --git a/core_ws/src/golem_upper_ik/config/h12/debug.yaml b/core_ws/src/golem_upper_ik/config/h12/debug.yaml index 553043cb..caa74f53 100644 --- a/core_ws/src/golem_upper_ik/config/h12/debug.yaml +++ b/core_ws/src/golem_upper_ik/config/h12/debug.yaml @@ -18,10 +18,6 @@ controller: planner: planner: RRTConnect timeout: 1.0 - # example RRT* tuning - # planner: RRTstar - # timeout: 3.0 - # range: 0.15 # planner extension length in joint space range: 0.0 try_direct: true diff --git a/core_ws/src/golem_upper_ik/config/h12/safety_full.yaml b/core_ws/src/golem_upper_ik/config/h12/safety_full.yaml index 553043cb..caa74f53 100644 --- a/core_ws/src/golem_upper_ik/config/h12/safety_full.yaml +++ b/core_ws/src/golem_upper_ik/config/h12/safety_full.yaml @@ -18,10 +18,6 @@ controller: planner: planner: RRTConnect timeout: 1.0 - # example RRT* tuning - # planner: RRTstar - # timeout: 3.0 - # range: 0.15 # planner extension length in joint space range: 0.0 try_direct: true diff --git a/core_ws/src/golem_upper_ik/config/h12/safety_split.yaml b/core_ws/src/golem_upper_ik/config/h12/safety_split.yaml index 99c38a50..99cec25b 100644 --- a/core_ws/src/golem_upper_ik/config/h12/safety_split.yaml +++ b/core_ws/src/golem_upper_ik/config/h12/safety_split.yaml @@ -18,10 +18,6 @@ controller: planner: planner: RRTConnect timeout: 1.0 - # example RRT* tuning - # planner: RRTstar - # timeout: 3.0 - # range: 0.15 # planner extension length in joint space range: 0.0 try_direct: true diff --git a/core_ws/src/golem_upper_ik/config/h12/sim_safety_split.yaml b/core_ws/src/golem_upper_ik/config/h12/sim_safety_split.yaml index 3d7f039a..dcb37739 100644 --- a/core_ws/src/golem_upper_ik/config/h12/sim_safety_split.yaml +++ b/core_ws/src/golem_upper_ik/config/h12/sim_safety_split.yaml @@ -21,10 +21,6 @@ controller: planner: planner: RRTConnect timeout: 1.0 - # example RRT* tuning - # planner: RRTstar - # timeout: 3.0 - # range: 0.15 # planner extension length in joint space range: 0.0 try_direct: true diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/config.py b/core_ws/src/golem_upper_ik/golem_upper_ik/config.py index 680a6e78..0b30e6ab 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/config.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/config.py @@ -5,9 +5,8 @@ A bare name ('safety_split' or 'safety_split.yaml') is a file of share/golem_upper_ik/config//. Anything with a '/' is a path. Every key and every -value is checked: an unknown key, a key from an older config, a value of the wrong type, a -non-finite number or a number out of its range raises ValueError naming the robot, the file -and the key. +value is checked: an unknown key, a value of the wrong type, a non-finite number or a +number out of its range raises ValueError naming the robot, the file and the key. ''' import math import os @@ -19,16 +18,13 @@ from golem_upper_ik.profile import ROBOTS, RobotProfile, load_profile from golem_upper_ik.robots import robot_module -__all__ = ['load_controller_config', 'SAFETY_INPUT_PREFIX', 'SAFETY_OUTPUT_TOPICS'] +__all__ = ['load_controller_config', 'SAFETY_INPUT_PREFIX', 'SAFETY_OUTPUT_TOPIC'] # The controller writes only golem_safety's inputs: golem_safety is the only writer of # /lowcmd SAFETY_INPUT_PREFIX = '/safety/' -# golem_safety's output topic in each of its modes; the follow gate reads one of them -SAFETY_OUTPUT_TOPICS = { - 'split_mode': '/lowcmd', - 'full_body_mode': '/lowcmd', -} +# golem_safety's output, which the follow gate reads +SAFETY_OUTPUT_TOPIC = '/lowcmd' TOPIC_DEFAULTS = { 'low_cmd': '/safety/lowcmd_upper_in', @@ -64,7 +60,7 @@ 'velocity_ratio': 0.10, 'torque_ratio': 0.25, } -# The planner section's keys, by kind: the fields of the H1-2 planner's PlannerConfig, and +# The planner section's keys, by kind: the fields of PlannerConfig (core.planner), and # enabled. The loader checks the keys a file gives and fills in only enabled (true) and # frame_names (the profile's planner frames); PlannerConfig supplies every other default. # The planner runs only when the profile has a self-collision model and enabled is true. @@ -91,25 +87,13 @@ PLANNER_TEXT_KEYS + PLANNER_BOOLEAN_KEYS + PLANNER_POSITIVE_KEYS + PLANNER_NON_NEGATIVE_KEYS + PLANNER_COUNT_KEYS + PLANNER_BOUND_KEYS + ('frame_names',)) SECTION_KEYS = { - 'topics': frozenset(TOPIC_DEFAULTS) | {'safety_out'}, + 'topics': frozenset(TOPIC_DEFAULTS), 'controller': frozenset(CONTROLLER_DEFAULTS) | {'hold_targets', 'imu_frame', 'max_state_lag_ms'}, 'planner': PLANNER_KEYS, 'frequency': frozenset(FREQUENCY_DEFAULTS), 'gains': frozenset(('kp', 'kd', 'ki')), - 'limits': frozenset(('clip', 'enforce')), -} -# What the error says about a key an older config may still carry -REPLACED_KEYS = { - 'mode': 'removed: the controller always writes a golem_safety input (topics.low_cmd)', - 'network': "removed: ROS supplies the domain, and the G1's interface is pinned in " - 'cyclonedds_local.xml', - 'controller.torso_target': 'renamed controller.hold_targets: {torso_joint: rad}', - 'controller.waist_target': 'renamed controller.hold_targets: {waist_yaw_joint: rad, ' - 'waist_roll_joint: rad, waist_pitch_joint: rad}', - 'limits.estop': "removed: the controller has no estop of its own; golem_safety's estop " - 'limits are the check', - 'planner.frame_z_min_margin': 'renamed planner.frame_limit_margin (x, y and z)', + 'limits': frozenset(('clip',)), } @@ -131,11 +115,8 @@ def _section(where: _Where, raw: dict, name: str) -> dict: if not isinstance(value, dict): raise where.error(name, 'must be a mapping') for key in value: - dotted = f'{name}.{key}' - if dotted in REPLACED_KEYS: - raise where.error(dotted, REPLACED_KEYS[dotted]) if key not in SECTION_KEYS[name]: - raise where.error(dotted, 'unknown key') + raise where.error(f'{name}.{key}', 'unknown key') return value @@ -252,22 +233,19 @@ def _gains(where: _Where, gains: dict, profile: RobotProfile) -> dict: def _clip_limits(where: _Where, limits: dict, profile: RobotProfile) -> dict: - if 'clip' in limits and 'enforce' in limits: - raise where.error('limits.enforce', 'give limits.clip or limits.enforce, not both') - name = 'enforce' if 'enforce' in limits else 'clip' - policy = limits.get(name, {}) + policy = limits.get('clip', {}) if not isinstance(policy, dict): - raise where.error(f'limits.{name}', 'must be a mapping') + raise where.error('limits.clip', 'must be a mapping') for key in policy: if key not in CLIP_DEFAULTS: - raise where.error(f'limits.{name}.{key}', 'unknown key') - values = {key: _joint_values(where, f'limits.{name}.{key}', policy.get(key, default), + raise where.error(f'limits.clip.{key}', 'unknown key') + values = {key: _joint_values(where, f'limits.clip.{key}', policy.get(key, default), profile) for key, default in CLIP_DEFAULTS.items()} for key, value in values.items(): if np.any(value < 0.0): slot = int(np.argmax(value < 0.0)) - raise where.error(f'limits.{name}.{key}', + raise where.error(f'limits.clip.{key}', f'must be 0 or positive, not {value[slot]} ' f'(joint {profile.body_joints[slot]})') @@ -276,7 +254,7 @@ def _clip_limits(where: _Where, limits: dict, profile: RobotProfile) -> dict: for slot, (low, high) in enumerate(q_limits): if low >= high: raise where.error( - f'limits.{name}.position_offset', + 'limits.clip.position_offset', f'too large for joint {slot} ({profile.body_joints[slot]}): it leaves ' f'[{low}, {high}]') return { @@ -354,8 +332,6 @@ def load_controller_config(robot: str, name_or_path: str) -> dict: if not isinstance(raw, dict): raise where.error('', 'the file must hold a mapping') for key in raw: - if key in REPLACED_KEYS: - raise where.error(key, REPLACED_KEYS[key]) if key not in SECTION_KEYS: raise where.error(key, 'unknown key') sections = {name: _section(where, raw, name) for name in SECTION_KEYS} @@ -366,17 +342,6 @@ def load_controller_config(robot: str, name_or_path: str) -> dict: if not topics['low_cmd'].startswith(SAFETY_INPUT_PREFIX): raise where.error('topics.low_cmd', f"'{topics['low_cmd']}' is not a golem_safety " f'input ({SAFETY_INPUT_PREFIX}...)') - unmapped = [mode for mode in profile.modes if mode not in SAFETY_OUTPUT_TOPICS] - if unmapped: - raise where.error('', f'robot {robot} has golem_safety modes with no known output ' - f'topic: {", ".join(unmapped)}') - outputs = sorted({SAFETY_OUTPUT_TOPICS[mode] for mode in profile.modes}) - safety_out = _topic(where, 'topics.safety_out', - topics_raw.get('safety_out', robot_defaults['topics.safety_out'])) - if safety_out not in outputs: - raise where.error('topics.safety_out', f"'{safety_out}' is not a golem_safety output " - f'of robot {robot} ({", ".join(outputs)})') - topics['safety_out'] = safety_out controller_raw = sections['controller'] controller = { diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/core/controller/upper_controller.py b/core_ws/src/golem_upper_ik/golem_upper_ik/core/controller/upper_controller.py index 9d317d2b..d7ae2769 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/core/controller/upper_controller.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/core/controller/upper_controller.py @@ -23,6 +23,7 @@ import numpy as np from pink.exceptions import NoSolutionFound +from golem_upper_ik.config import SAFETY_OUTPUT_TOPIC from golem_upper_ik.core.ik_solver import IKSolver from golem_upper_ik.core.low_cmd_handler import LowCmdHandler from golem_upper_ik.core.planner import PlannerClient, PlannerConfig, PlannerUnavailableError @@ -169,8 +170,8 @@ def _wait_for_state(self): f'No fresh {self.state_topic} with finite joint positions within ' f'{self.state_timeout:.1f} s on ROS domain {ros_domain_id(self.node)}; not ' f'enabling any motor. Is the robot (or the sim) up and publishing ' - f'{self.state_topic} on this domain, and is DDS on the interface wired to it ' - '(on the G1, G1_DDS_INTERFACE)?') + f'{self.state_topic} on this domain (ROS_DOMAIN_ID), and does the CycloneDDS ' + 'configuration (CYCLONEDDS_URI) use the interface wired to it?') # the start-up @@ -194,7 +195,7 @@ def _wait_until_following(self, deadline): handler = self.low_cmd_handler if handler.following: return - topic = handler.safety_output_topic + topic = SAFETY_OUTPUT_TOPIC self._logger.info(f'Waiting for golem_safety to relay the commands on {topic}...') while not handler.following: if time.monotonic() > deadline: @@ -212,8 +213,8 @@ def _not_relayed_error(self, topic): f'relays only once it sees {self.state_topic} report mode_machine ' f'{self.profile.mode_machine} (the {self.profile.robot} profile\'s) and, when started ' 'with use_estop:=true, a first sample on its estop topics; it must run the safety ' - 'config this controller config pairs with (writing ' - f'{topic}), and must not have latched its estop, at this start-up (it checks its ' + 'config this controller config pairs with, and must not have latched its estop, ' + 'at this start-up (it checks its ' f'estop limits from its first {self.state_topic} sample, so a joint already past ' 'limits.estop latches it) or in an earlier run; a latch holds until golem_safety ' 'restarts. Stopping the controller') diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/core/ik_solver.py b/core_ws/src/golem_upper_ik/golem_upper_ik/core/ik_solver.py index b283dea5..b8485996 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/core/ik_solver.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/core/ik_solver.py @@ -1,5 +1,6 @@ ''' -IKSolver: Pink differential IK on a RobotModel's body and reduced models. +IKSolver: Pink differential IK on a RobotModel's reduced model (the IK joints); a +configuration on its body model (no free-flyer) carries every body joint's q. Each frame task pulls one frame to a target pose. With a self-collision model the reduced model's IK keeps the collision pairs apart (a self-collision barrier); without one it runs @@ -16,6 +17,9 @@ # named configurations solve_ik_reduced falls back to, in order, after the measured pose SEED_CONFIGS = ('home', 't_pose', 'arms_front_45') +# solve_ik_reduced converges when the summed frame-task errors are below these +LINEAR_THRESHOLD = 1e-3 # m +ANGULAR_THRESHOLD = 1e-2 # rad @dataclass @@ -50,8 +54,8 @@ def __init__(self, dt: float = 0.02, d_min: float = 0.05): ''' - Initialize the IK solver with robot model and collision parameters - Uses model_body (no free-flyer) for IK solving + Initialize the IK solver with robot model and collision parameters. The IK solves + on model_body_reduced; configuration (model_body) tracks every body joint @param robot_model: the robot model instance @param named_configs: name -> reduced q (one value per IK joint); the seeds @@ -80,14 +84,6 @@ def __init__(self, robot_model.state['q'] ) - # limits using model_body (no free-flyer) - self.limits = [ - pink.limits.ConfigurationLimit(robot_model.model_body), - pink.limits.VelocityLimit(robot_model.model_body), - pink.limits.AccelerationLimit(robot_model.model_body, - 25.0 * np.ones(robot_model.model_body.nv)) - ] - # reduced-model bindings (configuration, limits, collision barrier) self.rebuild_reduced() @@ -142,15 +138,6 @@ def q_reduced(self): '''Get current reduced joint configuration''' return np.copy(self.configuration_reduced.q) - def compute_error(self, task: pink.Task, q: np.ndarray) -> np.ndarray: - '''Compute the error for a specific task (motor-only q)''' - configuration = pink.Configuration( - self.robot_model.model_body, - self.robot_model.data_body, - q - ) - return task.compute_error(configuration) - def compute_error_reduced(self, task: pink.Task, q_reduced: np.ndarray) -> np.ndarray: '''Compute the error for a specific task on reduced model''' configuration_reduced = pink.Configuration( @@ -204,18 +191,6 @@ def integrate(self, vel: np.ndarray): self.configuration.integrate_inplace(vel, self.dt) self.configuration_reduced.integrate_inplace(vel_reduced, self.dt) - def _ik(self, tasks, dt): - '''Helper function solving IK on body model for all tasks''' - return pink.solve_ik( - self.configuration, - tasks, - dt=dt, - solver=self.solver, - limits=self.limits, - barriers=[], - safety_break=False - ) - def _ik_reduced(self, tasks, dt): '''Helper function solving IK on reduced model for all tasks''' barriers = [] @@ -241,10 +216,10 @@ def _frame_task_errors(self, configuration): angular_err += np.linalg.norm(err[3:]) return linear_err, angular_err - def _task_error_score(self, attempt, linear_threshold, angular_threshold): - linear_scale = max(float(linear_threshold), 1e-12) - angular_scale = max(float(angular_threshold), 1e-12) - return attempt.linear_error / linear_scale + attempt.angular_error / angular_scale + @staticmethod + def _task_error_score(attempt): + return (attempt.linear_error / LINEAR_THRESHOLD + + attempt.angular_error / ANGULAR_THRESHOLD) def _reduced_q_to_full(self, q_reduced): q = np.copy(self.robot_model.zero_q_body) @@ -259,31 +234,6 @@ def _default_seed_items(self): (name, self.named_configs[name]) for name in SEED_CONFIGS ] - def _extra_seed_items(self, initial_positions): - if initial_positions is None: - return [] - - try: - seeds = np.asarray(initial_positions, dtype=float) - if seeds.ndim == 1: - return [('initial_0', seeds)] - if seeds.ndim == 2: - return [(f'initial_{idx}', seed) for idx, seed in enumerate(seeds)] - except (TypeError, ValueError): - pass - - items = [] - for idx, item in enumerate(initial_positions): - if ( - isinstance(item, tuple) and - len(item) == 2 and - isinstance(item[0], str) - ): - items.append((item[0], item[1])) - else: - items.append((f'initial_{idx}', item)) - return items - def _normalize_reduced_seed(self, seed, seed_name): q = np.asarray(seed, dtype=float).reshape(-1) if q.shape == (self.robot_model.model_body_reduced.nq,): @@ -300,27 +250,15 @@ def _normalize_reduced_seed(self, seed, seed_name): raise ValueError(f'{seed_name} seed must contain only finite values') return normalized - def _seed_list(self, initial_positions): - seeds = [] - seed_items = self._default_seed_items() + self._extra_seed_items( - initial_positions, - ) - for seed_name, seed in seed_items: - seeds.append((seed_name, self._normalize_reduced_seed(seed, seed_name))) - return seeds + def _seed_list(self): + return [(seed_name, self._normalize_reduced_seed(seed, seed_name)) + for seed_name, seed in self._default_seed_items()] - def _result_from_attempts(self, attempts, linear_threshold, angular_threshold): + def _result_from_attempts(self, attempts): candidates = [attempt for attempt in attempts if attempt.success] if not candidates: candidates = attempts - best = min( - candidates, - key=lambda attempt: self._task_error_score( - attempt, - linear_threshold, - angular_threshold, - ), - ) + best = min(candidates, key=self._task_error_score) return IKResult( q=np.copy(best.q), success=best.success, @@ -330,8 +268,7 @@ def _result_from_attempts(self, attempts, linear_threshold, angular_threshold): attempts=attempts, ) - def _solve_ik_reduced_attempt(self, seed_name, q_seed, alpha, timeout, - linear_threshold, angular_threshold): + def _solve_ik_reduced_attempt(self, seed_name, q_seed, alpha, timeout): self.configuration_reduced.update(q_seed) linear_err, angular_err = self._frame_task_errors( self.configuration_reduced, @@ -340,8 +277,8 @@ def _solve_ik_reduced_attempt(self, seed_name, q_seed, alpha, timeout, iterations = 0 while ( - linear_err >= linear_threshold or - angular_err >= angular_threshold + linear_err >= LINEAR_THRESHOLD or + angular_err >= ANGULAR_THRESHOLD ) and time.time() - start_time < timeout: self.posture_task.set_target_from_configuration( self.configuration_reduced, @@ -357,8 +294,8 @@ def _solve_ik_reduced_attempt(self, seed_name, q_seed, alpha, timeout, return IKAttemptResult( q=self._reduced_q_to_full(self.configuration_reduced.q), success=( - linear_err < linear_threshold and - angular_err < angular_threshold + linear_err < LINEAR_THRESHOLD and + angular_err < ANGULAR_THRESHOLD ), linear_error=linear_err, angular_error=angular_err, @@ -367,12 +304,6 @@ def _solve_ik_reduced_attempt(self, seed_name, q_seed, alpha, timeout, elapsed_time=time.time() - start_time, ) - def ik_step(self): - '''Solve one step of IK for all tasks and return joint velocity (motor-only)''' - self.posture_task.set_target_from_configuration(self.configuration) - tasks = list(self.frame_tasks.values()) + [self.posture_task] - return self._ik(tasks, self.dt) - def ik_step_reduced(self): '''Solve one step of IK on reduced model for all tasks and return motor-only velocity''' self.posture_task.set_target_from_configuration(self.configuration_reduced) @@ -383,41 +314,19 @@ def ik_step_reduced(self): vel_full[self.robot_model.reduced_mask] = vel return vel_full - def solve_ik_reduced(self, alpha=0.1, timeout=1.0, - linear_threshold=1e-3, angular_threshold=1e-2, - initial_positions=None): + def solve_ik_reduced(self, alpha=0.1, timeout=1.0): '''Solve IK on reduced model to convergence and return motor-only q''' attempts = [] - for seed_name, q_seed in self._seed_list(initial_positions): - attempt = self._solve_ik_reduced_attempt( - seed_name, - q_seed, - alpha, - timeout, - linear_threshold, - angular_threshold, - ) + for seed_name, q_seed in self._seed_list(): + attempt = self._solve_ik_reduced_attempt(seed_name, q_seed, alpha, timeout) attempts.append(attempt) if attempt.success: - return self._result_from_attempts( - attempts, - linear_threshold, - angular_threshold, - ) - - result = self._result_from_attempts( - attempts, - linear_threshold, - angular_threshold, - ) + return self._result_from_attempts(attempts) + + result = self._result_from_attempts(attempts) self.configuration_reduced.update(result.q[self.robot_model.reduced_mask]) return result - def goto_configuration(self, q: np.ndarray): - '''Solve one step of IK to reach a target joint configuration (motor-only)''' - self.config_task.set_target(q) - return self._ik([self.config_task], self.dt) - def goto_reduced_configuration(self, q_reduced: np.ndarray): '''Solve one step of IK to reach a target reduced joint configuration''' self.config_task.set_target(q_reduced) diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/core/low_cmd_handler.py b/core_ws/src/golem_upper_ik/golem_upper_ik/core/low_cmd_handler.py index 7670b9bd..10778ad9 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/core/low_cmd_handler.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/core/low_cmd_handler.py @@ -9,7 +9,7 @@ measured angles; it moves only through set_hold(), and controller.hold_targets is where the controller's start-up moves it. - Follow gate (FollowGate, controller.follow_tolerance): until golem_safety is seen relaying - the commands on topics.safety_out, every upper-body command stays within the tolerance of + the commands on /lowcmd, every upper-body command stays within the tolerance of the measured joints, and the hold restarts from the command the gate let through. Between control steps a thread re-checks at frequency.check_hz: it opens the gate on a relayed sample and keeps the stored command near a drifting pose. Once open, the gate and its @@ -20,7 +20,7 @@ changes nothing here. - shutdown() sends one last command and stops publishing: the IK joints mode 0 with zero gains and torque, and the held joints the same, except where the profile keeps them - (shutdown_keeps_held_joints_on_lowcmd, the G1): there they keep their hold angle, gains + (shutdown_keeps_held_joints, the G1): there they keep their hold angle, gains and torque. ''' import math @@ -30,6 +30,7 @@ import numpy as np +from golem_upper_ik.config import SAFETY_OUTPUT_TOPIC from golem_upper_ik.core.follow_gate import FollowGate, relays_commands from golem_upper_ik.core.ros_io import CommandPublisher, SafetyOutputSubscriber, StaleStateError from golem_upper_ik.profile import RobotProfile @@ -75,10 +76,7 @@ def __init__(self, node, profile: RobotProfile, config: dict, robot_model): dtype=np.float64) self._hold = np.zeros(len(self._held)) # start() sets it to the measured angles - self._safety_output_topic = config['topics']['safety_out'] - # golem_safety writes /lowcmd in every mode, so the profile alone decides whether - # the held joints stay on at shutdown (a walking policy may be on the legs) - self._shutdown_keeps_held = profile.shutdown_keeps_held_joints_on_lowcmd + self._shutdown_keeps_held = profile.shutdown_keeps_held_joints self._follow_gate = FollowGate(profile.upper_slots, config['controller']['follow_tolerance']) self._check_dt = 1.0 / float(config['frequency']['check_hz']) @@ -95,7 +93,7 @@ def __init__(self, node, profile: RobotProfile, config: dict, robot_model): node, config['topics']['low_cmd'], config['frequency']['pub_hz'], profile) self._safety_output = None if not self._follow_gate.following: - self._safety_output = SafetyOutputSubscriber(node, self._safety_output_topic) + self._safety_output = SafetyOutputSubscriber(node) @property def q_cmd(self) -> np.ndarray: @@ -119,10 +117,6 @@ def following(self) -> bool: '''False while the follow gate holds the commands near the measured pose''' return self._follow_gate.following - @property - def safety_output_topic(self) -> str: - return self._safety_output_topic - def _clip_q(self, q, joint_ids=None): limits = self._q_clip_limits if joint_ids is not None: @@ -329,7 +323,7 @@ def _open_if_relayed(self, msg): return gate.open() self._node.get_logger().info( - f'golem_safety relays the commands on {self._safety_output_topic}; ' + f'golem_safety relays the commands on {SAFETY_OUTPUT_TOPIC}; ' 'the follow gate is open') def _close_safety_output(self): @@ -342,8 +336,8 @@ def _close_safety_output(self): def shutdown(self): ''' Send the IK joints mode 0 with zero gains and torque (q and dq as they were), and the - held joints the same unless the profile keeps them on /lowcmd, where their command - stands; make sure that command went out, and stop publishing. + held joints the same unless the profile keeps them (shutdown_keeps_held_joints), where + their command stands; make sure that command went out, and stop publishing. Call it before the robot model's state source is detached (RobotModel.shutdown()): while the follow gate is shut, a detached source stops the gate's check with an error diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/core/robot_model.py b/core_ws/src/golem_upper_ik/golem_upper_ik/core/robot_model.py index a227abf7..bf2ea3b9 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/core/robot_model.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/core/robot_model.py @@ -338,7 +338,6 @@ def shutdown(self): if self.state_subscriber is not None: self.state_subscriber.shutdown() self.state_subscriber = None - print('RobotModel shutdown', flush=True) @property def state_staleness_ms(self): @@ -414,12 +413,6 @@ def _get_frame_transformation_reduced(self, frame_name, q_reduced: np.ndarray=No def get_frame_transformation_reduced(self, frame_name: str, q_reduced: np.ndarray=None): return self._get_frame_transformation_reduced(frame_name, q_reduced).np - def get_frame_position_reduced(self, frame_name: str, q_reduced: np.ndarray=None): - return self._get_frame_transformation_reduced(frame_name, q_reduced).translation - - def get_frame_rotation_reduced(self, frame_name: str, q_reduced: np.ndarray=None): - return self._get_frame_transformation_reduced(frame_name, q_reduced).rotation - def check_within_limits_reduced(self, q_reduced): '''Check if the given joint position violates joint limits for the reduced model''' return ( diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/core/ros_io.py b/core_ws/src/golem_upper_ik/golem_upper_ik/core/ros_io.py index 6f879205..082a80b9 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/core/ros_io.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/core/ros_io.py @@ -31,7 +31,7 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy from unitree_hg.msg import LowCmd, LowState -from golem_upper_ik.config import SAFETY_INPUT_PREFIX, SAFETY_OUTPUT_TOPICS +from golem_upper_ik.config import SAFETY_INPUT_PREFIX, SAFETY_OUTPUT_TOPIC from golem_upper_ik.profile import RobotProfile __all__ = [ @@ -388,15 +388,12 @@ class SafetyOutputSubscriber: controller never writes golem_safety's output. ''' - def __init__(self, node, topic: str, callback_group=None): - outputs = sorted(set(SAFETY_OUTPUT_TOPICS.values())) - if topic not in outputs: - raise ValueError(f"'{topic}' is not a golem_safety output ({', '.join(outputs)})") - self._topic = topic + def __init__(self, node, callback_group=None): + self._topic = SAFETY_OUTPUT_TOPIC self._lock = threading.Lock() self._latest = None self._subscription = _ClosableSubscription( - node, LowCmd, topic, self._receive, callback_group) + node, LowCmd, self._topic, self._receive, callback_group) @property def topic(self) -> str: diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py b/core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py index e48b4d62..e38c97af 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py @@ -44,8 +44,7 @@ # the G1 controller configs; both hold the waist stiff for a walking policy G1_CONTROLLER_CONFIGS = ('g1_split', 'sim_split') # the controller config each G1 safety config runs with: its speed limits suit the real -# robot or the sim, and its topics.safety_out is the safety config's output, which the follow -# gate reads +# robot or the sim G1_SAFETY_CONTROLLER_CONFIGS = { 'split': 'g1_split', 'sim_split': 'sim_split', @@ -102,7 +101,7 @@ def _g1_refusal(controller: str, safety: str, sim: bool | None) -> str | None: return (f'safety config "{safety}" must relay the upper-body channel: ' f'one of {", ".join(G1_UPPER_CHANNEL_SAFETY_CONFIGS)}') # a custom controller config is known to be a sim one only by its name, and is not - # matched to the safety config: its gains and safety_out are not known by its name + # matched to the safety config: its gains are not known by its name controller_known = controller in G1_CONTROLLER_CONFIGS or is_sim_config(controller) if sim is not None: for kind, name, known in (('safety', safety, True), diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/profile.py b/core_ws/src/golem_upper_ik/golem_upper_ik/profile.py index eeee1cb4..069c8dd7 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/profile.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/profile.py @@ -42,9 +42,9 @@ class RobotProfile: planner_frames: tuple[str, str] # (left, right) imu_frame: str held_joints: tuple[str, ...] # upper-body joints the IK does not solve for - # the controller's shutdown command keeps the held joints at their hold, gains and torque - # when golem_safety's output is /lowcmd; otherwise it turns them off with the IK joints - shutdown_keeps_held_joints_on_lowcmd: bool + # true: the controller's shutdown command keeps the held joints at their hold, gains and + # torque; false: it turns them off with the IK joints + shutdown_keeps_held_joints: bool enabled_joints: tuple[str, ...] # joints the IK solves for, the named configs' order # read-only: name -> read-only array over enabled_joints, rad named_configs: Mapping[str, np.ndarray] @@ -120,7 +120,7 @@ def load_profile(robot: str) -> RobotProfile: planner_frames=tuple(module.PLANNER_FRAMES), imu_frame=module.IMU_FRAME, held_joints=held_joints, - shutdown_keeps_held_joints_on_lowcmd=bool(module.SHUTDOWN_KEEPS_HELD_JOINTS_ON_LOWCMD), + shutdown_keeps_held_joints=bool(module.SHUTDOWN_KEEPS_HELD_JOINTS), enabled_joints=enabled_joints, named_configs=named_configs, startup_sequence=startup_sequence, diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/robots/g1.py b/core_ws/src/golem_upper_ik/golem_upper_ik/robots/g1.py index 12350c1c..6560dbaf 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/robots/g1.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/robots/g1.py @@ -25,10 +25,9 @@ # upper-body joints the IK does not move (its reduced model locks them at their measured # angles at each goal's start); every command holds them at controller.hold_targets HELD_JOINTS = ('waist_yaw_joint', 'waist_roll_joint', 'waist_pitch_joint') -# golem_safety writes /lowcmd in every mode, and a walking policy may balance the legs, -# trained with a stiff waist: the controller's shutdown command keeps the waist at its hold, -# and only the arms go limp. -SHUTDOWN_KEEPS_HELD_JOINTS_ON_LOWCMD = True +# A walking policy may balance the legs, trained with a stiff waist: the controller's +# shutdown command keeps the waist at its hold, and only the arms go limp. +SHUTDOWN_KEEPS_HELD_JOINTS = True # the Dex3-1 palms: end effectors for velocity limiting and the ee pose topics, and the # planner's frames EE_FRAMES = ('left_hand_palm_link', 'right_hand_palm_link') @@ -71,10 +70,8 @@ STARTUP_SEQUENCE = ('home',) # Controller-config defaults the two robots do not share (golem_upper_ik.config). The -# follow gate reads golem_safety's output, /lowcmd in both G1 modes. The /lowstate tick-lag -# check is off: the real G1's tick has not been measured. +# /lowstate tick-lag check is off: the real G1's tick has not been measured. CONFIG_DEFAULTS = { - 'topics.safety_out': '/lowcmd', 'controller.max_state_lag_ms': 0.0, } # the controller config frame_task_server and joint_state_publisher load when their config diff --git a/core_ws/src/golem_upper_ik/golem_upper_ik/robots/h12.py b/core_ws/src/golem_upper_ik/golem_upper_ik/robots/h12.py index 9e1148ab..1ac27171 100644 --- a/core_ws/src/golem_upper_ik/golem_upper_ik/robots/h12.py +++ b/core_ws/src/golem_upper_ik/golem_upper_ik/robots/h12.py @@ -24,9 +24,9 @@ ) # upper-body joints the IK does not move; every command holds them at controller.hold_targets HELD_JOINTS = ('torso_joint',) -# The controller's shutdown command turns the torso off with the arms, in both modes: gravity -# barely loads the torso yaw -SHUTDOWN_KEEPS_HELD_JOINTS_ON_LOWCMD = False +# The controller's shutdown command turns the torso off with the arms: gravity barely loads +# the torso yaw +SHUTDOWN_KEEPS_HELD_JOINTS = False # end effectors for velocity limiting and the ee pose topics EE_FRAMES = ('left_wrist_yaw_link', 'right_wrist_yaw_link') # frames the OMPL planner plans for and bounds by planner.frame_* limits @@ -87,11 +87,10 @@ # hold target; the staged moves keep the hands from scraping the hips STARTUP_SEQUENCE = ('init_1', 'init_2', 'init_3', 'home') -# Controller-config defaults the two robots do not share (golem_upper_ik.config). The -# follow gate reads golem_safety's output, /lowcmd in both H1-2 modes. A goal stops when -# /lowstate's tick lags by more than max_state_lag_ms (ms; <= 0 turns the check off). +# Controller-config defaults the two robots do not share (golem_upper_ik.config). A goal +# stops when /lowstate's tick lags by more than max_state_lag_ms (ms; <= 0 turns the check +# off). CONFIG_DEFAULTS = { - 'topics.safety_out': '/lowcmd', 'controller.max_state_lag_ms': 50.0, } # the controller config frame_task_server and joint_state_publisher load when their config diff --git a/core_ws/src/golem_upper_ik/test/fixtures/README.md b/core_ws/src/golem_upper_ik/test/fixtures/README.md deleted file mode 100644 index 5753cc7e..00000000 --- a/core_ws/src/golem_upper_ik/test/fixtures/README.md +++ /dev/null @@ -1,40 +0,0 @@ -# Test fixtures - -Recorded data. The tests compare `golem_upper_ik` against these files; nothing in this -package imports the code they were recorded from, which no longer exists in this repo. -The tools that wrote them were deleted with it. - -## Where they were recorded from - -| Package | Where it lived | Recorded at | -|---|---|---| -| `h12_ros2_controller` | git submodule `core_ws/src/h12_ros2_controller`, `git@github.com:correlllab/h12_ros2_controller.git` | `b53a6b2b7fca20f8135d98d4825e0e390e549894` | -| `g1_control` | in-tree, `core_ws/src/g1/g1_control` | GOLEM `9dbcbb194d8a76e0aec3529b74d5e0079973b176` (its code is unchanged since `e5a83e0`, which touched READMEs only) | - -## The files - -| File | Holds | Read by | -|---|---|---| -| `old_controller/.json` (`h12`, `g1`) | The robot's joint tables (body, upper-body and IK joints, the old joint limits), its named configurations and start-up sequence, the planner's default frames, and for each carried controller config what that package's own `load_controller_config` returned from it: topics, network, controller, frequency, planner, gains, the clip limits and logging. The H1-2's configs are `safety_split`, `sim_safety_split`, `safety_full` and `debug`; the G1's are `g1_split` and `sim_split`. | `test_profile.py`, `test_config.py` | -| `old_controller/g1_pairing.json` | `g1_control`'s `config_pairing` verdict (`accepted`, or `refused` with its message) for every combination of 7 controller config names, 7 safety config names and `sim` (unset, false, true), without a walking policy, and its `default_controller_config` for every safety config name in the table. | `test_pairing.py` | -| `fk_.json` (`h12`, `g1`) | 50 configurations drawn with seed 20261027 and what the package's `RobotModel` and `RobotDynamics` computed at each, built from the URDF its `frame_task_server` loads: `q` (the body joints, rad, uniform within the URDF limits), `imu_quat` (w, x, y, z), `dq` (rad/s, uniform in [-1, 1]), `frames` (the placement of each end effector, planner frame and the IMU frame in the free-flyer model, `[x, y, z, rotation row by row]`), `reduced_frames` (the same in the reduced IK model, the other joints frozen at 0), `full_q`, `gravity` (N*m per body joint), `twist` (per end effector, `(vx, vy, vz, wx, wy, wz)` with the measured state at `q`) and, for the H1-2 only, `collision_free_reduced` (`g1_control` had no self-collision model). | `test_robot_model.py` | - -## What was changed after recording - -- `old_controller/g1_pairing.json`: the refusal messages named `g1_safety_layer` as the - layer that owns split mode; they name `golem_safety` now. -- `old_controller/g1_pairing.json`: it held 10 controller and 10 safety config names. The - cases of the names of configs that no longer exist (the controller configs `g1`, - `g1.yaml` and `sim`, the safety configs of the G1 mode that shared the waist and arms with - Unitree's controller, and their `default_controller_config` entries) are gone. The - refusal messages that listed those configs list the remaining ones: `one of split, - sim_split`, and `use sim_split.yaml with sim_*, g1_split.yaml with the others`. No verdict - changed. -- `old_controller/g1.json`: `configs` held the controller configs `g1` and `sim` too; they - no longer exist, and their entries are gone. The other values are unchanged. -- `old_controller/g1_pairing.json`: each combination was recorded twice, with and without - `locomotion` (a walking policy on the legs, which needed the split configs and raised a - `KeyError` with `sim` unset). `pairing_refusal` no longer takes it: the G1 bringups start - the walking policy with the split configs they always run. The cases recorded with it, and - the `locomotion` field, are gone: 147 of 294 remain, each `accepted` or `refused`. No - verdict changed. diff --git a/core_ws/src/golem_upper_ik/test/fixtures/fk_g1.json b/core_ws/src/golem_upper_ik/test/fixtures/fk_g1.json deleted file mode 100644 index c2f2c93b..00000000 --- a/core_ws/src/golem_upper_ik/test/fixtures/fk_g1.json +++ /dev/null @@ -1,17 +0,0 @@ -{ -"urdf": "g1_29dof_with_hand_rev_1_0.urdf", -"seed": 20261027, -"body_joints": ["left_hip_pitch_joint","left_hip_roll_joint","left_hip_yaw_joint","left_knee_joint","left_ankle_pitch_joint","left_ankle_roll_joint","right_hip_pitch_joint","right_hip_roll_joint","right_hip_yaw_joint","right_knee_joint","right_ankle_pitch_joint","right_ankle_roll_joint","waist_yaw_joint","waist_roll_joint","waist_pitch_joint","left_shoulder_pitch_joint","left_shoulder_roll_joint","left_shoulder_yaw_joint","left_elbow_joint","left_wrist_roll_joint","left_wrist_pitch_joint","left_wrist_yaw_joint","right_shoulder_pitch_joint","right_shoulder_roll_joint","right_shoulder_yaw_joint","right_elbow_joint","right_wrist_roll_joint","right_wrist_pitch_joint","right_wrist_yaw_joint"], -"enabled_joints": ["left_shoulder_pitch_joint","left_shoulder_roll_joint","left_shoulder_yaw_joint","left_elbow_joint","left_wrist_roll_joint","left_wrist_pitch_joint","left_wrist_yaw_joint","right_shoulder_pitch_joint","right_shoulder_roll_joint","right_shoulder_yaw_joint","right_elbow_joint","right_wrist_roll_joint","right_wrist_pitch_joint","right_wrist_yaw_joint"], -"ee_frames": ["left_hand_palm_link","right_hand_palm_link"], -"planner_frames": ["left_hand_palm_link","right_hand_palm_link"], -"imu_frame": "imu_in_pelvis", -"q": [[2.8099673109238856,2.835355398437711,-2.570812717510764,1.227540434335133,-0.3374989609735586,-0.18750867525730305,-1.2991738066241183,-1.729400966874867,-1.2124226403037035,0.5162581746745518,-0.18391105965321308,-0.13178098685768436,-1.7994601518766555,0.350087850910354,0.385190754225908,1.6144579818512543,-1.0870248729355392,-2.4924573682562245,1.2530940011666345,0.3043983134571331,-0.5650649150895581,-0.43559287589963147,-2.2845700320117697,1.393193284598862,-0.7371589480949257,-0.8987143730472128,1.5979757755465296,-0.355642569676891,0.4522919230801017],[-0.8510992551705563,1.18239289537875,-2.3862491618927657,1.597307701491228,-0.6829228459122711,0.010263804109841845,1.897700431376371,-2.5505440863732525,-2.411900085374924,0.9808443158408728,0.40684193939818625,0.20867108346904717,-2.2338293311381316,0.5155975932139694,-0.05091309280920375,1.146599144571291,0.8264323396977016,-0.436363718082049,1.7060666678737995,0.43919057243305426,1.1862968603723945,-0.9671473703999758,2.4772833322417824,-1.8574986402633074,0.5097805466367427,-0.05124052834277226,1.547966504681498,-1.063073273276633,-0.2593157184068551],[0.4769841655971372,1.0962988789561954,-0.8980603038871027,0.3314125137014461,0.23524323715356787,-0.030239731749173077,-0.62546821018669,-1.601984990526715,1.478700858311191,1.873904303727157,0.2053573733617432,-0.1573651043036454,0.5047930972590393,-0.0658278829341824,0.1186695190360505,0.9274178759824503,1.8565537280678326,1.5342983405821475,-0.4524855181576517,-1.2669135945455783,-0.7172918275602816,-0.5935140477498786,-1.3507195501837077,-1.4317366600737953,1.97583477788432,-0.04902549333597461,-0.21030657656128104,-0.8706450034684666,0.7099719616858031],[1.1232134766447386,-0.5152801757487465,2.2638558496543206,2.0586509012671574,-0.6325862154789423,-0.15550411156924648,0.699770442043615,-0.3795782178703222,1.792042906434757,2.0350553455442353,0.14730959703718682,-0.08736064569978971,-1.2705960763089372,-0.26393074757583124,0.2224768615560082,2.470167221157907,0.7412397512569808,1.3661194411941748,1.9607524716138063,-1.9410421587227482,-0.30754024459164464,1.143364112139052,0.33498068862289765,0.3166111075092961,-0.8911587656169033,-0.3524904481925504,1.366232927936235,0.7907454037640729,0.4131872470758642],[1.806558645279678,0.48713662399451574,-1.7624834685429207,2.166488836565179,0.1175967024340766,0.026295386171465818,2.421181241687092,-1.009973986977391,-2.3046087306620318,2.3419527790847523,-0.038105576023000465,0.11112133637840277,-1.0327595290996545,-0.1716678260812718,0.030777405156761195,1.2526125059908075,0.8054244069338692,-0.5778639168960154,-0.8299077645654174,1.9052121962259938,-0.44955845747281176,0.8629521967140568,-0.9415019056093437,0.9220320567583777,2.1236408940989686,-0.8825349746510227,-1.2564013097362503,0.7429898973281206,-0.7017586979080691],[0.727590292012426,2.888795087254544,-2.0639707323777987,2.7809495029867213,0.41848471204077875,-0.13638676901903776,-1.6134607652052777,0.08845729413560344,-1.6393142412732888,2.587923112928488,-0.6499420684215109,-0.20726281803676855,2.5828873685629534,-0.04014191818132101,-0.055568287380867,-2.4751939483821834,2.2183602810709124,-1.7961518263631127,1.3168771035958595,1.554766431348174,-0.5152682614543425,0.120874524472105,-1.369765740282072,1.1434113407914523,-1.8840796128134125,0.030181294445242868,-1.1872532508043396,1.112214913410013,1.3758724191194622],[-1.695434035173172,2.7710478997495915,2.7328715161856785,1.32082129170695,0.20753951683926675,0.10115565452440978,-0.44920019999173055,0.3739364401371983,0.6012373511070761,2.5082917858144644,0.281964857748474,-0.11312190770921124,-0.27615696285805047,-0.2981718127347859,-0.45870044167441976,-1.5224831034962398,0.9513852793208724,-0.2417846773808061,-0.9150092163129504,-1.352234422001081,-0.7516528153390128,-0.42675840811376875,0.9741860005455001,1.035723828737075,2.1613605433521665,2.088416382408325,0.41208949724709454,-1.541836626658006,1.2353920647818504],[0.4975332386031046,2.355322155995005,1.9919055878054577,0.8799354854922198,-0.11495933660936031,0.07132122255612172,2.1618207943972965,-2.704880000369937,2.073542667159237,0.9778706776391233,0.2578999963584384,-0.04788435244666586,0.5636089364943833,0.4598953474451658,-0.21275324162592718,0.7135323378128429,1.0381327423182762,-1.1372450868330561,-0.04043465004628155,-1.1953449287434896,-0.3555353761495914,0.35545046395902347,-1.5951210486995309,0.3268510860082867,0.36028399671223266,2.011516549177525,1.3006334199761862,-1.0250614008784262,-0.2855407941952932],[2.682763755221598,-0.5163147313425483,-1.5728204455335022,1.9306251005142039,-0.4565826637686854,0.10242392515266574,1.162826238820653,-0.9854925442745719,-1.034603374740787,1.1811381193367998,-0.6820838134767003,0.02413339000989434,-1.0959059993825933,-0.48304238744110173,0.13323623436758503,-0.0636032523951724,0.7721330918059826,-0.7388872478980091,1.082971084657255,0.03508545168847643,1.344809313194421,0.12216831338706324,-2.5839680710974307,-1.718274339610895,1.8686155485453324,-0.9427369028881446,-0.967963259886907,0.8160984137809886,-1.1120315351898635],[0.9407075541986862,2.8452259870720824,-0.6726930440416465,1.531904401525803,0.0655284298488652,0.21310694623164822,1.4107316280086413,-1.4432650239781806,0.5325652523386033,0.318374060567256,-0.2626400719246792,-0.0750073737456157,2.238717835430585,-0.2679090404098739,-0.12175867565852716,-2.1067004734084995,-0.4387215991037874,1.3037424114864047,-0.7963863100489668,1.6462872825232684,0.1412813093341314,1.5843071594158078,-2.000422647025765,1.2862835094878373,-0.5544434809271546,0.18542395351052865,0.8385857908791401,-0.8336933418509177,0.9647652800283166],[-1.6228606066895863,2.8350138487679377,0.06084380792963451,1.437292423562193,0.18429135803445307,-0.020153852755175478,-1.781298421510233,-1.5778749980974769,-2.170653192562861,2.2448498309947933,0.14301537614489745,-0.07779728426715382,2.3924474468940526,0.15860230334077374,-0.4778494498336641,-0.7944165606800371,1.8366773291298713,1.745637673469472,2.0655825144447357,-1.5423450069532274,-0.11619401440360688,-0.16608292193053265,1.0289428412374173,1.2859208930027632,-2.013664935715008,-0.7282162149785666,1.6831768629096806,-0.1463064946211141,1.003576099056221],[-0.7828101532521017,1.7009531738580406,1.9915534621329893,0.8244728392603266,-0.09331087978043706,-0.21097905254536722,1.7001729928126266,0.4813686014396126,0.6841164846723218,0.179154810066132,-0.4421857719548557,0.10776469502150632,0.997078685676513,-0.49616106577362923,0.40482276209261137,0.6570131320040553,-0.16595379063092164,2.261150896689244,0.5461058257876465,1.2260926075833631,-1.5538240494258946,-0.43007661743814585,-2.4964740077684775,-0.5222156274537637,-1.2301567063621737,1.3006157099287996,-0.7652468633414651,0.5405918124883964,1.2040121506217891],[2.6476069472645007,2.264793707364517,0.49748542325384815,-0.0655501406279025,0.46513125025706736,-0.16924088610625332,1.9247338912041,-2.807971590696314,-1.6782419488437639,2.315597207255253,0.06480806206129752,-0.12166897395739801,-0.5294402435078496,0.13655860061118974,-0.007684227280339462,-0.5612777894462022,0.3798652710760613,0.9052505631380003,1.675753697312105,-1.0478245416529042,-0.8178770239885055,1.3091895473602848,-1.3589065176127986,-0.4797255783832046,-2.067790833817721,1.5826024821281004,0.4880005824210145,-0.9276978507570848,0.439484819965114],[0.15764848494887485,0.24101563318873942,-2.047354946851464,0.23630537683799768,-0.3012667471236645,0.13174225263833506,-1.9318063907187262,-0.21273782101055438,-0.9218822628296328,1.5896364936945129,-0.48429963167189105,0.06891131835410647,2.070070512597139,-0.037430754268546196,-0.0767133065545072,0.16760034631451282,2.226902560344437,-0.39592356134849505,-0.9896723708972168,0.3133447457709124,-0.05930895916901213,1.439366077352734,0.3191496127981339,0.0989725080518733,-0.39335168594139214,1.6907224000015548,-0.4779841891502963,-0.29748030038518003,0.6766435538464102],[-0.30231189579116213,2.735428533890301,0.5339852942212446,1.0591537479236894,-0.4381607193543413,0.14398811816035922,-1.3105358164423904,0.4776987386315894,-0.179714682989359,1.624545490744828,0.00802949240634443,0.23447028904143757,-1.03657544993406,0.23990563647408014,0.08394271969908718,0.7681930784081876,-0.544781082706542,1.475255356022989,-0.6268260647311961,1.6803294265185817,1.190869178379544,-0.6364042478617776,-0.016866529168876543,-0.15450275144459802,-2.412197417091709,0.6900384181101207,0.19582283089451913,0.46864090300418826,0.7754303557579532],[0.6878808105933754,2.7545812215380545,-1.0104749465512846,1.7821216973351057,-0.31346379280784564,-0.057668574183264154,-2.4043532881829757,0.01326936806307133,-2.5058678632295277,1.0794460614447914,-0.5578298679408455,-0.024290026258186825,-0.4313912792479955,0.3241222880144615,-0.45737491698164096,-1.1611385167288977,-1.2227312802974153,2.32752128418725,1.7894906607931473,-1.2732933571672644,-1.6105825335292119,-0.9003888537936862,0.7790636339621724,-1.8886638925015875,1.7200049380736409,2.071549387729384,-0.6832227482270172,0.6346545917837012,0.7237552465823824],[0.13217582473062617,0.7679489085423795,-1.7967596888316781,1.6816147727053687,0.41233492039729314,0.1321330054887081,-1.4291908698369762,-2.9648856132224624,0.9393390233662009,0.9278658890266915,-0.16312653711524272,0.08126998095946086,-1.8485830621888772,-0.2414100401114671,-0.38275809383829923,1.6151010734602735,-0.7414007343149494,1.5629969236193193,-0.6271470812271999,-1.8775709217474146,-0.5256241638134529,0.9871548218565744,0.3104732066585112,1.1733502661972444,-2.248138239912695,-0.0861292414839755,-0.5875246464096626,0.31021043814251015,-0.5408946858637755],[0.8544904044503259,2.782108077743675,1.1421562357046988,2.1204702143014886,0.11711859038876882,-0.16626438466716426,2.6268512165539577,-0.49114323098904666,-1.1739044881680156,0.7497695054319581,-0.3081608646015148,0.11332521547540653,1.8975523816022508,-0.18062841282785558,-0.14190884499410944,-2.2947430608254615,-1.2882953671694743,-2.441713013790702,0.2647178991016519,-0.8379941316916497,-0.9428729149843454,0.7089476747689829,0.9187650957730709,1.4664185334557085,0.3872554057004107,1.1223037933404358,-1.0438006732126697,0.742758209023602,-0.9935031993929039],[-1.046867608035519,2.002851290854081,2.4890477862767195,1.6238839976969586,-0.5191368507019989,-0.17777855951299365,2.402903889844237,-0.5679473880703636,-0.5315692655475628,-0.05024240624195862,0.09692672000814118,-0.009904194203215644,-2.1659480049869018,0.31534252622736403,-0.2944430419632354,2.048970651350886,-1.5055444528009767,2.271257720459762,0.6248758354435899,0.3229183160702269,0.8653683599147108,-1.1534270258182766,1.6273306206924287,-1.5248405466650146,2.1320697071485077,0.5609642933230967,1.89322666063322,0.48546245591667403,-0.44697189735002496],[-0.7694295690803181,0.5948676820841904,-0.41103188781723743,1.4667535043333448,-0.8143594444424718,-0.0289680601419883,0.9003812155834399,0.3071111242381406,-1.320462886604482,0.6002254917694736,-0.19430058824363738,-0.10730862645991324,1.9154676649387263,-0.14625747797459882,0.30386893515596247,-2.7665502849214922,-1.5294956014274066,0.8900877242904808,-0.7630067199738022,-1.0340988739058066,-0.8470619179521944,-0.1047126611121576,-0.10389738786562264,-2.154200282599504,2.5897480760207707,0.4731272170787042,-1.6228138178629383,0.7016243327761524,-1.4467291839356429],[-1.456038557864408,-0.4360749174727264,-0.1586808503732624,2.254365464344565,0.3952437881207719,-0.008901471133528405,1.0577723873333023,-2.2209918069133794,-1.1974177271085449,0.09698915365078493,0.22060815869121908,-0.09460837608930547,0.8336923152268274,-0.5144043937304031,-0.45896665209991544,-2.2571853124893475,-0.8991713361320748,2.5676169788288727,0.9685880915869156,1.0654067094035824,1.552841578690302,1.5535128601383235,-0.7746729401650976,-0.3553305997625966,-0.7686304101317394,0.2332225023086556,-0.1626467703212695,-0.4578214460921295,-0.5100690344071563],[0.5563399314691257,1.1318751954442106,1.450307723403668,1.3373712088325607,0.019354646552660726,0.16688862099570884,0.8591835180708545,-1.4042008578579723,-1.3705138221317628,1.2302193659052743,0.20481738710553798,-0.051451068598811794,2.443722712628461,0.26422392903134695,0.3695569222713533,-1.0577012599369788,-1.1545012207869156,-0.2421693516742658,-0.15579367149276513,1.508051788058936,1.5016605555128344,0.6057168350974018,-1.1921269670970716,-1.369571990501166,0.7233323844750639,0.6840164157043298,-0.4557260282357063,-1.1575755101843452,-1.3513390168770019],[-0.9285866051398959,0.3206777915642419,2.3286330879897825,0.27167591262160246,-0.2415717819561819,0.01732170444060882,2.164321568512094,-2.5723795695520786,0.6640265744092733,0.9688434805397679,-0.662379968976518,0.049134829517655854,-0.10792219804707548,-0.38797232182096675,-0.2622919077585702,-1.641767363589287,-0.6705372968874842,2.4097530003125303,1.8975936696979356,1.628076100919008,0.2400300635697934,-1.2204786584049467,1.474243139991077,0.20272111961479533,0.1025481494285363,0.012421559461292198,1.6163285550409254,-0.3375136880025016,-0.20197889471175134],[-1.8645129769894213,0.0699471644390044,-1.4502946772609688,1.3639451989717948,0.49246200001218343,-0.07013052835686712,-2.162342614443062,-0.5279087618817639,-0.833651108484107,0.5885138318587836,-0.8525571950069137,-0.22522967952994566,-0.6222167627763173,0.4039781335613588,-0.5016452107638696,-0.40418607221634595,1.8113078980506438,-0.49558734918502934,-0.7858028532517634,0.9173076728896778,-1.5566291683552802,1.0936088998866735,1.0250473726567355,1.3741423587868007,-0.7782455183878934,-0.6925141981318658,-0.061391754369353624,-1.5459413239167017,1.6075591960055797],[0.7515959710403166,2.2234448306312924,1.5517575212532382,-0.008946089518886341,-0.39858114995210453,-0.25765081256501815,-2.3058287854735577,-2.6742369524920915,-1.4185732272893674,1.6189221811419892,0.5142966732719624,0.24550756709006144,-1.7949894415345309,0.05401921309955704,0.29763028973666716,-2.673794569723711,-0.6244666109947208,0.07496748524681385,-0.4102392026375258,0.8314721046769069,-1.1525315595324357,1.4590529965462096,1.139268171606175,-0.4369742091629303,-2.1866064303127115,-0.6280346737916938,1.254755243486227,0.060390784689829724,-0.6535279263763214],[1.3088113887096307,0.34575299260978043,0.8759275464664724,-0.0630546812260869,0.5142086302718157,0.12103133895364532,-1.1770009855568553,-2.3607198950822403,1.9960923613242736,2.695285193326681,0.29794160133137193,0.08708572206413234,-0.19574386924227927,-0.3253261191709165,-0.3881096454008386,0.02479337301932416,1.5026468155516302,-0.7568563525133172,-0.6650049185702315,0.2970635319343229,-0.6143141566449364,-0.7295530248359533,-1.3916448451108856,-1.1793691391558807,-0.5726532063779493,1.8261401679119549,-0.22324622700008967,-1.0836035579256658,0.015332656987196103],[-0.4942364755720434,2.4460661303067663,-1.6925758200056351,2.229827334880297,0.2540861169967238,-0.25533871336351166,1.4027593842309525,-2.6230805104980046,0.7077441834574327,0.18698714772618913,-0.3076540361620075,-0.25266733370830347,-0.3124837184657552,0.022725414017911172,-0.4846999776816813,-1.5196629646908442,-1.187791142366113,-0.7809453413577248,-0.6561654472142069,-1.2224414304403015,0.9161985760093969,0.42734519776738566,0.5904878163275944,-0.12210146149576362,-0.6010838301230748,0.3617678327547966,-0.04614410908589939,0.3338492807723017,-1.14829846269252],[2.8321775834424145,2.720308453494362,-1.4630402847799011,0.26277530568399404,0.06339441608035179,0.1979557746664003,-0.7504647691602546,-2.0007216097871243,2.1263439819884873,0.6655699787597592,0.4977767279504307,0.004348153926619414,-0.3072942366279161,0.11151524929592838,0.03789354945999601,-1.1075806666996193,1.627386638182032,-1.3499087330889272,1.2774630380105332,1.9151372518338756,0.20065794306677853,0.27937136563028764,2.5065547377168373,-1.8631243404751425,-0.8652751080734036,0.8480978907322119,-0.3608440525355583,-1.5197474442462722,1.5392992437088504],[-1.652283038267542,2.6230708920312478,-1.0763723626813915,2.6779890520260934,-0.23937392724534434,-0.1748032479107094,-1.065325427684375,-2.3968227008991887,1.711298198646837,1.1088960098725738,0.16028046558839504,0.0581931909789824,1.708445448575278,-0.3698052696797023,-0.3243705684822522,0.7833020808076108,-1.1093065966513902,2.464812330061395,-0.20070553075615416,0.6304063351819025,-0.6793217623086187,-0.9451054616088825,-2.6338316050910895,1.4100554309435513,2.048554717708115,0.16292845586241445,-0.7975541886619688,-0.37896265716915467,1.5100358766580475],[-2.046231462898427,-0.03172311157641494,1.5961988445955617,2.03874572647288,0.23504092310400215,-0.07705166245609843,0.06675773867270651,-2.0160242165939053,-0.14512563239280274,2.1926785551188277,-0.5443796212415306,-0.24099814631574168,-1.2894466546236927,0.012675262420216105,0.4299766411067011,-0.8560903801391024,1.1401234746450677,-2.1410899389998255,1.005974643104936,-1.2437580532645351,-0.7635789279335653,-0.7954683190573769,2.1044482706343857,0.78216001481421,-0.007666309249088066,0.6781028448599546,-0.2854033386315997,-1.1542684152308873,-0.9666745581601263],[1.9057210252948402,0.8491804938908366,0.6793439225025453,0.8768428329895982,-0.7192757098816402,0.1517911628652438,1.6198872539809064,-1.4673329343770933,-2.04081940763649,0.006064929992148249,-0.027951657334767233,-0.1801537563290867,2.0945381920638835,0.3317943436773605,0.2650252881651838,0.563363906289577,-1.2557663643725099,1.9175086123330707,-0.5777214211236834,-1.1326753692521625,-0.20291927949361788,0.24275491792577952,-1.9529596231556312,-0.38653824786371915,0.8351756108533981,-0.9781845421501357,1.0710348921751218,-0.42813635780521064,1.4447897535189205],[-0.28556929698537026,1.1345725101694533,1.3382412520828542,1.3646522165006116,-0.41401100056201684,0.09695930065762326,-1.342187046667096,-0.46230356100851644,-1.1304716315437489,2.0696986676108895,-0.8618217626323038,0.019309624964845673,2.0191113010595116,-0.2318755919522843,0.37858110187610583,2.097237321942103,2.0420028903796332,0.6212605118996577,1.1701137031868611,-0.0009638342451685489,0.19933710287282835,-0.3793607631831646,-0.46332626668785526,-1.3591005667104308,-2.385663027412205,0.7178964151245126,-0.7596019735325321,1.5871276288646634,1.163942198678672],[-0.18215906245569347,2.186325220414071,0.04814731575732445,2.2470023470250045,0.3292771829164417,0.21088414625850932,2.1433644486745247,-2.0406775736466054,1.0726689876167157,1.2034801302282812,0.485626020031828,0.25048465226074584,2.288728115412393,-0.29148617657119563,0.40564216693193067,-1.6364572622310403,1.9148219960012511,0.6409620258813207,-0.4266910119534141,-1.8398853425671033,-0.04094882554170143,0.7314960361466445,0.8774892290101604,-0.17855693298552922,0.39777763138173317,1.2453148497954374,0.45283604958809986,-0.7955815608858096,-0.0638139247095939],[-2.4762892357118083,0.748550404047935,-2.6740096903652963,2.480613795493216,0.020249900570680768,0.1527420941489887,2.371172749547848,-1.3476201816934514,2.4700354985002146,1.081009868609201,-0.27828073914835727,-0.019711651799546315,-0.8915424223660733,0.30637187371272434,-0.26297244835122,-2.45485130972985,0.7701022779532347,-0.6883286300537561,0.20295899347461543,-1.2442983023669925,-1.0697225823491534,1.4369705259792924,0.22711702730272476,0.2186824072618636,-0.4586308369900496,-0.5653510505796885,-0.1526342687570057,1.611225167422336,0.7773273742503786],[1.681645979084196,1.3013357261636753,-0.034216374100818125,1.1670477689430216,-0.36345160319295167,-0.09488555724973297,2.130676239675594,-2.3401935497794053,-2.507069935923152,0.5041921061059722,0.36547501423360507,0.22578316290893552,1.6096752997969168,0.04565409954908839,-0.12382811146186307,-1.435257450801731,-1.2937436671708475,1.9211855467884758,-0.2891826746165087,-0.7088381757721463,-1.5265422579154575,1.456576529657956,2.3983217036625177,-1.914401953077596,0.11063870930389763,0.8384365728255054,-1.248400089911423,0.7147429860432959,0.013724575766882019],[-0.8114110363772753,0.7218193835928745,-0.9721544619038902,0.3314239171590421,0.4364617966603018,-0.21485327309585558,-1.9787176095948034,0.4482896234674021,2.034717884075529,-0.05499945892771368,-0.6188162007329008,0.06973380247322825,-0.6480270580331142,0.02247249470762258,-0.32145001542666285,-2.4118515603619386,-0.8078701710214249,0.5261963685390558,1.4134246633000929,-1.3152599202213162,-0.9928432841179464,0.5600685009504505,0.847300084283416,0.9129965032410325,1.1537699591116257,1.418930275776537,-1.1680660786107726,0.3872789471304594,1.372110171008323],[2.755293241207244,1.5501648357236997,-1.7195979991114159,0.3717654160504739,-0.7557742841306758,-0.1694643858159592,0.8978016139688276,0.0686401260743339,2.4266099847366194,1.0732292545019662,-0.38311273873973267,0.11008453538448776,-2.6152267712486577,-0.46840881453866284,0.03500938357844341,0.043752537752482556,-0.6306531995206853,1.4421575867965308,0.8829263537788752,-1.9481627660348655,0.42554004837320325,-0.7302011577222566,0.23072956085036278,0.06875331937267681,2.3334028309035353,-0.8575096963351121,-1.1852774950448164,0.7598606841956805,-0.8405118188787052],[2.107900460595309,1.2794948962843558,0.48601745798638474,0.08179064482306746,0.48603066093350356,-0.03763278415085139,-2.059838982233966,0.32682450415222464,0.7513122721960972,2.2997358963361134,-0.4223093647115032,-0.022038078766091568,-0.8037586018943237,-0.39693152324799325,0.3187433731759933,-1.7240228350019107,-1.1679668764859412,-1.5153506390862284,1.5621715079359497,1.9133530475179459,0.6207883561709873,1.4079001448471014,-2.1272387831361406,-0.5880035434186017,-2.219362644068445,-0.34189418373655023,-0.8787558962032922,-0.41168589414580903,-1.1832143612436377],[-2.0612138202823553,2.1797972588363037,-2.6644605652239934,0.6336731557657737,-0.13753826620099596,-0.2258707293111545,0.4875664246622722,-0.5242276101237313,-0.5699192291640323,0.06646345217840009,-0.7847860331171215,-0.17252415930085171,2.3222412150506737,0.34382075205722495,0.48638187666071553,0.5452719127034689,1.974347689339118,-1.5535554995834127,-0.08434479694280728,1.953815540850158,1.133300145282022,-1.233334850382046,1.0852652168952401,-0.2531191618540545,0.3968057716013047,0.030849439885405783,-0.7772780409441231,-1.57954068907411,-1.2105897633436853],[1.8988252127392307,2.4841457644093565,0.8921020286264678,0.876732854476696,0.2947916538635956,-0.0676608393967219,-0.03282722609778732,-2.892913712446632,-1.533449743315042,0.6402548657961233,-0.6646312341565878,-0.17713876568046705,2.3574876105939375,-0.13900682560038685,-0.3634259621185538,2.0114373680609905,0.5198312688233147,-0.6134032410522092,-0.18993424899062272,1.8287961278124985,-0.9904044185877671,-1.034745971022535,-0.9172614718687839,-0.5038407162848182,1.305924280685777,0.7258149858359708,-0.7520628451074436,-0.43378093855651945,-0.758878508515994],[-0.8176454704792997,1.6338109037170465,-0.9640446122196973,2.547916218166536,-0.011024502243594636,-0.11590871016705964,1.3234354990686277,0.13488322897125515,-2.7062273114567836,1.581900843864413,0.34024264965328854,0.2295683006805534,-1.0640876709787905,-0.224572510152811,-0.451120982705649,-2.6164700173174484,-1.1044683550611605,-1.5946673263012061,1.9067486932229858,0.7243518102605493,-1.3117423681346054,0.14452275300279038,0.8250273459206907,0.9329959985524421,2.2031037527161357,-0.3448583420265878,-0.8204107338321607,-0.6414482922499435,-0.5806704221464953],[2.527019672523387,1.2387717751361431,1.0062261433482873,0.9141898929846153,-0.4726377951691134,0.111424312667744,2.5293800876726302,-0.434289809077415,0.5432180947161016,-0.0752611673987877,0.08180956049897392,0.14963524285875185,-1.327673915798121,0.19829906891934412,-0.1484542830410855,-2.02081925050512,0.18273837367466372,0.3374710008596309,0.6412665580967438,-1.2456567119145177,-0.6214874016265093,1.3423628752634664,0.5100632829420371,0.2863304858319826,-0.013014449136853123,1.7630242933701517,1.791321210855942,-0.6854955550772183,-0.7691830040973225],[0.48036514560108934,1.825719176492563,-0.80489148768077,2.7079228668928086,0.17897611187164697,-0.21468511301189788,1.4210492041467857,0.36541362100436947,0.2894781641249238,2.42227195904287,-0.7208681585335329,0.016183289296604952,1.3950641526173926,0.2569809014648241,0.04620827845246278,-3.064095626101485,-0.04644180279282284,1.97676089692087,1.2791015567392356,1.381894319892878,-1.1284576178230086,1.127421796159083,2.5579205803576937,-2.0488867502177395,-0.5100451367650014,0.39891751355919647,-1.3626313751971784,-1.1945864890114666,-1.1337414852967593],[-1.6854242453729449,2.277504325490409,1.9463973295510613,1.6895006510693031,-0.8011328789003458,-7.16183124583325e-05,1.726680365182705,-2.575313523949977,0.7181138244863927,0.07282752022882562,0.12621281270774987,0.20427907039555127,-1.5664523125350323,-0.3565056704427664,0.09556449669016065,2.0371875176958314,1.2063385116493195,-1.6289269021985895,0.7097149981396051,-1.5494300161141144,-0.8795616021634939,-0.7513656380853103,-0.2514299712621062,-1.0795484185499906,2.2066248360467022,0.4009844967202363,1.3068725804079073,1.5001093608111795,1.4716024664493037],[2.8770463115795724,1.0985878956705966,0.35964810615187126,0.9942854089404018,-0.3190726262733804,0.16443338204114988,1.1651175900969064,-0.6677342209578332,-0.5304594553710027,0.5521631294312352,0.185047959081592,-0.033249795479915606,1.519357106288394,-0.031309399953174555,-0.36748078765056474,-1.0517967656813387,0.3451680618347941,-1.9814690648668984,-0.20991256072424136,-0.6479604401007986,-0.6502269054500038,0.897379895785644,0.45082732984939256,-1.5362867425078433,-0.03486449283689996,1.8649098047923143,1.6907246129174722,0.05218307619652518,1.3426183717404074],[-0.8588843081577748,0.33070355240656735,0.5193908366452686,2.0651692345863233,-0.5422029499495631,-0.19970665151491135,-0.7095781721203016,-0.14182470897218602,-2.0724806948996095,2.252559405131263,0.08299051789347833,-0.007960027862373209,-0.33900043474644237,0.25306928429435094,-0.38806297034384396,-1.5507426247189415,0.7825167005902065,2.0829961810677085,-0.13661999842686623,-1.3462019523849027,0.19843821255286986,-1.348476317596474,-1.8562198479179532,-0.6589551527388384,-2.4637346063549477,0.10576218640878321,0.40797164868165847,1.1516095453452422,-1.310477343293717],[1.2745472916124125,2.238808722311695,1.7987556473809594,0.7837213290013131,-0.6512644846759663,-0.16334645194827863,-0.3077797557525286,-1.1719615007251845,-0.9741817191145574,0.8302962614891507,0.3271105695532003,0.09543312020885403,1.3490465732980623,-0.2461865659930048,-0.04146178144422141,0.9512591900916485,-0.8255296522467707,-1.5861595013484022,1.3981889782675474,-0.49093096076930953,1.2157423738511908,-0.8575559024273668,0.7494994300918751,-1.1257416395039102,2.392950131578694,1.645267044576561,-1.7007042375706432,-0.7910807901095753,-0.3994139788268132],[-1.2344504267645875,0.4769082360461617,1.6574836160187854,2.873083951834746,0.11302516344436908,0.030567736623401787,1.1298467203424822,0.49665094804114407,2.629388307156683,1.151692571584274,-0.33103240880702933,0.2064096577330105,-1.3286376511981068,0.39512615218565006,0.44083412447118564,2.348421108234642,2.182468424466427,-0.47895154961687947,0.5079052637051251,-0.6089128138166722,-0.1903959090714693,0.17091596566538603,-1.4487686356296101,-0.7344607913567531,-2.1429567330926416,1.714972458236297,-1.8136787105142849,1.2859804190530466,1.540493506522142],[0.5136582184243852,2.401600965855456,-0.9841241202249806,0.14679006934326527,0.02664072070987411,0.10051150967858813,-2.505062565055145,-2.828702547145375,-0.6043012686204503,2.709612442735168,0.06995999917615114,0.255399059701564,-1.7119555814245369,0.5163059783830615,-0.5089339489744972,1.3345339468357889,-0.7041370911691202,1.6048690342502065,0.40046402096409484,-1.6155763930757976,-1.2178582515544962,-1.0883060570476895,2.32336668473706,-1.6745515171236902,2.441293682278674,1.1790050132423928,-1.0487213582036534,0.17726529696920523,-0.7176103413088061],[-1.2763576831058934,2.7431614004093348,-1.1571281279294454,0.34730219274278207,-0.5095323895715995,0.2303106486467965,1.9989003438473252,-1.5571437100601415,-1.4747580576927561,2.199931784947385,-0.7677271551479412,-0.25081041777122814,0.9089953301752942,0.3381444334873659,-0.11244943841238086,0.6245379203057806,0.4346720814563203,-0.1946183310553562,0.5786627240778852,0.978861872896156,-0.017276024385645128,0.2693763076359368,1.2342430832907638,0.3650280874666305,-2.43919395299781,0.9507325692064355,1.1588316700039591,1.2967484420439852,-1.1252887867704895]], -"imu_quat": [[0.038359244731825906,0.32158718716749124,-0.16762835728739844,-0.9311342455448801],[-0.007897855354256045,-0.4056531133800262,-0.06485142648470477,0.9116893483905544],[-0.6627112086106901,0.5398121364372905,-0.0524429716779594,-0.5163975658911952],[0.5171885318701547,0.7119044205364613,0.37913486596587975,0.2862950784272649],[-0.7413002972735878,-0.5302826811962618,-0.21502300200207358,-0.3507695196213751],[-0.07995695199428071,0.29996582511733405,0.5839999861085168,0.7500476023658189],[-0.26972458553592726,0.9097005762162573,0.1622510603090228,0.27086547033145325],[-0.39959719746329914,0.13173029506979586,-0.12590010887099284,0.8983976690344727],[-0.23621302585415432,0.7622480870388769,0.548232288120431,0.2502451168047724],[0.12106066731079984,-0.47566403321108475,0.8633468413651623,-0.11713357266140774],[0.6192555002484069,0.6010939698522009,0.4632430077479963,-0.2015305946798312],[-0.5425803142900228,-0.13987765682122913,0.25585128857425293,0.7877696121341802],[0.13463066874872387,0.2630106427174576,0.04297647336967134,0.9543861941511557],[-0.229528411656779,0.05329851108412284,-0.9358582143752238,-0.26200263270614177],[0.8127571059564922,-0.4417649912682552,0.14058633100290127,0.35285274937589545],[-0.15975002084456166,0.28956759349760713,-0.05651281779240322,0.9420386621903208],[-0.3573905157886346,-0.5270283452524418,0.6240048313818112,-0.45291402378041273],[0.47255483738861687,0.32347202989275814,-0.8073899509479278,-0.14205364707032014],[0.549416516409972,0.5167079155072625,0.024431295214530854,0.6561688299227655],[-0.7354041888866554,-0.1527407479618371,0.43147284963284244,-0.49968202179959775],[0.37597720602799006,-0.21516987442809038,0.39537589979901316,-0.809951210594877],[0.4953087902917149,-0.26777380692818403,0.08898412885801867,-0.82161317868832],[0.48551865234036706,0.12387703091960253,0.01953034567622014,0.8651847693053231],[0.6430087903621419,-0.06840529426450086,-0.7583030440572386,-0.08268557677810301],[0.644109618161359,0.44703146426115026,0.5826983418977622,-0.21388855065701556],[0.3810673196753423,-0.41072642044866553,-0.5695702111709153,-0.6013994346252709],[-0.15985998391527945,-0.10786906709212603,0.3154254582375202,-0.9291479054504355],[0.8575528233211676,0.25571709173945895,0.12856679776317048,0.42741373716708203],[-0.31645982475383566,-0.9357585530532712,-0.15497966705893315,-0.013799003685841686],[0.27742152535329206,0.8693097405616301,0.2961691661539263,0.2821731689140768],[-0.25569557031782836,0.8256491168250285,0.5023246886536863,-0.024356074701565467],[0.06045825707327685,0.715919177460015,-0.3057963053512222,-0.6247344636965207],[0.3246425767965409,-0.29034346077298817,-0.5585368594920506,-0.7059351589955154],[0.21252387885967805,-0.3384381036053822,-0.8572929981343554,0.3245642714351619],[-0.6454722003081614,0.28280329893040335,0.6595195601141308,0.2615757683163432],[0.487418867058437,-0.856576277430812,-0.04121006644914245,0.16432181657193162],[-0.8522447921411459,0.036371411092048174,0.5147265105932718,0.08609619048449613],[-0.4181178624581449,-0.2022061269012739,0.8664679107363389,-0.18309422437974907],[-0.7697153385222448,0.13468803500583593,-0.5917378683782779,0.19810029277359661],[0.6486774108883474,0.1356423397585882,0.46736458947567994,-0.585140250514554],[0.9870923669854011,0.03447141577327857,-0.0661730085630664,0.1417092568305134],[0.03734256691183191,0.3069668927217551,-0.8778729632460858,-0.36567187459623185],[0.3314384307866685,-0.28529183165868816,0.7359160085854229,-0.5168991832980597],[-0.06588618379512737,-0.0021031764122114325,-0.5068291334561768,0.8595224353755723],[-0.5994844569381976,0.36882286105706147,0.6627582464593187,0.25561609847849304],[0.5206489877126583,0.5501765881310782,-0.24835384542422312,-0.6037803581843577],[-0.017305526102822877,-0.4821980934702523,-0.8758856526712694,0.003137015234647543],[0.1913220121487076,-0.744756814635263,-0.5815556380256038,0.2655677213110415],[-0.3573866486396536,-0.2737410981380604,0.38354177413152074,0.8063723098298522],[0.24726067599564114,0.3560707824352515,-0.27789606344378404,0.8572336518852809]], -"dq": [[0.7576510969559009,0.6956300336464714,0.9789647489881865,-0.57520851142901,0.13547532261195405,-0.29365507745723596,0.36899078083211423,0.028330301403755476,0.7608588995354935,0.8287244324292202,0.6084222666543444,-0.5950600525993226,-0.9787187505251616,-0.011507019975504518,0.7628240489608462,0.9175655414772164,-0.8929660717734405,0.6155792346305582,-0.2243678867373402,0.9956746327012398,-0.9106566040346991,0.7016856715175628,0.5153046591997688,-0.34612973930718494,0.4617843175072105,-0.9813721906580852,0.9394678754798074,-0.24619215787277482,0.6965444846289324],[0.29642007669403614,0.11895423389808268,0.6664530220232523,0.9539484365037774,0.2834150007909628,0.7304348078674345,0.005838898972253981,0.41033057769828796,0.6813751462973137,0.8656138995918985,-0.5287541662429311,0.8402928753646197,-0.8848968807727637,0.7586589312594316,0.3545444967335212,0.860442712463918,-0.9197969480364727,-0.15389789225896222,0.6203694308325707,-0.6441083507773231,0.45487646501600976,-0.14150770458028816,-0.23456698119101005,-0.6820402433401207,-0.8499472029759894,-0.4967672087735935,0.7233713058874003,0.02511638674015937,-0.7957556540672841],[0.7395869232458798,0.024597636002641643,0.11038192145344716,-0.8065749528405126,0.08271091337989045,0.8802936883184005,-0.00042290616741458287,0.48370419826919453,-0.7251692087368877,-0.6046812557614274,-0.8479340450092512,-0.6684694190353178,-0.1549657413927521,-0.8672890050404802,0.8800328033990448,-0.7193775778166598,-0.2442919301256985,0.1420081522900074,0.7997871486039785,-0.9461349047066647,-0.9878710299089817,-0.9751727404022681,-0.5566567699794509,0.6039272443115227,0.40720705121346534,0.5008519666710465,0.005541304728476915,0.9329388556048643,-0.9582498439791372],[-0.14185757648482222,-0.49072373470796826,0.20481891277286834,-0.2219829780805047,0.0700769548822151,-0.07436163478862912,0.22261695731246478,0.8558232828449113,0.7225104102030488,-0.9239480398893847,0.24341089059998344,-0.12290324395649299,0.7307007855251122,-0.9504806334949218,0.547263605481787,0.93207638574096,-0.13782347780916981,0.9202059340316626,-0.421209145072742,-0.00807209394452535,-0.5408323651300029,0.790391825485893,0.1610110622608074,0.5640059882024284,-0.9119321765561141,0.7073153291586483,0.2811682377923588,-0.9600332987334896,0.9149735689805976],[-0.22518103034415038,-0.516254806738699,0.5129659555013122,-0.7442220560678818,0.6070084589340301,0.31922234513673664,-0.28565646426199076,-0.6878618082915904,0.7373038531000116,0.5967327039340524,0.28425217989133067,0.03669117016314316,-0.35801184811174425,0.1779988670157282,0.15562270888961272,-0.7265437539617678,-0.8872296452859487,-0.4117844730645348,-0.25693299477212395,-0.10883070106815218,-0.42739024461065633,0.06970281312222282,-0.8064956829729599,0.48795546734763384,0.11039833666040799,0.5528204104817429,0.3149159021906327,0.7581033495223535,-0.5092496100928532],[0.5905323890321619,-0.5223710510003803,0.7106115538636966,-0.4233954202227528,-0.48133031568545803,0.1662101598437744,0.9163977074611973,0.9966265711566664,0.5512893785696562,0.622871549022249,-0.27263935818168306,0.5254695348631739,0.4449554607775734,-0.4060030314813239,-0.4794021125975605,0.4971406625105037,-0.8977097885230583,-0.3282826491952442,0.7632794173702722,-0.48831100768339253,0.412696488644011,-0.6528517404867866,0.5049939396712062,-0.8290720432731546,-0.3452554144264872,0.5478431660984813,-0.9240381356184222,-0.7751694782506284,-0.7432727887510675],[0.0890161930898461,-0.558968107059705,0.4922151316928929,0.2688043361167489,-0.7412199974772995,0.7986700625063603,0.1236328343988573,-0.3945670400214194,0.47631522408129534,0.146095065240049,0.6917285686881214,0.5938543308931905,-0.2847035272069405,0.27774893103640186,0.934280989701507,-0.5089500816437933,-0.06147705573951656,-0.43061484928236093,0.48691732114723196,-0.8374922230826705,-0.6711810449562663,0.0475640709680365,-0.6983644607126809,0.5252528642367582,0.12489197523209894,-0.6733882586554525,-0.1448280140435083,-0.6800652162709135,-0.888670580284473],[0.3200291809388258,-0.6914280205215986,-0.7677675069460241,0.5872816426019785,-0.8881297366756351,-0.5795424094864032,0.3328527168501443,0.11098696742952563,0.663208386334019,0.36310468179743594,-0.38111976350018595,0.8553326957858922,0.34233544298483376,0.224096754502928,-0.9941495443667177,0.7813367800654105,-0.9747045905236094,-0.8170168772871997,-0.8895840714766383,-0.5231311439201778,-0.639759517813975,-0.9330125356814662,-0.0695159280537414,-0.782649071740013,0.31665098641896017,0.5947023814600041,-0.4966414246391444,0.3468257890985784,0.8638802951120055],[0.4570118318683827,-0.7989576761293622,0.0030050954894911275,-0.8969786585411268,-0.722072284513422,0.33182316355321007,0.7706273846987153,0.4472559236659883,-0.31060336178157555,0.4247523625162861,0.686275609548491,0.9397224881878494,-0.9547033046693134,0.38128398468243585,0.3689598340859619,-0.46366257889226437,0.9517538630395603,-0.21154354730003044,-0.2828286481959501,-0.9756977023724114,0.9848119844936332,0.6511475424314501,0.12194402337240029,0.9522305640637763,-0.49054024306595423,-0.22135416641930816,0.6902644421675459,-0.8255912306503159,0.771402725879879],[0.27131641614701585,-0.41990766317593065,0.24516401779475894,0.05813639802040327,-0.8414290368563067,0.9679162566194781,0.20336060838340475,-0.7926733634844438,-0.37071780406419674,0.6402765653278286,-0.14689120665001898,-0.6774165105901819,0.3871229234574871,0.13128489571557878,-0.7667718179645377,-0.0996146823357631,-0.07447338719470076,-0.4205176419848673,-0.7126647510755246,0.23002964633762613,-0.5869799872770405,-0.7901348546607976,-0.46861327047600754,0.6410155970497806,-0.4896433858578775,0.12253971474093528,0.3650807377944001,0.4293584787054401,-0.2551965053581655],[-0.5628301176874675,0.32456014081439477,0.2875705980923253,0.7866161929368729,-0.21344686380310973,-0.7743843294321686,0.5814574973045628,-0.6052734685079069,-0.1276891833075986,0.3905663624910458,-0.09224635905653877,0.9394226506113679,0.08610938962108539,0.18220586356367785,0.4542639745397212,-0.12852022013667352,0.1591483785445138,0.2320554730998492,-0.988944542251091,-0.33970993827005413,0.09896595036423617,-0.1928070357380478,-0.8525296020949473,-0.8218688334223136,-0.9553771186682205,0.09624603895974237,-0.3017287793464156,0.4049727253569231,-0.31400701258683417],[-0.447263874785538,-0.03882216598914656,-0.2931574772037946,-0.47767071687416274,-0.5946199489259769,-0.9737969563715623,-0.779780024030251,-0.08234346156013306,-0.10949672788432374,0.5008229852715405,-0.49481453595725977,-0.5061648123279248,0.4919997702636858,0.11257871661765528,0.9757223672783353,0.9313991431771695,-0.024257629522673962,0.6539698110205638,0.9517789991920032,-0.20735718346379217,0.5343880245420243,0.09734070486947766,-0.12606825133666266,-0.7120560693036371,-0.49137859282845553,-0.7389446750390201,0.3164688460465537,-0.8594973764569702,-0.5795230884313483],[-0.9114269332636542,-0.883070731711934,0.30448263280133014,0.6394028137529331,0.27278189292994415,-0.8683824926312618,-0.6003463894216952,-0.3184126682663686,-0.20939768678018722,-0.5299194744736555,-0.5287744777201446,0.2498834568552284,-0.1355632243707805,-0.18425967167203505,0.222554746146034,-0.3436496534244562,0.056114496568907546,0.8791789730799027,-0.40801655640527734,-0.9049357091926133,-0.7198183010185439,-0.6524106111889005,-0.2697476856391303,-0.7963001207026501,0.33765008668921537,0.3434560897019283,-0.5404530907291045,-0.7587448640738299,-0.752252719605794],[0.14971778951442505,-0.7457217184764897,-0.003901958256206761,-0.3196055568751448,-0.635503599664532,0.7690396343250729,-0.22693131712637094,0.03558143353421017,0.8324729762876251,0.6437193315244218,0.2296501406546967,0.20645658494614394,0.5243863239555893,0.5342809173632808,-0.597112951948015,0.9174314485324644,-0.6865375803708009,-0.5441541355336781,-0.7800345761714542,-0.04919799562060101,-0.28095259261648287,-0.6427788314745444,-0.9753504620978835,-0.5881604447328015,-0.20068079678938577,0.014104035147467497,0.776395724765117,-0.5947303503788728,0.09069940924540298],[-0.8255606342292718,-0.9687020771261501,-0.34027469055770676,-0.6237080118106761,-0.9546163679137842,0.17140691189176538,0.37263863390994767,0.33700807494043405,-0.25574154724150255,0.6933495274389989,0.5557708465116769,-0.5182023939068947,-0.2060827684163391,0.8784508386411498,0.5555577631146196,-0.014206632120622231,0.2850928573137952,-0.5791006073986862,0.2647272856115821,0.15592130086325096,-0.7654050415211384,-0.6313080934623085,-0.7017436550624008,0.11316252299973417,-0.3379010542846601,0.5345347492255559,0.9500647132288558,-0.9644676728319808,0.08917028726348564],[-0.3968509879323878,0.7412307361353885,-0.84263029020194,0.120194882057574,-0.8749386657328326,-0.7378520716592574,-0.8939118116895781,0.14520010493726865,-0.853760950246272,0.462185600631569,-0.566389495484261,-0.8858047131699278,0.1949600031881964,-0.3297804201175989,-0.15374634205469562,-0.6726940597888942,-0.966589533093271,-0.4504112071785904,0.2044924311073748,0.9899138916941688,0.2367509988160934,0.8270748447175258,0.9433386838922777,-0.6442640857505828,0.6208151917055846,-0.145713115337873,0.28129413410226434,0.7188771810262415,-0.5241581547335168],[0.07412545297031992,0.5498202495805153,0.8224946081449747,0.013164721704958637,0.7112384311101176,-0.1984662195809448,-0.03409042774213211,0.37151503183701573,0.5512907402235896,0.18376510350069486,-0.2662193185436441,-0.9870991015285722,0.0288140884048409,-0.9522883996232083,0.25337224689247684,-0.2310460297108503,-0.18618823761108727,0.03956360663488212,0.6116454348138582,0.5799182915552781,-0.7820274818410722,-0.6642115443621706,0.6742021408225427,-0.8857046074093649,-0.9608564299634741,0.8060262086687746,0.9152188939956529,0.4879629782540993,0.6674413248517235],[0.15682241959587717,0.9972847627059358,-0.729546322591849,-0.4850058040025498,-0.4465513667373957,-0.045340859855014015,-0.5300538620878867,-0.7203306900157063,0.5779678669216157,-0.3773865100044054,-0.39627169379293936,0.30254125827192957,-0.6622912060158952,0.49869511399835664,0.13319755518612575,0.6612490123611487,-0.5297082713818837,0.6674959022003837,-0.4148176155451073,0.37670499904672483,0.10244732782237231,0.5981909730222228,0.9584804674973995,-0.34662202277093246,0.12956676829654024,-0.629334630328237,-0.6184080727999444,-0.8339190475439626,0.7525574930808325],[0.10982130465897222,0.8384328992644621,0.9185548094870419,-0.8586198765261011,0.41024022345958744,-0.9399030807842197,0.8236203144773662,-0.13707177218252897,0.12469302641245239,0.6349361380499057,0.28137236072319194,-0.7681316642523011,0.30664194550833757,0.9035839678001978,0.6758516826003902,0.4887116713317603,-0.9659693514644858,-0.9903350722358246,-0.13738713202032415,-0.9597366715783247,0.5118634107777518,0.09263128133083742,-0.9869677384427324,0.2831094540970567,0.8033412873021097,0.23665481386572162,-0.8989396692235361,-0.7119734789994039,0.40102723480663127],[-0.5803013849983205,-0.9761271501832087,-0.17895112501629717,0.3844868336288014,-0.821746486054838,-0.7192658497147075,0.9947336919233174,-0.482955561541744,0.6634580010008064,0.2893022254404589,0.012599375629475107,0.5449709536945291,-0.38368650474651345,-0.5950629803602026,0.13931384210008413,0.335354300084467,0.42848979901934525,0.5749150709893733,0.7116081329268653,-0.8585882754730039,-0.6451204884970352,0.23597615908675862,0.6621556748388555,-0.8619893827694374,0.4411784587418677,0.24204860797780992,-0.18310239988018195,-0.36494985024773263,0.3895957453924084],[-0.13643628680612574,-0.14335025374580446,0.44446859486190005,0.0035699369420214566,0.9549783807412071,-0.8337096234926014,-0.13147932954145714,-0.6992662396691536,-0.5642239349982778,0.41251515994551124,0.23749654769553197,-0.17656134366818588,0.024950061558158154,0.14236780915274805,-0.3107733203113343,0.1734869687978935,0.5249963274045024,-0.35178809294652025,-0.556183111234466,0.047544008186863884,0.38180229065486326,-0.5067086182260427,0.40553888823797846,-0.4563419195562277,0.7580104757492501,0.5488993960734199,0.9848645062188652,0.9895777237955152,-0.09586202643003716],[0.17448927909407486,0.47132124728024016,0.2789582807607722,-0.23354780975502476,0.5299058282161524,-0.1606368174833439,-0.977977004070846,0.20900446804736128,0.6436068282078884,-0.9388145986585035,-0.665727620129466,-0.9896721214372248,0.6412387695470232,0.4744074053794427,-0.4045130747986212,0.14168106013836512,0.007212673697112981,-0.8434110036515094,0.6615671102160285,0.5106572406465542,0.13040162978373715,-0.3557892325316603,-0.673696240232097,0.10118531764097893,-0.9671568782901871,-0.8046821793346384,0.43081735290883705,0.1440767996108212,-0.5689545455033334],[-0.07261442564603149,0.5491482750741408,0.7516960491373899,-0.7821434774291138,0.35272219672122707,0.29633401092601885,0.7840789628463622,0.5829713117033919,0.5367651705167211,0.6763597026995765,-0.8398288949745412,-0.6348397417068703,0.9868482179300802,-0.5330680239390293,-0.5291672637889562,0.15625739584741427,-0.6677045147463427,0.34802424021355627,0.07594670425069494,-0.18747357266128395,-0.7236213762157002,-0.8249811309435828,0.2317932634895572,-0.5462126313617959,-0.24266576999353262,-0.7749769739035453,0.9790828004166128,0.06080233738658558,-0.6862526620415481],[0.5960350445258071,0.8039366311501581,-0.18931956950317197,0.826387185556722,0.38249165542318786,0.04103918076369273,-0.3848010671507196,-0.27660835577318243,-0.8635242637218723,0.7307186396681391,0.29695229419295344,0.5511300611795744,-0.7835740721995816,0.5036955751380789,0.824901586295872,-0.43214502741757,0.6167526867992152,0.5519144098266977,-0.4657932243948919,-0.30126117363660354,0.3025755866282871,-0.7194771767225601,-0.7650082878677114,0.030185859391824277,0.5117366414336868,-0.7093465293557115,0.6274560527051083,0.0334958969787813,-0.03512286477945037],[-0.3445012324951304,-0.20281690423057475,-0.46283051779328743,-0.6309890057042804,-0.8810927999519718,0.5168662785108837,0.09609829536695313,-0.691086076949716,0.34337968764429205,0.2019197439142859,0.2843509903202772,0.8167949967047716,-0.41571213182401023,0.147554796847569,-0.5184169442755961,-0.6878162218386301,-0.3101360653094982,-0.08229982375869982,-0.6177450855945528,-0.5066093085523953,0.6698367805549654,-0.2727802527846255,0.06613253081215165,0.4905616174976466,-0.5512690786040324,0.5774813881733072,0.996647695740873,-0.1557702238738483,-0.18257827707470686],[0.3400246205150368,-0.5718940734819229,-0.6723381402885489,0.6925299518372738,-0.029379349345249217,0.26758254159592965,0.4108707634320732,-0.13019826961577263,-0.7544879648250102,-0.2773653718293252,0.6966305007835829,-0.7166109640251939,0.8496565673342535,0.4166729995303935,0.2086226185367217,-0.8271212980916423,-0.37815029299376146,-0.4844736242930281,0.3822589881558476,0.8026399707816163,-0.643645282488148,0.9963248925208514,-0.15766255673066976,-0.9373936579575757,0.7225170136220835,-0.1431173968804904,-0.8223057177033075,-0.5499441240272056,-0.414083866763969],[-0.39225447929236745,-0.32789696660448353,0.67928673794148,0.31438600947959916,-0.8291770525041904,0.8383831062246545,-0.6865133442868812,0.06828440316591688,-0.037610198138696704,-0.1036958979798166,0.6757929561829725,-0.2755983434793585,0.785739425107868,0.35416025544460417,0.6288179320937646,0.8753746517209411,-0.25154318013115917,0.10423625840781159,0.7360457965164524,-0.048023302054906125,-0.2075347039304738,-0.2737731661597582,-0.240629881976139,0.9324583241118929,-0.7059392168633558,-0.7896735745870487,0.9241264281400272,0.6900566060860402,0.005573073287169494],[-0.7792429188240955,0.07621670636544153,-0.7125442140420242,-0.9404874146024285,0.5361717357851337,0.029714159805963813,-0.1946000790447231,0.40592230194968093,0.00017985869857994885,-0.9740039190599872,-0.4975096269289725,-0.005429477805248828,0.9083013978012446,0.9676682400174887,0.33325623594953835,-0.09516219118450464,0.8581968275021343,-0.08405908494887004,0.03871030985153556,0.6858341854710615,0.5053508196470344,-0.4358327629436545,-0.17045861628774528,0.005037181171792859,-0.5566327887790032,-0.32902655946439685,-0.0345434045266082,0.9533343399354139,-0.7304933590222003],[-0.3146859647314455,0.45003929400039744,0.008043712828731397,-0.9801632988945037,-0.005015399615963645,0.465347045748,0.8798739202838048,0.9312955524282478,-0.3430907131291592,-0.01978155581136254,0.14288037400825426,-0.08171635652265596,0.4253877052059418,-0.20908792032185564,0.9007220566811625,-0.1858213154313504,0.17179358358111552,-0.6846024394222272,-0.5923968814401668,0.6608644492639828,-0.545879987180935,0.09160554038813018,0.7372686324155402,-0.5666736130880308,0.9737779997726137,-0.266030116909991,0.6142502179468319,-0.022416532710989978,-0.7332702100573947],[0.9023686492152299,-0.4690258471850093,-0.5338092123817297,-0.9353789262404764,-0.5873715478202002,-0.02995544432665298,0.15731921372551727,-0.2170311829823277,-0.6825480025435136,-0.7629760647166888,-0.26728458906453123,0.42555204022048754,-0.18678299024309264,-0.30528445110562297,0.48044919077973924,-0.6764909198545952,0.16944675827317668,0.43770919509935435,0.4324633826229327,0.7715381904793177,-0.2503838508390983,-0.3878536568244373,0.5721697875676062,-0.9531889592632754,0.3014584156639033,-0.25427993330100396,0.26382630421235587,-0.909325947088315,0.3817839640925813],[-0.8262941634786765,-0.9530543078210707,-0.8000185243982405,-0.4009127385300113,0.14407010254376806,-0.029595105260078913,0.7896339878807084,0.2030993176317717,0.05944042130458316,0.5905907983432872,0.6103841144660247,0.5178249807327298,-0.7608658827454595,0.0007413280416082024,0.053145617058019345,-0.286643431852851,0.20583209028809746,-0.6924424359299919,-0.3816211862394465,-0.8065774512471875,-0.5913927005479322,-0.16006181160637234,0.5520511272738224,0.7103193591491823,-0.6957081707431663,0.44020535809830985,-0.4515900809981279,-0.8748427124478049,0.3526802548437127],[-0.6170664329793247,-0.5168387768184313,-0.7710275309843964,0.23509605847394432,0.2859618644959223,0.2129761251068898,-0.6568453832203072,0.277631698801484,0.8169430928617805,0.079263266003057,0.6477652504120033,0.14699116928170808,0.5017801745719368,0.12990430984890766,0.6477703056031274,-0.8666922419040595,0.7556850174497212,0.5099266138649272,-0.05994690452718454,0.004357629549202047,0.4258741919829869,-0.7842202217307135,0.757419941472564,-0.7599246896422052,-0.5926812984676288,-0.7480243531820272,0.5161360853482126,0.3994452554513561,0.9328442458331321],[-0.1776982204707711,-0.8872837739850536,0.04554355300640767,0.2056445569842038,0.0471239505406591,-0.3763107978098852,0.7197174088845639,-0.27897268475143333,0.142623337199979,0.46436824473419436,-0.9765901055438786,0.5716015882529104,0.3817827479579041,0.18877712055245444,-0.9628727112783544,0.7977792835772652,0.5452742027473443,-0.6591503919517125,0.11505789517220855,0.8195171606084621,0.01090331569529246,-0.6989421716823159,-0.09935192699485618,-0.7537488703564177,-0.07561937752058201,0.2859422375136802,-0.6205520591312572,0.1880433331162641,0.5677930885020073],[0.12601759794993073,0.9509745794763069,-0.2651455934641558,0.837824143631503,0.8883847412882029,-0.512819793129629,0.7869682299349037,0.8451456033322065,-0.04808468268950472,-0.37365745243829496,-0.3619618113827594,0.9278849630775299,-0.7728537580517092,0.2926699654101319,0.5962363386553822,0.006398646128959795,-0.6498768220809086,0.8615950634923959,-0.27435888263977537,-0.4678045690232471,-0.13334915789089874,-0.5393314480622549,-0.4759548939251901,-0.3143818777664764,0.9688433058921624,0.2871654159831789,-0.30886299804733786,-0.9902813602867548,-0.45344393756380685],[0.42634985370462886,-0.18729416288803713,-0.1480681014706533,-0.257708874053606,0.6960167300879447,-0.4531697841794875,-0.4426318644343197,-0.3131510594035618,-0.7211413217300424,0.5632426632883891,-0.9488466249561316,-0.37298161119752726,-0.529023263403207,-0.6054146999458239,0.2070673411009849,0.09413326274276002,0.31093118112304907,0.6812084681160344,0.7940802860117193,0.6875501522101073,0.09937995302706759,-0.21381189085060837,-0.6380963120105065,-0.756343178069895,0.16874276622539242,-0.14080345304419373,-0.5073427101228831,0.370859945692644,0.4293654381629215],[0.45957459560817027,-0.8117876950415233,-0.9250642217303795,-0.9714066241387742,0.2617342766744035,0.02234682448518477,-0.8085592939854243,0.07541667497529536,0.8864729327385761,0.6230116493828206,-0.3976079480233363,-0.965971919448537,0.05383441375627074,0.7366551928013743,-0.7281127187905152,-0.7911686302621452,-0.457816539368586,0.15757842529824218,0.8017360631717629,-0.9635336431468116,-0.44105505275321066,-0.6348903919298605,0.27407116584383573,-0.11385302129969332,-0.8732483420409023,0.8840441924027029,-0.35260804042201555,0.5127342445323504,0.9601659821997965],[-0.5480913355125758,0.7406081281422279,-0.4475540848515811,0.3847696368510647,-0.6680178417322999,0.4339626338220055,0.7388376187614141,0.2580352560310992,0.9781968457277532,0.7348644681611542,-0.08874101558899783,-0.08128158570986255,0.9599888663793088,0.47762574936640956,0.14634676972784733,0.9143168065712444,0.9173049904983008,-0.45562938288069876,-0.9710051374511781,0.3071360693760501,0.21714177726698147,-0.5185750000907867,0.4302611768102498,-0.26245374118404974,0.20714948368021036,0.3740485609892714,-0.021614711161792144,0.5233943148549012,0.4346388697171435],[0.1748528505345499,0.757468581828507,0.5442712254657383,-0.3666061734842021,-0.29168918470906924,0.409481965566612,-0.7483367285720386,-0.8411883510735392,0.18694625955955013,-0.9864823794443871,0.3019463458283471,-0.3461936597337727,0.8422629996509121,0.6489195350140475,-0.9873600635572557,0.8235561743470416,0.00038860044413602246,0.6548846244734852,0.5241172827971203,0.07675519460079827,-0.9101273684281515,0.8922940119684188,-0.9308342602141224,0.24698350028578275,0.5376905975378272,0.3328108031833812,0.5504890238562261,0.8681789693125808,0.8469061588285371],[0.25739615702280716,-0.07886992913899449,-0.40408412177708364,-0.20525140531929575,0.6641100189762021,0.8304745551931048,-0.19033090310301803,0.1980608706158964,0.12970271408807732,0.21031854460599186,0.34632530637907033,-0.21474710376327444,-0.6776124417421061,-0.44871582446420466,-0.9461712668943998,0.35354701442184133,-0.6482889278835073,0.27785573534051955,0.5573084307556555,0.8116891470584877,0.5870377955767097,-0.6862801410404475,0.60864030643986,0.8421728696051414,-0.9395162762085361,0.6685926562754883,-0.3854477469945954,-0.7119530715486251,0.3509900401172503],[-0.7754312251635174,0.49905989791597993,0.21828349254279567,-0.9527968495965873,-0.10748820150860516,-0.15762245534115982,0.23814933595438537,0.07693505148213364,0.19921391447432923,0.034624099645776285,-0.5925344010288565,0.2246543971310886,0.3026691158099848,-0.19858487468186925,-0.1376027879995556,-0.7478159539361076,-0.7342645047710086,0.08842512376326028,-0.5752283750370275,-0.4745902953070018,-0.6499944096732122,-0.6650097067475058,-0.7015657027529623,-0.9035140102362269,0.7377701062416038,0.7232436913485971,-0.5375289416647726,-0.9376077337106301,-0.384969181823166],[-0.8611285920593925,0.9780063158378951,-0.6804152364444944,-0.3110142253745709,-0.8375210529323187,-0.6192666272596627,-0.4830377373989321,0.2782628614522471,-0.7559619801199584,-0.067295230772219,-0.8148856261240363,0.6532303426924513,-0.7494600318435942,-0.7307997930500585,-0.2605804276762014,-0.18582058779324617,0.9663642833461674,0.9826176894729393,-0.9689156527259293,-0.2762128318779349,0.03977235732685136,0.3395456713658247,-0.8187752553793688,0.23526446544325164,0.6549734660656417,0.05697311268290606,0.5702623657125832,0.38320791572626733,-0.20339707055052259],[-0.1919629592385259,-0.27861973903895776,-0.24970132656031674,0.7066009487447082,-0.5191963742049333,0.6269376203149875,-0.979417617510894,-0.4344899635552817,0.6202883101888701,0.17397011974579923,0.5863022979368679,0.48919790474856684,0.5178162170575462,0.4036416573795951,0.15347167926043004,0.4054124376213184,-0.20886496877320604,0.39531769559769003,-0.36565995978068533,0.47972599731706467,-0.592979674196427,-0.34799305408786374,-0.02691112497741166,-0.17685618097703149,-0.03604414148641122,0.046319831581020265,-0.23492435316835603,-0.29035207342830627,-0.9910529474331198],[-0.3743136967101459,-0.25651699504952363,-0.7344834813529211,0.3911001447316358,0.4594638646660236,-0.756420517352304,-0.40593698999485617,0.4231202168621635,0.24489152877511455,-0.45164107208389637,-0.2520964719840739,0.6777912857256772,-0.27518575470997053,-0.06557829216428468,-0.41806693902611425,0.46558456933663717,0.4116044651873465,-0.5136474337869588,-0.8367702779532269,-0.674513481605514,-0.7761794885883835,0.20085846188787237,0.8847347811221569,0.21514330730143327,0.7255172103475294,0.2355846343148349,0.5078827740579341,0.9851849493454408,0.7694789137071183],[0.7585998816422888,0.5891165425381928,0.11666569553177064,-0.4886148125139327,-0.9378253349422432,-0.7474545248197797,-0.18534143958137084,-0.10661074568486817,-0.15719890624758603,-0.027097249493483133,0.7357777679643882,-0.5415694285220696,0.7250712780525539,-0.025162274381356875,0.35258624032181185,-0.7564841400977356,0.4804939068980032,0.7822883690962503,-0.3901010555897415,-0.7895168330029168,-0.33332934481574017,0.7579463176742371,0.3477423805242046,-0.5317936293117245,0.9452446887867509,-0.4025653765118111,-0.3265211700823072,-0.6592509961324344,-0.25374600995298136],[-0.16303119046155734,-0.6345301558962531,0.8217260413779897,-0.7146581937219905,0.280401435425895,0.868214097716971,0.39348959658851923,0.7390596910865999,0.21938775139777822,0.9175187488249072,-0.8254909099881502,-0.2095615545948153,0.8557175634986838,-0.4790569874804895,-0.9438343003754808,0.03139948751362365,-0.7529130209637853,-0.28695812102486196,-0.7662121268394981,0.5873081316386641,0.7018497487975623,-0.1933814014323274,-0.4355267500789908,0.4131814496600239,-0.23769318475922785,-0.7634768806483927,-0.8115025094184589,-0.33657484392459813,-0.22156818130900735],[0.6630637431073012,-0.28960038964900603,-0.19891448754008678,0.3869847042355665,-0.02950503178842223,-0.8587942614766331,0.1208603721311221,0.0945132718770274,0.17907100080130767,0.36499370655229146,0.33160714268081626,-0.15933479728269617,0.37395481897104976,0.5066698048571243,0.2188755568911891,-0.2239112657349085,-0.5666079967100603,-0.2713836825862759,0.48374350796393295,-0.2526744763099755,0.30211271233314685,-0.3690628413494217,0.19595031392258333,-0.8471276804876933,-0.05805430902656883,0.3765193485691243,-0.27066598613362336,-0.8353006124421651,0.9362097637778966],[0.5325120631215818,0.7113489841187146,0.843512653582001,0.46849967242251767,-0.9949276048023175,-0.3595023102462007,-0.13340353916274106,0.7399735955016966,0.4486989583500569,-0.642618454422101,-0.0015342594976903712,-0.6705256612806765,0.1686561820543946,0.3530446768067841,-0.8654586428458091,-0.8721050797334564,0.9969160926813987,0.7328458860536664,-0.5104537574199834,0.24685766901280481,0.7317028622726183,-0.36885491535894466,-0.27226687693147555,-0.6517300342135368,-0.37165163615421437,-0.9010195832547987,-0.2702370133648473,-0.1560043695094211,0.9026970576450315],[0.1278794984019611,-0.410760838084268,-0.5469877875217228,0.18896936273395304,0.04853355661684189,0.05989049000187352,0.7775010634905277,0.4734652038119762,-0.9541189885147947,0.9445647413182889,0.7951197291160998,0.9201028826763353,-0.16366528465278107,-0.8589571421426938,0.8728530631332383,-0.6088871904898256,-0.47109070650925067,-0.09175608202655039,-0.165011635012134,-0.3554952710694732,0.9059849345681059,-0.20575887400685633,-0.6180601888737087,-0.9932961384868753,0.43287421364950096,0.7809531246093435,-0.5180306102731644,0.19678074040037163,0.07959996358926813],[-0.2647266166951112,0.13158917016515814,0.13111590752944258,-0.25510531283167714,0.14158900254005968,-0.5992815385753516,0.0840419858185133,0.9958801326066269,0.4540594381503602,0.47614323281189774,0.7362367215747465,0.6263740677282681,-0.5161189088973483,0.9749367111064853,-0.556657692313435,-0.3807823984937688,-0.08651925964317253,0.9354377622872478,-0.014974965389274786,-0.7534330799909907,-0.02895617226458036,-0.024501982453591253,-0.31687401426156114,0.9796145375269529,-0.6737699618964494,-0.8759765595469471,-0.3424839182546098,-0.7502483236263426,-0.5546700631272632],[-0.5432467595470323,-0.8379052118748718,-0.9292227121919434,-0.2770359275773069,-0.8352936268250923,-0.44718439570575197,-0.7808904180751455,-0.5813019509714459,0.25172184548405374,0.03776206051616193,-0.6813997423851474,0.404526311327164,-0.047402380426547985,-0.8207143331839695,0.18813559935344548,0.46674069486180425,-0.928772181269246,0.18858567027414042,0.8590251625657872,0.08515169820198931,0.9224746621461799,-0.9987159192803055,0.577878816827905,-0.5535942707954278,0.26025862778302833,-0.1526155548623105,-0.13768101668233235,-0.7096047862028292,0.1177433311328222]], -"frames": {"left_hand_palm_link":[[-0.3264431069612137,0.022616453210745867,0.19544181251721537,-0.9342815572921072,-0.33145352348003954,0.13136412553091553,0.16857941549846048,-0.7353327437996642,-0.6564044001728316,0.3141638941285784,-0.5911212377073963,0.7428840622581987],[0.3239743849906188,0.12737751983042991,0.4474247417967935,-0.1315427765322783,0.28093250163482775,0.9506699887276219,0.8365612811424443,-0.483055142513034,0.25850135818796943,0.5318474601194325,0.829297690107063,-0.1714748388178235],[0.052593664509538715,0.24940299363213103,0.4268743233119437,0.492480674441587,0.1789940249520473,-0.85171821885705,-0.8393237212264284,0.35653863582572454,-0.4103850535170011,0.23021397935003934,0.9169740128242445,0.3258223189359849],[0.23385653981838042,0.15253880397746672,0.5914062788560563,-0.9127049315599113,-0.4070269603110122,0.03603833356573602,-0.0012036427713149611,-0.08551698364830222,-0.9963359858761376,0.40861749736189296,-0.9094041450794436,0.07756185764942508],[0.07416434726335776,0.10280666780347918,0.14676550443235553,-0.49354800170688506,0.42817381145691763,-0.7570188618479706,-0.7769826027526693,0.17402183316736608,0.6049912698549932,0.3907792279930299,0.8867827178985421,0.2467954744432041],[-0.269617005599707,-0.4704405521913664,0.29325666189232463,-0.8110912221721694,-0.4937376738706507,-0.3136146340910382,-0.5583429171378473,0.4937615156269245,0.6666728977300647,-0.174310688706274,0.7158370450644487,-0.6761605643012191],[-0.09435580594976295,0.26814517555628636,0.3519261252871695,-0.9622475597069848,-0.24361009496745437,0.12138268191097235,-0.001178641606424613,0.4496971574381893,0.8931803162833225,-0.27217318869042906,0.8593175130427404,-0.4330071213457392],[-0.17931375427398055,0.13686149109956752,0.09546828465685062,0.1718151957051529,-0.6568599495448693,0.7341760995896774,0.5093658964005358,0.697140761170274,0.504520705917021,-0.843223530333967,0.2872799432589102,0.4543614333240429],[0.32804503067081886,0.13159431141580158,-0.155790418583785,0.5118270298731371,0.8589035157931172,-0.017828125236966794,0.7488174177516079,-0.45620536956094715,-0.48077971635206657,-0.42107667515911673,0.23272604354042975,-0.8766601521085358],[-0.24103041396568636,-0.1336159366351214,0.3497283860093159,0.5232506275493441,0.695294742645337,0.4927210180406074,-0.49420778159871837,0.7186162692876803,-0.48923340658864434,-0.694238755311522,0.012485125742700779,0.7196364861919508],[-0.2717876856169207,-0.38377759076846535,0.4210917246046701,-0.7868361492717398,-0.6159141882812107,-0.039224824704745295,-0.5681522673757535,0.7477089900847242,-0.3437066586818942,0.2410225617551415,-0.24815515070290026,-0.9382575051149925],[-0.3212553240980648,-0.0005386444147704433,0.037787224012816215,-0.15501562433881697,-0.4626903294308317,0.8728618534808559,0.2884633492561076,0.8238385185608758,0.4879333883547736,-0.9448592764781409,0.3274259525245509,0.005761359786949333],[0.2684334019180088,0.17662528382420675,0.028840310970021463,-0.6137836723820854,-0.7192978824897791,-0.3253923166930661,0.45291207794731225,-0.6584126153420903,0.6011351575210326,-0.6466376521411421,0.2215928342796962,0.7299016116089088],[-0.2011207733272566,0.0031091752267794065,0.28704739203102636,0.5406055936582371,-0.2265268682403514,0.8102044001797593,0.604463937453694,-0.565239215493354,-0.561362607933976,0.5851230130693902,0.7932151077946489,-0.16864415893508378],[0.047371691013207366,0.23848891717430756,0.26778535161955547,-0.3536669010750174,0.0665434365642909,0.9330014438007095,0.8954198678803194,-0.2643178067140605,0.3582727414401774,0.2704495947291944,0.962137239744977,0.03389614442280736],[-0.12458732848217532,-0.13896808685802386,0.08087655164747626,0.5940394649149736,-0.7169138744444999,0.3648994529353363,0.1676772661435698,0.5539958965862515,0.8154586936100525,-0.7867664510781777,-0.4232293033268977,0.449305584500436],[0.26810193613369365,0.28181935099259814,0.183982952385189,0.27150537263661567,-0.815909576405042,-0.5104668409994888,0.9100958598402471,0.04512443598018129,0.41193362472504225,-0.3130660609697159,-0.5764159508618887,0.7548074542960546],[0.03891801125954658,-0.038921216781257775,0.0642461359995515,0.4180433463658171,0.8566993843353979,-0.3021687036054863,-0.7098631652430867,0.10050441881280914,-0.6971320882230035,-0.566863340841808,0.5059298633985249,0.6501545401911873],[-0.19476214830494204,0.22971452776889617,0.18423979887548186,-0.04203931229624419,-0.9991123203894773,-0.0026958241065864197,0.4195031648406252,-0.020100011717849614,0.90753131307831,-0.906779902131911,0.03702108554455269,0.41997577110441114],[-0.1235134058682936,0.09656562066541585,0.4050353521354245,-0.7777406877201377,0.6276671064908197,-0.03396212734870108,0.512035895612838,0.6639497486521627,0.5449678640696978,0.3646075482879726,0.406453853093032,-0.8377688231470911],[0.04285664305196571,0.06782929031971141,0.31449624550602634,0.16284578463176497,-0.5288355205730055,0.8329551264071452,0.7112499193201002,0.648025021229379,0.2723731340051984,-0.6838163515848505,0.5480844497346565,0.48166236438426785],[-0.14055899366938496,0.2390051292488375,0.27158109325011137,-0.9871933540639861,0.012277228922018435,-0.15905518332293797,-0.10929640369720056,-0.7783191502768024,0.6182827803289159,-0.11620489589838984,0.6277488111995053,0.7696933494625322],[0.08497728419291914,-0.09428399711804704,0.36678825322732905,-0.7951961487282653,0.4675392663009301,0.38609599779656145,-0.3754814546077734,-0.8796670125603991,0.2918897467515663,0.4761058309802193,0.0871377155992217,0.8750601443482523],[0.007289705586567705,0.09132917009497013,0.33944343480959926,-0.8395131976970894,-0.5086929480842164,-0.19091640961896314,-0.08888902577564979,-0.21805734157097578,0.9718794868108156,-0.5360189660721557,0.832876029404369,0.13784479552942716],[0.028248815719380805,-0.02574969420416712,0.4227865992897568,0.8614102057326052,-0.46777683124563,-0.19788201941942068,-0.502955971940632,-0.7313252110621309,-0.460650329375964,0.07076544177043573,0.49633483840521136,-0.8652421513289259],[0.07763859942249851,0.1942621191783724,0.1201048250543983,-0.1429417428086878,0.16735463840074288,-0.9754794119655072,-0.8120046669404972,-0.5833446022487551,0.01890756182309805,-0.565876381402924,0.7947965148596202,0.21927704151902017],[-0.2683019433402289,0.0657302404429425,0.2718479454425847,-0.9578179875838402,-0.12249082457779098,-0.25996288303351234,-0.025618629341709553,-0.8646135182853248,0.5017839672896341,-0.2862313548364241,0.4872776024942305,0.8250043330892147],[0.3157848225726633,0.388144112500041,0.40233016118761145,0.9037246945900997,0.3529649519204938,-0.2422755024838333,0.41448984341131634,-0.579746422124193,0.701492876473496,0.1071440436836931,-0.734377170635063,-0.6702315459250979],[-0.04311806741788582,-0.29049892303411723,0.10821317224589086,0.07324363732848732,-0.7828857476779622,-0.6178391988807537,-0.29338701544883156,-0.6090046457521315,0.7369107141427302,-0.9531838378596857,0.1272919774967744,-0.274294228353497],[0.4080556252356998,-0.1207135860571278,0.006350975656929339,-0.4008182690652853,0.5810287325948768,-0.7083433680657283,-0.039714760445780206,-0.7834601843491584,-0.6201716515145764,-0.9152963743980649,-0.2204444407073443,0.3371005718995198],[0.03460076270046977,-0.15881797043915968,0.2305476842540831,0.041078752136129215,0.94983186909192,0.3100515385871972,-0.9751470138989171,-0.029505186839237002,0.21958539394414825,0.21771733373106403,-0.3113661259786162,0.9250136745941138],[-0.5779688443249875,-0.11631527732769877,0.1310515873194861,-0.7799885987328904,-0.5100309670967869,0.36261025695506544,-0.5959641506087131,0.7821641382010616,-0.18178556626263218,-0.19090447096895685,-0.35789338288728834,-0.914039282224542],[-0.2837624899749041,-0.18454884404339228,0.44198713109651405,-0.13771113487113706,-0.9138278703056959,0.3820396141567783,-0.3064231767828389,-0.32748492707784355,-0.8937888225230456,0.9418813513722504,-0.240150465311596,-0.23491971809068596],[-0.11430428600947372,0.05006806421520816,0.61679846730722,0.2693391462559793,0.6357127311241045,0.7234125709308845,0.17707976185445373,-0.7710556809517494,0.6116501408596292,0.9466251540264871,-0.036639600963522964,-0.3202473378586676],[-0.03256902721040919,0.1086474920993935,0.15373651913404404,0.25257619823014177,0.8794897020753231,0.40336475804297445,-0.11668024624863846,0.44152436991967997,-0.8896302326823129,-0.9605159989416965,0.17763472270501818,0.21413762179156193],[-0.07173881276579931,-0.19246301633791676,0.4355898093705777,0.5833270575974889,0.3918531443150055,0.711463742691853,-0.7181984660694148,-0.16028116557592542,0.6771269536040952,0.37936856381386586,-0.9059586421311218,0.18793198646781348],[-0.008104999366212366,-0.0273479382231976,-0.10547704842617822,0.10183733915135529,-0.8275608131895039,0.5520618233746101,-0.3940141151074592,0.4760086371033905,0.7862370218318765,-0.913445145288242,-0.2975884370672018,-0.27759518849079934],[-0.05735479352526206,-0.14093808366981936,0.28030118380128133,-0.8012784749328346,0.45050168954934866,-0.3937017060192822,0.5979648572774908,0.5812733836938547,-0.551868900075196,-0.019769549069745138,-0.6776204550648386,-0.7351460288999051],[-0.26775185614056424,-0.1840320741873366,0.30716176552446844,-0.9195190593008077,0.09599149668429041,0.3811434535012602,-0.38392806463279133,-0.011706153946449971,-0.9232887994268647,-0.08416614978645928,-0.9953133167621601,0.04761786120807493],[0.19617097240879222,-0.2891353394322949,0.1379846966941942,0.8561463143869787,0.485947459005335,0.17569449464297682,-0.47344052780609985,0.8739177901466433,-0.11009887690566647,-0.20704481397097135,0.011079853404534976,0.9782687165887806],[-0.14642432752913165,-0.06061731044983009,0.29664273276477304,-0.2018000746140684,-0.6956483067840659,0.6894564258560646,-0.7534716077412814,0.5600030830692437,0.34449540386006716,-0.6257453685114461,-0.4499666434541615,-0.6371599120825719],[0.05788088760754803,-0.1717644844827866,0.610028208633206,0.4215796736992364,-0.2514072968596258,0.8712433355896511,-0.8927708071506505,-0.2833780579663189,0.35022444541010167,0.15884226334077783,-0.9254681233564848,-0.34391552455189534],[0.12225085392333279,0.0333439810545138,0.6402290281367308,-0.6713066589691159,-0.09909953670492452,-0.734524779329188,0.3501177740789514,0.8310821913023442,-0.4321110222779489,0.6532724652836088,-0.5472491874137075,-0.5232144999685248],[0.36828144350250447,0.2747337973486505,0.35255694594767595,-0.6734961001708198,0.7198440173674349,-0.168010695239944,0.7338258284435764,0.6237740996730309,-0.2690827495143428,-0.08889688725594354,-0.30451677004298455,-0.9483495559121604],[0.018501718883238143,0.08453767717992257,0.1848425792531287,0.9042829020416507,-0.36274825815035117,-0.2251358129752271,0.24252832417971237,0.8704504390427428,-0.428364383604516,0.35135800125471833,0.33276077650345554,0.8751101762493146],[0.04723716266144651,0.31529560279654756,0.369408464585082,-0.9449457179049673,0.055592992759608134,0.32247016818474294,0.05845019329477821,0.9982899860108825,-0.0008238532611240523,-0.32196454015445447,0.018069947050916642,-0.946579268681027],[0.06683634204100625,-0.21406163133877726,0.17840272978188337,0.00907101922962511,-0.1686446533782744,0.9856351746447856,-0.8062157592126598,-0.5843359530560387,-0.09256156633962825,0.5915520824131131,-0.7937949828847577,-0.141264499927254],[0.32481165822105434,0.1115673586443798,0.43883019789768524,0.40909746669003266,0.6875034352789621,-0.5999819074167348,0.9061017167390141,-0.22839866929757452,0.3561091501010728,0.10779119477215632,-0.6893279874941413,-0.7163853599752498],[0.0632760296771944,0.36901970923532973,0.1249061985206636,0.3904366753834523,-0.5381819437156117,-0.7469400230098819,0.3769870473009469,0.8336564934788684,-0.4036057693440858,0.8399047378618782,-0.12400421903711148,0.5283776915977487],[-0.2274883237592537,0.16300828345868007,0.03823125262373968,0.03900666516896305,0.4606291917988614,0.8867351508399361,0.7950687693310896,0.5232191916747136,-0.30676917951059895,-0.6052636880787938,0.7169814677702535,-0.3458228488218643]],"right_hand_palm_link":[[-0.227304502475743,-0.14941140129723063,0.3531319056720133,-0.9384888764910271,-0.181594124949556,0.2937042772695261,-0.3452639992293758,0.4796726936915984,-0.8066640426861628,0.005603529133247576,-0.858450744487097,-0.5128654012026955],[-0.2014017351449956,0.4065631053925768,0.048570079121257444,0.20077106779799717,0.802157573643501,-0.5623470506561079,0.22581130145603268,-0.5964717644464756,-0.7702147040616926,-0.9532576957307947,0.027652509189567243,-0.30089051873652667],[0.28435735040384336,-0.0972109962460612,0.15672768808763132,-0.3964307902215048,0.1115673948539481,-0.9112603058236743,-0.03972208919140491,-0.9937433461631764,-0.10438542803803291,-0.9172048758018156,-0.005184434580212616,0.3983821500060718],[-0.18072575247589223,-0.17183906654523415,0.12262252353342178,0.10728982274499307,0.42067910290400856,-0.900842930990318,-0.9941832137550637,0.03681490906190432,-0.10121462324396463,-0.00941442632357048,0.9064622192075311,0.42218196755232895],[-0.04330827704046296,0.07933220883058678,0.32979110439068166,-0.1666548039115725,0.021979779838209604,-0.9857702904893506,0.034921008019880684,0.999255886490426,0.0163767064182192,0.9853967219004083,-0.03169483542309309,-0.16729834988855202],[-0.1858051908113765,0.0566940068802004,0.12514982216961704,-0.7509071346257886,-0.08181087497455472,-0.6553208801068261,-0.6395256572773805,-0.15747796414709087,0.7524676899987278,-0.16475863817118072,0.9841278735733406,0.06593117323385506],[-0.08459646316900415,0.20094695580207333,0.05923488266305649,-0.5638333965830867,-0.8225745164022903,-0.0739125555174365,0.8101513234449103,-0.533482755962053,-0.24300407859044815,0.16045788861268157,-0.19689416968684217,0.967205227407963],[0.3393659835439661,0.03563847745058035,0.2734452718238689,0.986287823363007,-0.11109313685383188,-0.12204361691558352,-0.06591433679195405,0.412792541878064,-0.9084369089678842,0.15129980069761742,0.9040246856772347,0.39980962719154045],[0.115545159934632,-0.06724709329540984,0.4405827092779545,0.2827416046641287,0.8861356516730531,0.3671795089951599,0.9577031661282359,-0.23944966993215508,-0.15958853704866896,-0.05349608004242934,0.3967712973532171,-0.9163573031393042],[-0.15959721881600802,-0.10853489100749938,0.4284288909622963,0.47879517384872594,0.7956010730022385,0.37117935575251887,0.04399347492122396,-0.44400256670185756,0.8949448558020311,0.8768236742147897,-0.412165808140182,-0.24758754196571936],[0.28524643840633634,-0.08420133532131908,0.1599350903952281,0.7019165474653427,-0.7013504664027012,0.12418004538188465,0.6362565004081645,0.5390500006226567,-0.5519082917632867,0.3201418842992726,0.4664039237671864,0.8246069086612627],[0.17258465276975002,0.41092356191130996,0.5110721904505034,-0.581644884854732,-0.7893583354814211,-0.19647556114775636,0.8118771851084315,-0.5783156889147556,-0.08003999160082975,-0.050444664958099905,-0.20606887722567824,0.9772363857410411],[0.13868367383283908,-0.45119210297975004,0.17604223061346377,-0.1876364195862153,-0.3621432652409409,-0.9130415267037391,-0.9311817010859191,-0.2302095825573292,0.2826732878457274,-0.31255913617681835,0.9032473656421087,-0.29402547993837425],[0.11721879657565225,-0.055303754395378546,-0.09142971493958114,-0.6330437171747381,-0.6334781018285196,-0.4449282488776432,-0.33957663574501873,-0.28926244940975737,0.8949943820033526,-0.6956603773406136,0.717657808335702,-0.031998586423646835],[-0.3018327996989348,-0.0723019309929395,-0.10183278740234589,-0.5516845786507758,-0.02239676195662177,-0.8337520679031474,-0.5521840715391746,-0.7393822248437125,0.3852358715431129,-0.6250894950463778,0.6729133009823928,0.3955386359682754],[-0.2989746590546011,-0.4608230656720298,0.31668340565151465,0.1129035210473342,-0.3929601830878463,-0.912597988953888,-0.29726268936592715,0.8630441731111285,-0.4083988843886147,0.9480968771418092,0.317391004545647,-0.01937167487478897],[0.04685473798882572,0.2356427615178487,0.04331361486379012,0.05812337392377308,-0.09728843286407007,-0.9935575646305403,0.9702344769101322,-0.22886098430240487,0.07916886811781587,-0.23508877731325023,-0.9685853657241509,0.08109041920210619],[-0.20872459180892072,0.005408927831205423,0.11190458349864771,0.1095988065830541,-0.7328641689598574,-0.6714895467915697,0.40878623979186035,-0.5825499385489373,0.7025164619092288,-0.9060253372574093,-0.35149065272266733,0.23573800986729176],[-0.16811425182668455,0.5403606489731484,0.3033987286699926,-0.14630898964549155,0.9635293512477263,0.22406443009333388,0.7800423976892681,-0.026928069123476617,0.6251469722396957,0.6083810790582602,0.26624437717521166,-0.7476539268047018],[0.3545445286895421,0.07238651708222969,0.3890550208317412,-0.2022536708260403,0.9433021150382258,0.26320063146163175,-0.7041278410206473,0.04672336087962401,-0.7085343400624382,-0.6806595596456383,-0.3286305635702838,0.6547553104408426],[0.15408643007651007,-0.0005047709670431694,0.5184157035426156,0.02961453510898618,-0.3152252300516452,-0.9485547077787165,-0.4388283948155981,0.8485282216132198,-0.29568479336866815,0.8980827463067332,0.4250093075037226,-0.11320101289901074],[-0.05498858944701588,0.4928190205773538,0.20203814266331013,-0.330837053068395,-0.9152709435762605,0.22983895266477783,-0.05493272130333108,-0.22446266567048362,-0.9729331466500207,0.9420877031610329,-0.33450801420163717,0.023982243173787547],[-0.18562052627327874,-0.11686797012796912,0.038887325564140006,-0.03945279612540506,0.7541556712898525,-0.6555094967574757,-0.680985347519125,-0.5003710751110224,-0.5346847142536395,-0.7312335012444001,0.425297555428657,0.533309999910244],[-0.1262189049923738,-0.07866993637111336,0.1122518593760751,0.7120084448936476,0.6758265749613848,0.1905319263957972,-0.6805788252990025,0.5974488635318037,0.42410767267191257,0.17279015294500819,-0.4316402391350001,0.8853418927198241],[-0.30720497993031615,0.06965037533675375,0.4359605598255236,-0.5511138258875103,-0.2076474080131358,0.8081807377443844,0.0663843041161678,-0.9763829852538386,-0.20559520975405826,0.8317852337488586,-0.0596558467564523,0.5518826912171895],[0.03724115201200223,-0.29013776386255413,0.5613712257796436,0.012841470691368918,0.8077637059706525,-0.589366517497423,-0.1444418361685564,0.5847315930431752,0.7982640666229731,0.9894299634611531,0.07487829734808345,0.12418368649510031],[-0.13981682202429258,-0.29622455098200556,-0.0212223002553118,-0.66120232022361,0.6127893120472747,0.4327825675458088,-0.7377657251928403,-0.4265023052539183,-0.523256646726801,-0.13606331783988312,-0.6652706536844396,0.7340992650075885],[-0.19193120114491005,-0.41563593608264854,0.21675976398778518,-0.9213856570016339,0.25182021277555244,0.29603217985439095,0.10831279192348364,-0.5651390493339994,0.8178546289063866,0.37325167136585885,0.7856235964859625,0.4934356639594002],[-0.02684791922557622,0.02492044070025477,0.0499844868578406,-0.7392466713237408,-0.651757366036594,0.16948951222364889,0.6189980094071256,-0.5584885159326822,0.5522065210782339,-0.2652467215382408,0.5131305032721707,0.8162972885688542],[-0.013858050827061555,0.15357843930027465,0.25441071762249695,-0.9088491377883934,-0.09845447098689386,-0.40533931697281095,0.4025252906636995,0.04783437217313191,-0.9141582265750859,0.10939211634024765,-0.9939912424068399,-0.003843813355059733],[0.09262615534593954,0.14960369689177042,0.3026819943764776,0.3661533468040897,-0.407626723775672,0.8365238673750296,-0.36541365946058646,0.7637589019067564,0.5321138949865982,-0.8558063940540446,-0.5005125311339347,0.1307005052557772],[0.3489443208661577,0.2574868433234844,0.3285878762253341,0.17746035970730903,-0.4593769492456299,0.870333636736126,0.978488688585151,0.17690040793524536,-0.10614203683392914,-0.10520317031089887,0.8704476228780926,0.4808879586581622],[0.27382116766109693,-0.0492420623433875,0.10861941027153121,0.5081450811864685,-0.7066910818712695,-0.4923172668814408,0.20057801311655926,-0.4587993857357061,0.8656047506239509,-0.837590017323637,-0.5386008154952547,-0.09138995802404148],[-0.07810455098613672,-0.19834816768009722,0.03223324305054206,0.898076787348222,0.437334774511308,-0.046865542026524605,-0.15125390127520008,0.20702366504656713,-0.9665730492103114,-0.4130137302503738,0.8751454148390599,0.25207173881822076],[0.5220102171519873,-0.07028596527526643,0.23547615485722656,0.8149519963425679,0.46028630517795244,0.35212179842050667,0.21525007602229201,0.32372438725190184,-0.9213413731461202,-0.5380712298847251,0.8266432352375539,0.16474317347413756],[-0.010060886378999077,0.2148128713327883,0.05860188533689107,-0.721199977484129,-0.6219463849930931,0.3050463680670776,0.6037164229310796,-0.34836669363906464,0.7170544801103247,-0.3397014470975851,0.7013011770680336,0.6267212984116599],[0.09005677271285327,-0.06740730174255802,0.24782855948776752,-0.369504283517187,0.8019685412414932,0.46937516265929746,-0.8620851719969455,-0.4843794012485677,0.148948822986543,0.3468079305400149,-0.3496041397138637,0.8703454514211479],[-0.03869322931932506,-0.27803978844257893,0.3393242752052345,-0.480885898887167,-0.8765989769678038,-0.017971806544740843,0.4795913416872791,-0.28014356412808805,0.8315718420294227,-0.7339897119374883,0.3912720499302399,0.5551263691388755],[0.12280824572420558,0.0792296464363425,0.07515472856925104,-0.4515195773690356,-0.8357652320434626,0.3124524734416812,0.8700993322400772,-0.3348633846210717,0.361654069070834,-0.19762900416654106,0.4351585809190225,0.8783961442109613],[-0.14475651115835186,0.22009106797371575,0.22760662610880333,-0.9006844968346539,-0.3123988528496634,0.3019509792994899,-0.41674101374271005,0.42466092386492543,-0.8037350478900909,0.12285912512469316,-0.8497470544111363,-0.5126749251652482],[-0.0049721246726069455,0.2033663015412973,0.3586354883239089,-0.35337916354568616,-0.10413327017428418,-0.9296662997089663,-0.1288683085354873,0.9897293937803668,-0.06187637790264258,0.9265614528065336,0.09793870087946727,-0.3631692237003137],[-0.09596417089044654,0.2060876190538365,-0.042147140490112894,-0.233032313512335,-0.19757857388339314,-0.9521862464882034,0.9622926631375797,-0.18812989574245625,-0.196468757820467,-0.14031668229641703,-0.962065408103907,0.23396875688687443],[0.37416974543621134,-0.1652853402852892,0.10232573299836682,-0.898792442194929,0.33088544390920016,-0.2875534191455339,-0.37477486315595954,-0.9202685055441594,0.11247079465332854,-0.22741140648548652,0.20885569351236538,0.951137924534482],[-0.2720499577659827,0.0721119480074578,0.19678933229965567,0.7949969915156498,0.5200632943436003,-0.31227224237441914,0.6057784069447312,-0.7076307593244721,0.36371861395504,-0.0318167433998193,-0.4783229853711332,-0.877607438724785],[0.5002650031119688,-0.17930508983376992,0.27935268823383974,0.6416922282169404,-0.7667342430614387,0.018699859972214,0.7638463383033145,0.6366997169692648,-0.10560417544716912,0.06906414197393912,0.08204919821826705,0.9942324041013465],[0.005922947786720582,-0.43567828179297796,0.2725939375972112,0.9920620662060408,-0.12501246488375137,0.013591924760334877,-0.1103902761140399,-0.9175550165300587,-0.3819774582092381,0.06022328232891122,0.3774449301079735,-0.9240716860727416],[0.36187579487560195,-0.2820163895435981,0.3582928173720064,0.32743203765410855,0.7563354095702116,-0.5663435432208428,-0.7209934742578209,0.5873956327293595,0.3676068290009215,0.6107017854856127,0.2879637457749427,0.7376450435839372],[-0.14122140884498255,-0.4391574164846963,0.0025698122912335263,0.9720498214403221,-0.23369525706858016,-0.02248713991347291,-0.06660511480363988,-0.18265427428374179,-0.9809185362546033,0.22512863727174262,0.9549994465493318,-0.1931143541289417],[-0.5305343162690164,0.39118090940837413,0.18286495423660884,-0.23988753047178224,-0.8859332418526824,-0.39695876826762067,0.8719492862001081,-0.0168585004677117,-0.48930586881530685,0.4268002050365861,-0.4635062911572797,0.7765330018990504],[-0.1715910290688302,-0.28557988203430473,0.12088475826729429,-0.6156196585634177,0.17096142546564191,-0.7692753908666367,0.5286355672792646,-0.6343659991427464,-0.5640250137528968,-0.584428672292514,-0.7538912190325479,0.30015188966259765]],"imu_in_pelvis":[[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0]]}, -"reduced_frames": {"left_hand_palm_link":[[-0.04721414146275125,-0.25153415161341197,0.29953639699889295,-0.18897261989000844,0.9218528327555815,0.33834406108636816,-0.7829094069612283,-0.34941501523291235,0.514744604266428,0.592741266847116,-0.16762011177150968,0.7877571254556714],[-0.28599396131309285,0.35284058454981565,0.3224555567989794,-0.5385336662734497,0.23122483836329868,-0.8102570977251254,-0.27593752369465574,0.8601889610679052,0.4288746136974992,0.7961406743203984,0.45454375518841345,-0.399437105577183],[0.11871909465936775,0.16725709395482696,0.45615470096700705,0.005339776774116775,0.2167031558863909,-0.9762229402205631,-0.9858518932034575,0.16469568549116642,0.031166903082493702,0.16753367258900942,0.9622448094998869,0.2145166546899454],[-0.20682966986115753,0.11640170598214042,0.6117282432345339,-0.2989541706957488,0.17982802212875026,0.9371703613969441,-0.9486372042237035,-0.1625292987482746,-0.27142527850410264,0.10350777065637434,-0.9701783905579713,0.21918036387434808],[-0.053981830662015236,0.0971040360769464,0.1636513819364253,0.4065646918941863,0.040660579824374404,-0.9127167515463551,-0.8766564463352281,0.2986568560867832,-0.37719697428697413,0.2572520678320678,0.9534939955556638,0.1570686920917753],[-0.005689578962631659,0.5313878830312699,0.3153146588053471,0.38352254814008635,0.7187237251175708,0.5799540171610653,0.9096603898242738,-0.1855572981731217,-0.3715998712048235,-0.15946294319648477,0.6700781268238901,-0.7249599117877491],[0.013178914311602091,0.1315493882538791,0.43990500843590297,-0.9792719302949247,0.09125005173837658,-0.18083117705250895,-0.17191653188711292,0.09763099421219562,0.9802616462113806,0.10710365353413254,0.9910305832954308,-0.07991989972940802],[-0.08686563169949231,0.2123605079182773,0.013057143717993741,0.21665389608166175,-0.21246479505905164,0.952848256635416,-0.07067639426472477,0.970055779955795,0.23237175187206302,-0.9736867754089984,-0.11768812444282406,0.19515011852099068],[0.03442602003061415,0.40444857769426945,0.03554912731429763,-0.4277745129503315,0.7297327168003065,0.5333845967979867,0.9019051754417575,0.38363836707577204,0.19846575981465295,-0.05979983763756487,0.5659609220950497,-0.8222604295962694],[0.08857582544558348,0.18135734961029668,0.4022348687968148,-0.7914938585850478,0.10217096645263041,-0.6025766054505336,0.0827922062675879,-0.9589161866008606,-0.2713396352474313,-0.6055434934036511,-0.2646523014995324,0.7505173128632698],[0.08158524706156671,0.5198271806411288,0.33612094412569493,0.20837017245023204,0.7492738542502386,-0.6286259321488834,0.978006809687365,-0.16566583916063735,0.12671823049721148,-0.009195185599166836,-0.6412047419289677,-0.767314751252446],[-0.20939003343018742,0.23998364719969673,0.08962455974570295,0.418856868913297,0.13506569145267944,0.8979511024307277,0.7019861734683166,0.5791034759632022,-0.4145534662581503,-0.5759985552268858,0.8039878252310457,0.14774722080903493],[0.14205649274563326,0.28327078754352053,-0.011351147292230562,-0.7634787266021688,-0.2856173686072583,-0.5792434313618038,-0.007870783323304237,-0.8927120445988689,0.4505588265676136,-0.6457850143784762,0.3485511787069049,0.6793186226110827],[0.1178368191133845,0.16586342602121662,0.28480271075067176,0.31366211528987653,-0.3245472874901226,-0.8923481022642923,-0.7853857072766437,0.4394922392455434,-0.43590808945063664,0.5336528536768509,0.8375652988428672,-0.11704273551490454],[-0.19550935554633408,0.21073116917378787,0.20735025221721337,-0.9663935494621025,0.1805514141470016,0.1829882357080836,0.21145452100639128,0.15351730421742021,0.965256143648811,0.14618650110046733,0.9715110005323326,-0.18653654532200686],[-0.00929668360625701,-0.15730092996896233,0.14888655199781417,0.035551437035343636,-0.9979122779785121,0.053918278760162267,0.12927284263427746,0.0580911428384358,0.9899060315408619,-0.9909715573526695,-0.02822241278595855,0.1310681804808614],[-0.2530070717924007,0.1418400703764603,0.33732453342869445,-0.993446647247997,-0.11290088181491698,-0.017808704532556674,0.08603463674179163,-0.6360892170501402,-0.7668041139908209,0.07524493572607854,-0.7633111415654524,0.6416340863838396],[-0.046699391316750695,-0.027606925883626843,0.06580285614589827,-0.8815161380049441,-0.12902699164449916,-0.4541820492534012,-0.06350735993502948,-0.9207934926901581,0.38484563016038315,-0.46786334935633844,0.36809153650502485,0.8035001599766104],[-0.012912511492096001,-0.23226414598072986,0.28004013019622537,-0.5357475565015406,0.6356221670122908,-0.555840819394571,-0.5379161525264615,-0.7643381256402626,-0.35557480583396117,-0.6508615586695509,0.10849742160682885,0.7514037136925213],[0.015842523786625488,0.03011200616580625,0.4373128292656749,0.5782854329024842,0.30913185108653024,0.7549989779705676,0.49988387332077266,-0.8656248829289703,-0.02845479305896162,0.644749619066664,0.39386680578340433,-0.6551082872436969],[0.1777875787447013,-0.12102280922934033,0.22446582081691194,0.3844346847385255,0.5031674449755424,0.7739718957990496,0.6476747646258768,0.45041873976887437,-0.6145244975847868,-0.6578201672469504,0.7375265969318013,-0.15273227026856706],[0.15526777207424863,-0.030151405364162106,0.36725934584564807,0.748099261184964,-0.6385081519442262,0.1807064894110879,0.6628324864821973,0.731989285554819,-0.15762227221041242,-0.03163210834482144,0.23769523708672508,0.9708245897112139],[0.16044582898997647,-0.20041073225754497,0.27610748999155177,-0.6552137693732084,0.4804990373484933,0.5829370390796277,-0.6049563590632754,-0.7958991018480317,-0.02392536951443555,0.4524629488175332,-0.36832770026793793,0.8121649987303531],[0.07383636189318016,0.18827864614528422,0.275991429415474,-0.683570935651179,0.20680351649072648,-0.6999736291461569,-0.727027665265714,-0.10816124116380552,0.6780353382001485,0.064510075734388,0.9723854438837382,0.22428775858652783],[-0.09256940047548033,0.05366979958617625,0.41057033748930855,0.28006713075212086,0.6311104558740152,0.7233685055065414,0.9541020839417307,-0.26625545020763997,-0.1371030585099174,0.10607363334175605,0.7285654587758191,-0.6767132011335814],[0.08738926244924095,0.17054110660191335,0.1555695341523853,-0.2862189788297891,0.4767003212094095,-0.8311651459947529,-0.6001981195938352,-0.7653791406754328,-0.23228643579632513,-0.7468874837575317,0.43237897124835045,0.5051806724614085],[-0.13788925117076936,-0.014752434401893725,0.37247988903393764,-0.929438698528585,0.36814722801604294,0.024724970810789837,-0.3252551344998554,-0.8491045418277196,0.4162097722651641,0.17422055894460803,0.37879954534244414,0.9089323964351727],[0.17192289656867557,0.5024657504340075,0.35519053769308595,0.7342707962998754,0.5371943257057296,-0.4150525920057763,0.6762041366470813,-0.5247788363597817,0.5170639597673083,0.0599530089530311,-0.6603252451167075,-0.7485827992807728],[-0.23875053944842206,0.053780193326282824,0.21759857643707298,-0.5719574120083941,-0.33318115678027915,0.7495699004197109,0.31440226718188513,0.7549641346912604,0.5754827275607559,-0.7576383921367439,0.5648180876144384,-0.32705442156316705],[0.22562231629880225,0.3579890940263135,0.1028716108063833,0.3129396335899645,0.9244699220395676,0.21777086346142532,-0.40765512170919826,0.3378324323200031,-0.8483434147896767,-0.8578380310249933,0.17670486953365616,0.4825860561705612],[-0.19154374411499966,0.10753414321393326,0.1591468699235954,-0.8501179732193099,-0.4749166729510582,-0.2274941435786648,0.49838227100495,-0.8651233018493956,-0.05636297140098437,-0.17004276978878383,-0.16129422292908394,0.9721469179564691],[0.055266128061971745,0.5359762372925687,0.3000218587891777,-0.19786818002150908,0.9788701920574167,0.05158808424871087,0.9793736190818382,0.19961930131289432,-0.03129614656622051,-0.04093284233373183,0.04433149720941439,-0.9981779504545538],[-0.14494723582302563,0.20661411354676512,0.5031306707374383,-0.5194379756828577,0.31401165131915604,-0.7947206252855852,0.02174140156105947,0.9345883594495632,0.3550660640494033,0.8542315265059326,0.1671564573003117,-0.4922877389354252],[0.039382101072653844,0.11795336100940584,0.615853323798605,0.2398133072557174,0.9550400775104341,-0.17432162233050524,0.5913659645116871,-0.0013026449779556803,0.806402256403852,0.7699193942746454,-0.29647386641140216,-0.5650905881846177],[0.12234000271321963,0.0333000480751222,0.13744515453607442,-0.24255604257236718,0.43085696462077305,-0.8692116210966152,-0.29142807270289206,-0.8869462430204327,-0.3583239322632768,-0.9253303435272777,0.16639903253839367,0.34069798549151525],[0.18083935323362188,-0.1878998485597229,0.3997468590177806,0.9739545303757542,0.10127453647983092,0.20286951723050795,-0.2119175320832398,0.08835502004841497,0.9732853384429425,0.08064448125131057,-0.9909272721512108,0.10751562189127414],[0.025091364444328437,0.08494976067754234,-0.07959820110349476,0.13216742832339598,0.49847312537926164,-0.8567708644468549,0.7620264975172951,-0.6038716659429453,-0.23378329314306445,-0.6339143380393791,-0.6219835643696084,-0.4596618949643021],[0.006919832206896577,-0.2196420228499276,0.2204506746461715,-0.9115848183982024,0.0071562248763129,0.4110497625731413,-0.141674674606065,0.9331341993317981,-0.3304373656423706,-0.3859292751802872,-0.35945702731053075,-0.8496171138082698],[-0.022930610022667348,0.3911896446804228,0.19172484212638533,0.49115220854665154,0.36272353646355526,-0.7919603172729797,0.8509663874864485,-0.3940190329140909,0.3472826069208018,-0.18607986298364024,-0.8445002296013997,-0.5021888557058721],[-0.2845816992065445,0.05251858961880207,0.2601174945373905,-0.9652383878077342,0.21177673209883546,0.15318443277260305,-0.23811126124431725,-0.954180482042758,-0.1812253706225766,0.10778627914055916,-0.21140062306007493,0.9714380549468122],[0.07544497339615662,-0.2097507310155959,0.24025078168700323,0.18616210103961875,-0.9683933322827608,-0.16600610267940627,-0.38914497706766893,-0.22781521797583493,0.8925617139904818,-0.9021695289464828,-0.1015607229637206,-0.4192559607108385],[0.26026466951511507,0.1260471500082965,0.5626371146259297,0.9747302881813387,0.08705717801294888,-0.2057229035823331,0.22174615934900083,-0.4884328032201924,0.8439561822467042,-0.027009370890336254,-0.8682479164932596,-0.4953948419081709],[0.02616013738666865,0.040761320244183996,0.6518014541438792,0.20641251074799083,0.8274017870480334,-0.522302745729363,0.8645002798590733,0.09580535335566592,0.4934172680318155,0.45829372844311883,-0.553378366995058,-0.6955136529296805],[-0.31179811732618856,0.23855289835845428,0.43451021726415484,-0.7031118241923044,-0.6146414994664017,0.3575606099875422,-0.5971296330407241,0.7833915347223152,0.17243521876364576,-0.386095796436092,-0.09226879461177229,-0.9178325040633886],[0.1298285256528801,-0.0185326626250295,0.14286502368398502,0.3855800984887654,0.9179231549360433,-0.09351507515718888,-0.90117935585727,0.3964076777528584,0.17531891396897836,0.19799938440831066,0.016674371077997505,0.9800603089213864],[0.03343953324215293,0.38456307646052085,0.28438155424761086,-0.9362956490247197,-0.343089296386345,-0.07510121385504781,-0.33147220023042584,0.9339100615081029,-0.13393348158071805,0.11608892320880283,-0.10050737146625023,-0.9881404911191353],[-0.18968814387915867,-0.14164939562187023,0.1547803907395585,-0.7619057010052734,-0.6381444028225114,0.11077645924095503,-0.32471792233826785,0.22836645147835072,-0.9178273447394694,0.560408755898747,-0.735268988184247,-0.3812106259360201],[-0.12719735151472197,0.46770951009979456,0.24318974636259785,-0.6483731046816177,0.7217342520840844,-0.24230556430702738,0.6085906465650803,0.3001270463916696,-0.7345346696635293,-0.45741637709970245,-0.6237174243139597,-0.6338350199919085],[-0.2956210402775195,0.049271446704449134,0.2812391936370522,-0.09823188367244703,-0.5508062261934794,0.8288323100704149,0.7046771415849216,-0.626590598402145,-0.3328878912212734,0.7026952562511486,0.551358978507233,0.4496917318136873],[-0.022179751183511395,0.2618883498743878,-0.052775315812743584,0.5659626856494212,0.7688964602028047,0.29746339596678323,0.23111417725799535,0.1983732085388906,-0.9524884814024503,-0.7913737900150943,0.6078209470079018,-0.06543103893079308]],"right_hand_palm_link":[[0.04904596047623374,-0.07011914276841394,0.4482333991394153,0.3990953902097941,-0.12862695588775042,0.9078424839873616,-0.7831777853519717,-0.5627058047345809,0.26456517882969566,0.47681802193758716,-0.8165888093386159,-0.32531106716259794],[-0.1857455825043671,-0.35355356571087126,0.25915257301345446,-0.34384284003013216,-0.04745990897652239,0.9378270940850202,-0.4532657529938031,0.8830565313395468,-0.12149617122168309,-0.8223881434717876,-0.4668604925440877,-0.32514461702220776],[0.18884586311575635,-0.22954214433654319,0.1655218235445384,-0.2564895490294925,-0.3724486099206872,-0.8919053448695076,0.21695075432714273,-0.9214134290730644,0.32238123847503986,-0.9418840063593454,-0.11081211888581793,0.31713592176305744],[0.10400054105481328,-0.23623091553802905,0.08647353983345686,0.9702870150795811,-0.1298081316078265,-0.20418853380256757,-0.18241261753772742,0.16198369841531812,-0.9697870479701872,0.15896145863382055,0.9782183449599483,0.13349203816268446],[-0.09895981008595614,-0.04540708907356383,0.32339116564817644,-0.14455427507063,-0.8482428402521576,-0.5094979347551443,-0.29170468800350224,0.5285542929146108,-0.7972068329099515,0.9455223088619242,0.03338328028352716,-0.3238411957147014],[0.19192500386472122,0.04712276117851701,0.1163399326762369,0.2902134717493659,0.040923771938271014,0.9560864948871122,0.9461826561681795,0.13726255149414762,-0.2930825363679057,-0.14322891461471138,0.9896889596472527,0.0011140783773188612],[-0.09391777622026529,0.15827845343318336,0.16045042362782155,-0.5351504698012423,-0.7584492887448533,0.37197936915094576,0.5509720265630744,-0.6471660476078639,-0.5268832249831029,0.640346625365113,-0.07701157851935045,0.7642155560818356],[0.3565122505732285,-0.03362896938792798,0.24512568411522867,0.8637557155340739,0.2565522315735348,-0.43371305762780454,-0.45494296763348135,0.7671301700153171,-0.4522589948835866,0.2166863172492928,0.5879559972709779,0.7793297024944119],[0.060505730595066316,-0.12043094554108849,0.4407359258543245,-0.7522837257339976,0.5241039180179703,0.39922960701078797,0.6353309660122972,0.4166611336564246,0.6501907899424785,0.1744240798217284,0.7427708817787324,-0.6464268385207933],[0.06472150890318161,0.08389084572869134,0.46041945271134027,-0.17032894106738722,-0.8946387220039196,0.41304916042289186,-0.6208818176895954,-0.22805988250483883,-0.7499963056271297,0.7651756793686837,-0.38420079006044805,-0.5166197176062883],[-0.17456682213929148,-0.11262351184705277,0.28485716782084447,0.14227906902152687,0.9878029373384627,-0.063261548383456,-0.8815168305493674,0.15552055542138044,0.44580425558716197,0.45020522428839704,-0.007662494824146604,0.8928922343690217],[0.22659773062369326,-0.1536861282546637,0.6301025953146461,0.1798517709272791,-0.8343407940553692,-0.5210842349077327,0.8411419575238325,0.40509331498564904,-0.35830100955329813,0.5100328889270733,-0.37386474229190625,0.7746557988446934],[0.34890008843770604,-0.29842832150143905,0.21558483865031158,0.30689786032016314,-0.18901373467087562,-0.9327848151833699,-0.9326451263729683,-0.2550764572826835,-0.25516478830917333,-0.1897017964692364,0.9482667393966125,-0.2545651574164485],[-0.11497065559784606,-0.07130954605584605,-0.08607022388943576,-0.04626174693555759,0.10615819506888124,0.9932725146656319,0.7438834773343441,0.6673023785393027,-0.036672983853964905,-0.6667062493478179,0.7371824558516746,-0.10983990106085811],[-0.08518026758205356,-0.3227483451199766,-0.03426803456985131,0.22950835237772316,0.5600682989602519,-0.7960209901055579,-0.8828492370725132,-0.22452219627923992,-0.4125130397677481,-0.40975985746452026,0.7974417118998204,0.44292615110528244],[0.1194027564394314,-0.4284673470835449,0.46434125430543405,0.6317493854181583,-0.5983341777043332,-0.49283762621644306,0.09072903473033844,0.6884819544731791,-0.7195560024221135,0.7698447611568714,0.40986438019870763,0.48923433399920835],[-0.2244128766762815,-0.018829935024437427,0.12699625670081366,-0.9843301596435186,-0.125097394011131,0.12427702453707523,-0.14787247156697328,0.2016559766095824,-0.9682296211386585,0.09606179765567402,-0.971434768324286,-0.2169945204881933],[0.08577907982753157,0.18055782065383857,0.13421514434165138,0.21641211762854026,-0.33983298221852554,0.9152482392990408,-0.06842006456584637,0.9298801135397567,0.3614438672990514,-0.9739016840052883,-0.14084217635265445,0.1779859299268106],[-0.2268088833085372,-0.3399010197496563,0.5125753298644711,-0.3215148867010097,-0.4953163928410087,-0.8070253085328914,-0.3422489731227242,0.8554469526730405,-0.38868515736885284,0.8828894710132621,0.15123551879562078,-0.44456045688808365],[-0.13600674960877757,-0.404614351715088,0.3056534932303348,-0.38433291198241204,-0.1254825505182194,-0.9146268869230569,0.5228988411756655,-0.8460695373754663,-0.10364911877295813,-0.760831791307557,-0.5180931069225946,0.390787049292869],[0.2501396629236904,-0.33303943502974176,0.3163292037209437,0.00386197283383638,0.7124108882363227,-0.7017519586635752,-0.7177462911366748,0.49060774545437635,0.49410960490535083,0.6962940088248487,0.501771627757596,0.5132249865820193],[0.24693068524557626,-0.2891176557813636,0.4006419607855042,-0.10098852654413068,0.7077831508679309,-0.6991740333092392,0.4918789700173967,0.6463873639183932,0.5832996267967742,0.8647869080652587,-0.28500243352311005,-0.4134213547034487],[-0.18079272411717343,-0.12412626942794742,0.03323064911838046,-0.20938187421549537,0.8374294665930199,-0.5048476198138765,-0.3540048825546228,-0.5461881154552697,-0.7591831700340231,-0.91150412718621,0.01975932734560734,0.4108160112572963],[0.005953706461728256,-0.09959561112473529,0.17177715813383868,0.9575748359948154,-0.18087986261986777,0.22434996939629928,-0.059012740844638076,0.6389226432383126,0.7670041410418665,-0.28207787911764065,-0.7477033711679958,0.6011420288555012],[-0.11956555786025813,-0.29337887137358554,0.4357179614708785,-0.1970520036924564,0.9719294223236554,-0.12854456761136723,-0.5063642443978495,0.011376110726094651,0.8622446497950234,0.8395032816113247,0.23499740876518793,0.48990862213015224],[0.23772801961100581,-0.42816670918690963,0.3790386511793087,0.37562661438254186,0.7433687491965197,-0.5534505843212298,-0.44812451520252283,0.6684134228961021,0.5936395496989911,0.8112268890033253,0.025027960570495257,0.5841956314007053],[-0.06454319496563218,-0.3262603739424939,0.010550301972038975,-0.4098244318132098,0.3243659314260551,0.8525436514449497,-0.9081647940841998,-0.23252361491659107,-0.3480940609836999,0.08532667739331395,-0.9169075804224923,0.38987157768781855],[-0.0655994199633123,-0.4321743829138195,0.2640572844815963,-0.9251247194022657,0.37914454555810084,0.01984104645476434,-0.132835573191187,-0.37219435313722815,0.9186000620442659,0.3556669285242132,0.8471840278630738,0.3946900795407084],[0.0311305273764015,0.01944487245426242,0.04692354002891282,0.673058904645459,-0.2039091383240522,0.7109238877583147,0.699427593695846,0.48793869624919384,-0.522223007803635,-0.24040123139096636,0.8487266297286656,0.4710311623822012],[-0.2255160978149555,0.031992848208142896,0.17345121006045292,-0.6304933314031189,0.34726340660137084,0.6941802975393411,-0.7599047678946522,-0.09389429427028245,-0.6432173856749057,-0.1581862914009043,-0.9330551901892497,0.3230868447859361],[0.002891688218067007,-0.062311828805004875,0.3516083938967811,-0.28163397216898045,0.956512019790186,-0.07594117274062157,-0.40576430683088965,-0.19044409566261805,-0.8939163124864069,-0.8695042455459143,-0.2209429845551712,0.44175396382317145],[0.00861015095622333,-0.4800646848563497,0.24170641519344968,0.8353296121645093,-0.008611674313668272,-0.5496819790629904,-0.5442402804286433,0.12824469748973008,-0.829069246037217,0.07763347345084032,0.991705166181604,0.10243977337282004],[-0.20461941473348652,-0.18507758213791792,-0.029183469898082993,0.20660570455703128,0.21863201073598754,0.9536845006216844,-0.2522515043858249,0.953666430537094,-0.16398023600948577,-0.9453482222861485,-0.20668909779709613,0.25218317840855586],[0.11311930692136087,-0.18029022068927358,0.05876164647408195,0.5087264747775895,0.2897868353938793,0.8106916577175118,0.45104313460539797,0.712363830600031,-0.5376782156440668,-0.7333194832988577,0.6391880497046635,0.23169197769677988],[-0.06331036082757631,-0.5096044853729111,0.26800279917061154,0.12025504853219803,0.4079063684015388,-0.9050696757266741,-0.8464021327480253,-0.4343024759401706,-0.308196023776803,-0.5187891218810518,0.8031150316173304,0.29302575485472515],[-0.12739659948772186,0.16547870453159363,0.09657709500161417,-0.9989152540818038,-0.044803706204438076,0.012686334105318997,0.03835701440052024,-0.6372368284206399,0.7697129100844562,-0.026401791775629027,0.7693645770470265,0.6382641247761667],[-0.0519789235179348,0.0003539744010101816,0.2710712477288645,0.7324563223467453,-0.4518538062283559,-0.5092503054993965,0.3428903290006993,0.8910433802819264,-0.2974355673646826,0.5881615068432082,0.043241557010658294,0.8075866576505712],[0.1057969463581481,-0.31782025529589236,0.2771358377842901,-0.4310747647269457,-0.39925441260743066,-0.8091788808595437,0.27140966406449296,-0.9126229283037616,0.3057060434251611,-0.860529686569852,-0.0878368074770644,0.5017702201050093],[-0.05977005398296794,-0.12490661896061843,0.10251332977763107,0.8799155152299327,0.2288371893970454,-0.41639191491490585,-0.315247994258667,0.9368706025377643,-0.1513016067872843,0.35548190976972227,0.2643993472937995,0.8965074439050039],[0.30281064214365455,-0.07852654658550415,0.11392384596338141,0.4100890726141317,0.1839462404770168,-0.8933032705284656,0.9051124531078378,0.03845765300169951,0.4234293992564915,0.11224259330394026,-0.9821836842159274,-0.1507209697037339],[-0.01970371723120673,0.021911571007881846,0.4158027094113707,0.3048086432891246,-0.7448551186305512,-0.5935339444598859,-0.5685225440443181,0.3576844760711747,-0.7408400181490373,0.764116357559253,0.5632518689072917,-0.3144416071114369],[-0.2319419464417343,-0.0596477038115808,0.0013677776329436187,-0.9997084739857927,0.0009453929647527115,0.02412619475050981,-0.022272166386992476,-0.42194056084546605,-0.9063498848225466,0.009322963339381318,-0.9066230028769156,0.4218386101449426],[-0.10449446565173728,-0.36943282426846114,0.19690934756242412,-0.5057964799562058,-0.8623857684201942,0.021464093044493482,0.7347205133034163,-0.4176142753302925,0.5345877705031412,-0.45205717359099185,0.28616272200736753,0.8448403448807698],[-0.07758990091972787,-0.30797180192397117,0.08551060395288224,-0.623287845613739,0.7321991800189162,-0.27458445383854313,0.7585723344563535,0.6514130898925834,0.015132736501885548,0.1899480847695558,-0.19886011942122586,-0.9614439026777141],[-0.06458238260792257,-0.515940721271558,0.30233492798740386,0.7607309509322635,0.5950568786458692,0.2592213946994521,-0.6034362987645417,0.7954971609623761,-0.05521684737605176,-0.2390670483923805,-0.11441843416615428,0.9642382321272686],[0.2614703426016305,-0.3386540338762334,0.2851900226136368,0.9006837055451125,0.3975048385210931,-0.17538177190872767,0.23368369733765723,-0.7835171779545562,-0.5757540807052356,-0.3662796638691909,0.4775884579870503,-0.7985915555735761],[-0.18691448806937017,-0.4791222312042368,0.25543307864326653,-0.6110605207210904,0.744151411606126,0.26989575139427424,-0.6124082086740453,-0.6604539878082054,0.4344614090306368,0.5015587961193753,0.10019584119537664,0.8593017906663724],[0.33107890754475283,-0.23967968527283517,0.26296645594622664,0.333004755561055,-0.31086204667458045,0.8902093128646841,0.942891280267622,0.1177820736374882,-0.3115821187503192,-0.007991643698679537,0.9431289260047878,0.3323311038167345],[-0.07510218370021182,-0.43609747252044295,0.5500318818826702,-0.4569097300023205,0.1374303395054391,0.8788324074660799,-0.10252846638415736,-0.9895444435477676,0.10143819706921077,0.8835844114798083,-0.04375723971137868,0.4662230064708537],[-0.31904277602416115,-0.01235573800140443,0.16597105905292847,-0.05357126976524426,-0.45315652010614826,-0.8898198061074083,0.5707461257808757,-0.7450919071943864,0.34508971259594623,-0.8193771896763178,-0.4893743128922051,0.2985528477831384]],"imu_in_pelvis":[[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.04525,0.0,-0.08339,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0]]}, -"full_q": [[0.0,0.0,0.0,0.32158718716749124,-0.16762835728739844,-0.9311342455448801,0.038359244731825906,2.8099673109238856,2.835355398437711,-2.570812717510764,1.227540434335133,-0.3374989609735586,-0.18750867525730305,-1.2991738066241183,-1.729400966874867,-1.2124226403037035,0.5162581746745518,-0.18391105965321308,-0.13178098685768436,-1.7994601518766555,0.350087850910354,0.385190754225908,1.6144579818512543,-1.0870248729355392,-2.4924573682562245,1.2530940011666345,0.3043983134571331,-0.5650649150895581,-0.43559287589963147,-2.2845700320117697,1.393193284598862,-0.7371589480949257,-0.8987143730472128,1.5979757755465296,-0.355642569676891,0.4522919230801017],[0.0,0.0,0.0,-0.40565311338002624,-0.06485142648470478,0.9116893483905545,-0.007897855354256047,-0.8510992551705563,1.18239289537875,-2.3862491618927657,1.597307701491228,-0.6829228459122711,0.010263804109841845,1.897700431376371,-2.5505440863732525,-2.411900085374924,0.9808443158408728,0.40684193939818625,0.20867108346904717,-2.2338293311381316,0.5155975932139694,-0.05091309280920375,1.146599144571291,0.8264323396977016,-0.436363718082049,1.7060666678737995,0.43919057243305426,1.1862968603723945,-0.9671473703999758,2.4772833322417824,-1.8574986402633074,0.5097805466367427,-0.05124052834277226,1.547966504681498,-1.063073273276633,-0.2593157184068551],[0.0,0.0,0.0,0.5398121364372905,-0.0524429716779594,-0.5163975658911952,-0.6627112086106901,0.4769841655971372,1.0962988789561954,-0.8980603038871027,0.3314125137014461,0.23524323715356787,-0.030239731749173077,-0.62546821018669,-1.601984990526715,1.478700858311191,1.873904303727157,0.2053573733617432,-0.1573651043036454,0.5047930972590393,-0.0658278829341824,0.1186695190360505,0.9274178759824503,1.8565537280678326,1.5342983405821475,-0.4524855181576517,-1.2669135945455783,-0.7172918275602816,-0.5935140477498786,-1.3507195501837077,-1.4317366600737953,1.97583477788432,-0.04902549333597461,-0.21030657656128104,-0.8706450034684666,0.7099719616858031],[0.0,0.0,0.0,0.7119044205364613,0.37913486596587975,0.2862950784272649,0.5171885318701547,1.1232134766447386,-0.5152801757487465,2.2638558496543206,2.0586509012671574,-0.6325862154789423,-0.15550411156924648,0.699770442043615,-0.3795782178703222,1.792042906434757,2.0350553455442353,0.14730959703718682,-0.08736064569978971,-1.2705960763089372,-0.26393074757583124,0.2224768615560082,2.470167221157907,0.7412397512569808,1.3661194411941748,1.9607524716138063,-1.9410421587227482,-0.30754024459164464,1.143364112139052,0.33498068862289765,0.3166111075092961,-0.8911587656169033,-0.3524904481925504,1.366232927936235,0.7907454037640729,0.4131872470758642],[0.0,0.0,0.0,-0.5302826811962619,-0.2150230020020736,-0.35076951962137515,-0.7413002972735879,1.806558645279678,0.48713662399451574,-1.7624834685429207,2.166488836565179,0.1175967024340766,0.026295386171465818,2.421181241687092,-1.009973986977391,-2.3046087306620318,2.3419527790847523,-0.038105576023000465,0.11112133637840277,-1.0327595290996545,-0.1716678260812718,0.030777405156761195,1.2526125059908075,0.8054244069338692,-0.5778639168960154,-0.8299077645654174,1.9052121962259938,-0.44955845747281176,0.8629521967140568,-0.9415019056093437,0.9220320567583777,2.1236408940989686,-0.8825349746510227,-1.2564013097362503,0.7429898973281206,-0.7017586979080691],[0.0,0.0,0.0,0.29996582511733405,0.5839999861085168,0.7500476023658189,-0.07995695199428071,0.727590292012426,2.888795087254544,-2.0639707323777987,2.7809495029867213,0.41848471204077875,-0.13638676901903776,-1.6134607652052777,0.08845729413560344,-1.6393142412732888,2.587923112928488,-0.6499420684215109,-0.20726281803676855,2.5828873685629534,-0.04014191818132101,-0.055568287380867,-2.4751939483821834,2.2183602810709124,-1.7961518263631127,1.3168771035958595,1.554766431348174,-0.5152682614543425,0.120874524472105,-1.369765740282072,1.1434113407914523,-1.8840796128134125,0.030181294445242868,-1.1872532508043396,1.112214913410013,1.3758724191194622],[0.0,0.0,0.0,0.9097005762162574,0.16225106030902284,0.2708654703314533,-0.2697245855359273,-1.695434035173172,2.7710478997495915,2.7328715161856785,1.32082129170695,0.20753951683926675,0.10115565452440978,-0.44920019999173055,0.3739364401371983,0.6012373511070761,2.5082917858144644,0.281964857748474,-0.11312190770921124,-0.27615696285805047,-0.2981718127347859,-0.45870044167441976,-1.5224831034962398,0.9513852793208724,-0.2417846773808061,-0.9150092163129504,-1.352234422001081,-0.7516528153390128,-0.42675840811376875,0.9741860005455001,1.035723828737075,2.1613605433521665,2.088416382408325,0.41208949724709454,-1.541836626658006,1.2353920647818504],[0.0,0.0,0.0,0.13173029506979586,-0.12590010887099284,0.8983976690344727,-0.39959719746329914,0.4975332386031046,2.355322155995005,1.9919055878054577,0.8799354854922198,-0.11495933660936031,0.07132122255612172,2.1618207943972965,-2.704880000369937,2.073542667159237,0.9778706776391233,0.2578999963584384,-0.04788435244666586,0.5636089364943833,0.4598953474451658,-0.21275324162592718,0.7135323378128429,1.0381327423182762,-1.1372450868330561,-0.04043465004628155,-1.1953449287434896,-0.3555353761495914,0.35545046395902347,-1.5951210486995309,0.3268510860082867,0.36028399671223266,2.011516549177525,1.3006334199761862,-1.0250614008784262,-0.2855407941952932],[0.0,0.0,0.0,0.7622480870388769,0.548232288120431,0.2502451168047724,-0.23621302585415432,2.682763755221598,-0.5163147313425483,-1.5728204455335022,1.9306251005142039,-0.4565826637686854,0.10242392515266574,1.162826238820653,-0.9854925442745719,-1.034603374740787,1.1811381193367998,-0.6820838134767003,0.02413339000989434,-1.0959059993825933,-0.48304238744110173,0.13323623436758503,-0.0636032523951724,0.7721330918059826,-0.7388872478980091,1.082971084657255,0.03508545168847643,1.344809313194421,0.12216831338706324,-2.5839680710974307,-1.718274339610895,1.8686155485453324,-0.9427369028881446,-0.967963259886907,0.8160984137809886,-1.1120315351898635],[0.0,0.0,0.0,-0.47566403321108475,0.8633468413651623,-0.11713357266140774,0.12106066731079984,0.9407075541986862,2.8452259870720824,-0.6726930440416465,1.531904401525803,0.0655284298488652,0.21310694623164822,1.4107316280086413,-1.4432650239781806,0.5325652523386033,0.318374060567256,-0.2626400719246792,-0.0750073737456157,2.238717835430585,-0.2679090404098739,-0.12175867565852716,-2.1067004734084995,-0.4387215991037874,1.3037424114864047,-0.7963863100489668,1.6462872825232684,0.1412813093341314,1.5843071594158078,-2.000422647025765,1.2862835094878373,-0.5544434809271546,0.18542395351052865,0.8385857908791401,-0.8336933418509177,0.9647652800283166],[0.0,0.0,0.0,0.601093969852201,0.46324300774799637,-0.20153059467983123,0.619255500248407,-1.6228606066895863,2.8350138487679377,0.06084380792963451,1.437292423562193,0.18429135803445307,-0.020153852755175478,-1.781298421510233,-1.5778749980974769,-2.170653192562861,2.2448498309947933,0.14301537614489745,-0.07779728426715382,2.3924474468940526,0.15860230334077374,-0.4778494498336641,-0.7944165606800371,1.8366773291298713,1.745637673469472,2.0655825144447357,-1.5423450069532274,-0.11619401440360688,-0.16608292193053265,1.0289428412374173,1.2859208930027632,-2.013664935715008,-0.7282162149785666,1.6831768629096806,-0.1463064946211141,1.003576099056221],[0.0,0.0,0.0,-0.13987765682122916,0.255851288574253,0.7877696121341803,-0.542580314290023,-0.7828101532521017,1.7009531738580406,1.9915534621329893,0.8244728392603266,-0.09331087978043706,-0.21097905254536722,1.7001729928126266,0.4813686014396126,0.6841164846723218,0.179154810066132,-0.4421857719548557,0.10776469502150632,0.997078685676513,-0.49616106577362923,0.40482276209261137,0.6570131320040553,-0.16595379063092164,2.261150896689244,0.5461058257876465,1.2260926075833631,-1.5538240494258946,-0.43007661743814585,-2.4964740077684775,-0.5222156274537637,-1.2301567063621737,1.3006157099287996,-0.7652468633414651,0.5405918124883964,1.2040121506217891],[0.0,0.0,0.0,0.2630106427174576,0.04297647336967134,0.9543861941511557,0.13463066874872387,2.6476069472645007,2.264793707364517,0.49748542325384815,-0.0655501406279025,0.46513125025706736,-0.16924088610625332,1.9247338912041,-2.807971590696314,-1.6782419488437639,2.315597207255253,0.06480806206129752,-0.12166897395739801,-0.5294402435078496,0.13655860061118974,-0.007684227280339462,-0.5612777894462022,0.3798652710760613,0.9052505631380003,1.675753697312105,-1.0478245416529042,-0.8178770239885055,1.3091895473602848,-1.3589065176127986,-0.4797255783832046,-2.067790833817721,1.5826024821281004,0.4880005824210145,-0.9276978507570848,0.439484819965114],[0.0,0.0,0.0,0.05329851108412285,-0.9358582143752239,-0.2620026327061418,-0.22952841165677904,0.15764848494887485,0.24101563318873942,-2.047354946851464,0.23630537683799768,-0.3012667471236645,0.13174225263833506,-1.9318063907187262,-0.21273782101055438,-0.9218822628296328,1.5896364936945129,-0.48429963167189105,0.06891131835410647,2.070070512597139,-0.037430754268546196,-0.0767133065545072,0.16760034631451282,2.226902560344437,-0.39592356134849505,-0.9896723708972168,0.3133447457709124,-0.05930895916901213,1.439366077352734,0.3191496127981339,0.0989725080518733,-0.39335168594139214,1.6907224000015548,-0.4779841891502963,-0.29748030038518003,0.6766435538464102],[0.0,0.0,0.0,-0.4417649912682552,0.14058633100290127,0.35285274937589545,0.8127571059564922,-0.30231189579116213,2.735428533890301,0.5339852942212446,1.0591537479236894,-0.4381607193543413,0.14398811816035922,-1.3105358164423904,0.4776987386315894,-0.179714682989359,1.624545490744828,0.00802949240634443,0.23447028904143757,-1.03657544993406,0.23990563647408014,0.08394271969908718,0.7681930784081876,-0.544781082706542,1.475255356022989,-0.6268260647311961,1.6803294265185817,1.190869178379544,-0.6364042478617776,-0.016866529168876543,-0.15450275144459802,-2.412197417091709,0.6900384181101207,0.19582283089451913,0.46864090300418826,0.7754303557579532],[0.0,0.0,0.0,0.28956759349760713,-0.05651281779240322,0.9420386621903208,-0.15975002084456166,0.6878808105933754,2.7545812215380545,-1.0104749465512846,1.7821216973351057,-0.31346379280784564,-0.057668574183264154,-2.4043532881829757,0.01326936806307133,-2.5058678632295277,1.0794460614447914,-0.5578298679408455,-0.024290026258186825,-0.4313912792479955,0.3241222880144615,-0.45737491698164096,-1.1611385167288977,-1.2227312802974153,2.32752128418725,1.7894906607931473,-1.2732933571672644,-1.6105825335292119,-0.9003888537936862,0.7790636339621724,-1.8886638925015875,1.7200049380736409,2.071549387729384,-0.6832227482270172,0.6346545917837012,0.7237552465823824],[0.0,0.0,0.0,-0.5270283452524418,0.6240048313818112,-0.45291402378041273,-0.3573905157886346,0.13217582473062617,0.7679489085423795,-1.7967596888316781,1.6816147727053687,0.41233492039729314,0.1321330054887081,-1.4291908698369762,-2.9648856132224624,0.9393390233662009,0.9278658890266915,-0.16312653711524272,0.08126998095946086,-1.8485830621888772,-0.2414100401114671,-0.38275809383829923,1.6151010734602735,-0.7414007343149494,1.5629969236193193,-0.6271470812271999,-1.8775709217474146,-0.5256241638134529,0.9871548218565744,0.3104732066585112,1.1733502661972444,-2.248138239912695,-0.0861292414839755,-0.5875246464096626,0.31021043814251015,-0.5408946858637755],[0.0,0.0,0.0,0.3234720298927582,-0.8073899509479279,-0.14205364707032017,0.4725548373886169,0.8544904044503259,2.782108077743675,1.1421562357046988,2.1204702143014886,0.11711859038876882,-0.16626438466716426,2.6268512165539577,-0.49114323098904666,-1.1739044881680156,0.7497695054319581,-0.3081608646015148,0.11332521547540653,1.8975523816022508,-0.18062841282785558,-0.14190884499410944,-2.2947430608254615,-1.2882953671694743,-2.441713013790702,0.2647178991016519,-0.8379941316916497,-0.9428729149843454,0.7089476747689829,0.9187650957730709,1.4664185334557085,0.3872554057004107,1.1223037933404358,-1.0438006732126697,0.742758209023602,-0.9935031993929039],[0.0,0.0,0.0,0.5167079155072625,0.024431295214530854,0.6561688299227655,0.549416516409972,-1.046867608035519,2.002851290854081,2.4890477862767195,1.6238839976969586,-0.5191368507019989,-0.17777855951299365,2.402903889844237,-0.5679473880703636,-0.5315692655475628,-0.05024240624195862,0.09692672000814118,-0.009904194203215644,-2.1659480049869018,0.31534252622736403,-0.2944430419632354,2.048970651350886,-1.5055444528009767,2.271257720459762,0.6248758354435899,0.3229183160702269,0.8653683599147108,-1.1534270258182766,1.6273306206924287,-1.5248405466650146,2.1320697071485077,0.5609642933230967,1.89322666063322,0.48546245591667403,-0.44697189735002496],[0.0,0.0,0.0,-0.1527407479618371,0.43147284963284244,-0.49968202179959775,-0.7354041888866554,-0.7694295690803181,0.5948676820841904,-0.41103188781723743,1.4667535043333448,-0.8143594444424718,-0.0289680601419883,0.9003812155834399,0.3071111242381406,-1.320462886604482,0.6002254917694736,-0.19430058824363738,-0.10730862645991324,1.9154676649387263,-0.14625747797459882,0.30386893515596247,-2.7665502849214922,-1.5294956014274066,0.8900877242904808,-0.7630067199738022,-1.0340988739058066,-0.8470619179521944,-0.1047126611121576,-0.10389738786562264,-2.154200282599504,2.5897480760207707,0.4731272170787042,-1.6228138178629383,0.7016243327761524,-1.4467291839356429],[0.0,0.0,0.0,-0.2151698744280904,0.3953758997990132,-0.8099512105948771,0.3759772060279901,-1.456038557864408,-0.4360749174727264,-0.1586808503732624,2.254365464344565,0.3952437881207719,-0.008901471133528405,1.0577723873333023,-2.2209918069133794,-1.1974177271085449,0.09698915365078493,0.22060815869121908,-0.09460837608930547,0.8336923152268274,-0.5144043937304031,-0.45896665209991544,-2.2571853124893475,-0.8991713361320748,2.5676169788288727,0.9685880915869156,1.0654067094035824,1.552841578690302,1.5535128601383235,-0.7746729401650976,-0.3553305997625966,-0.7686304101317394,0.2332225023086556,-0.1626467703212695,-0.4578214460921295,-0.5100690344071563],[0.0,0.0,0.0,-0.26777380692818403,0.08898412885801867,-0.82161317868832,0.4953087902917149,0.5563399314691257,1.1318751954442106,1.450307723403668,1.3373712088325607,0.019354646552660726,0.16688862099570884,0.8591835180708545,-1.4042008578579723,-1.3705138221317628,1.2302193659052743,0.20481738710553798,-0.051451068598811794,2.443722712628461,0.26422392903134695,0.3695569222713533,-1.0577012599369788,-1.1545012207869156,-0.2421693516742658,-0.15579367149276513,1.508051788058936,1.5016605555128344,0.6057168350974018,-1.1921269670970716,-1.369571990501166,0.7233323844750639,0.6840164157043298,-0.4557260282357063,-1.1575755101843452,-1.3513390168770019],[0.0,0.0,0.0,0.12387703091960253,0.01953034567622014,0.8651847693053231,0.48551865234036706,-0.9285866051398959,0.3206777915642419,2.3286330879897825,0.27167591262160246,-0.2415717819561819,0.01732170444060882,2.164321568512094,-2.5723795695520786,0.6640265744092733,0.9688434805397679,-0.662379968976518,0.049134829517655854,-0.10792219804707548,-0.38797232182096675,-0.2622919077585702,-1.641767363589287,-0.6705372968874842,2.4097530003125303,1.8975936696979356,1.628076100919008,0.2400300635697934,-1.2204786584049467,1.474243139991077,0.20272111961479533,0.1025481494285363,0.012421559461292198,1.6163285550409254,-0.3375136880025016,-0.20197889471175134],[0.0,0.0,0.0,-0.06840529426450087,-0.7583030440572388,-0.08268557677810302,0.643008790362142,-1.8645129769894213,0.0699471644390044,-1.4502946772609688,1.3639451989717948,0.49246200001218343,-0.07013052835686712,-2.162342614443062,-0.5279087618817639,-0.833651108484107,0.5885138318587836,-0.8525571950069137,-0.22522967952994566,-0.6222167627763173,0.4039781335613588,-0.5016452107638696,-0.40418607221634595,1.8113078980506438,-0.49558734918502934,-0.7858028532517634,0.9173076728896778,-1.5566291683552802,1.0936088998866735,1.0250473726567355,1.3741423587868007,-0.7782455183878934,-0.6925141981318658,-0.061391754369353624,-1.5459413239167017,1.6075591960055797],[0.0,0.0,0.0,0.4470314642611503,0.5826983418977623,-0.2138885506570156,0.6441096181613591,0.7515959710403166,2.2234448306312924,1.5517575212532382,-0.008946089518886341,-0.39858114995210453,-0.25765081256501815,-2.3058287854735577,-2.6742369524920915,-1.4185732272893674,1.6189221811419892,0.5142966732719624,0.24550756709006144,-1.7949894415345309,0.05401921309955704,0.29763028973666716,-2.673794569723711,-0.6244666109947208,0.07496748524681385,-0.4102392026375258,0.8314721046769069,-1.1525315595324357,1.4590529965462096,1.139268171606175,-0.4369742091629303,-2.1866064303127115,-0.6280346737916938,1.254755243486227,0.060390784689829724,-0.6535279263763214],[0.0,0.0,0.0,-0.41072642044866553,-0.5695702111709153,-0.6013994346252709,0.3810673196753423,1.3088113887096307,0.34575299260978043,0.8759275464664724,-0.0630546812260869,0.5142086302718157,0.12103133895364532,-1.1770009855568553,-2.3607198950822403,1.9960923613242736,2.695285193326681,0.29794160133137193,0.08708572206413234,-0.19574386924227927,-0.3253261191709165,-0.3881096454008386,0.02479337301932416,1.5026468155516302,-0.7568563525133172,-0.6650049185702315,0.2970635319343229,-0.6143141566449364,-0.7295530248359533,-1.3916448451108856,-1.1793691391558807,-0.5726532063779493,1.8261401679119549,-0.22324622700008967,-1.0836035579256658,0.015332656987196103],[0.0,0.0,0.0,-0.10786906709212603,0.3154254582375202,-0.9291479054504355,-0.15985998391527945,-0.4942364755720434,2.4460661303067663,-1.6925758200056351,2.229827334880297,0.2540861169967238,-0.25533871336351166,1.4027593842309525,-2.6230805104980046,0.7077441834574327,0.18698714772618913,-0.3076540361620075,-0.25266733370830347,-0.3124837184657552,0.022725414017911172,-0.4846999776816813,-1.5196629646908442,-1.187791142366113,-0.7809453413577248,-0.6561654472142069,-1.2224414304403015,0.9161985760093969,0.42734519776738566,0.5904878163275944,-0.12210146149576362,-0.6010838301230748,0.3617678327547966,-0.04614410908589939,0.3338492807723017,-1.14829846269252],[0.0,0.0,0.0,0.25571709173945895,0.12856679776317048,0.42741373716708203,0.8575528233211676,2.8321775834424145,2.720308453494362,-1.4630402847799011,0.26277530568399404,0.06339441608035179,0.1979557746664003,-0.7504647691602546,-2.0007216097871243,2.1263439819884873,0.6655699787597592,0.4977767279504307,0.004348153926619414,-0.3072942366279161,0.11151524929592838,0.03789354945999601,-1.1075806666996193,1.627386638182032,-1.3499087330889272,1.2774630380105332,1.9151372518338756,0.20065794306677853,0.27937136563028764,2.5065547377168373,-1.8631243404751425,-0.8652751080734036,0.8480978907322119,-0.3608440525355583,-1.5197474442462722,1.5392992437088504],[0.0,0.0,0.0,-0.9357585530532714,-0.15497966705893318,-0.01379900368584169,-0.3164598247538357,-1.652283038267542,2.6230708920312478,-1.0763723626813915,2.6779890520260934,-0.23937392724534434,-0.1748032479107094,-1.065325427684375,-2.3968227008991887,1.711298198646837,1.1088960098725738,0.16028046558839504,0.0581931909789824,1.708445448575278,-0.3698052696797023,-0.3243705684822522,0.7833020808076108,-1.1093065966513902,2.464812330061395,-0.20070553075615416,0.6304063351819025,-0.6793217623086187,-0.9451054616088825,-2.6338316050910895,1.4100554309435513,2.048554717708115,0.16292845586241445,-0.7975541886619688,-0.37896265716915467,1.5100358766580475],[0.0,0.0,0.0,0.8693097405616301,0.2961691661539263,0.2821731689140768,0.27742152535329206,-2.046231462898427,-0.03172311157641494,1.5961988445955617,2.03874572647288,0.23504092310400215,-0.07705166245609843,0.06675773867270651,-2.0160242165939053,-0.14512563239280274,2.1926785551188277,-0.5443796212415306,-0.24099814631574168,-1.2894466546236927,0.012675262420216105,0.4299766411067011,-0.8560903801391024,1.1401234746450677,-2.1410899389998255,1.005974643104936,-1.2437580532645351,-0.7635789279335653,-0.7954683190573769,2.1044482706343857,0.78216001481421,-0.007666309249088066,0.6781028448599546,-0.2854033386315997,-1.1542684152308873,-0.9666745581601263],[0.0,0.0,0.0,0.8256491168250285,0.5023246886536863,-0.024356074701565467,-0.25569557031782836,1.9057210252948402,0.8491804938908366,0.6793439225025453,0.8768428329895982,-0.7192757098816402,0.1517911628652438,1.6198872539809064,-1.4673329343770933,-2.04081940763649,0.006064929992148249,-0.027951657334767233,-0.1801537563290867,2.0945381920638835,0.3317943436773605,0.2650252881651838,0.563363906289577,-1.2557663643725099,1.9175086123330707,-0.5777214211236834,-1.1326753692521625,-0.20291927949361788,0.24275491792577952,-1.9529596231556312,-0.38653824786371915,0.8351756108533981,-0.9781845421501357,1.0710348921751218,-0.42813635780521064,1.4447897535189205],[0.0,0.0,0.0,0.715919177460015,-0.3057963053512222,-0.6247344636965207,0.06045825707327685,-0.28556929698537026,1.1345725101694533,1.3382412520828542,1.3646522165006116,-0.41401100056201684,0.09695930065762326,-1.342187046667096,-0.46230356100851644,-1.1304716315437489,2.0696986676108895,-0.8618217626323038,0.019309624964845673,2.0191113010595116,-0.2318755919522843,0.37858110187610583,2.097237321942103,2.0420028903796332,0.6212605118996577,1.1701137031868611,-0.0009638342451685489,0.19933710287282835,-0.3793607631831646,-0.46332626668785526,-1.3591005667104308,-2.385663027412205,0.7178964151245126,-0.7596019735325321,1.5871276288646634,1.163942198678672],[0.0,0.0,0.0,-0.29034346077298817,-0.5585368594920506,-0.7059351589955154,0.3246425767965409,-0.18215906245569347,2.186325220414071,0.04814731575732445,2.2470023470250045,0.3292771829164417,0.21088414625850932,2.1433644486745247,-2.0406775736466054,1.0726689876167157,1.2034801302282812,0.485626020031828,0.25048465226074584,2.288728115412393,-0.29148617657119563,0.40564216693193067,-1.6364572622310403,1.9148219960012511,0.6409620258813207,-0.4266910119534141,-1.8398853425671033,-0.04094882554170143,0.7314960361466445,0.8774892290101604,-0.17855693298552922,0.39777763138173317,1.2453148497954374,0.45283604958809986,-0.7955815608858096,-0.0638139247095939],[0.0,0.0,0.0,-0.3384381036053822,-0.8572929981343554,0.3245642714351619,0.21252387885967805,-2.4762892357118083,0.748550404047935,-2.6740096903652963,2.480613795493216,0.020249900570680768,0.1527420941489887,2.371172749547848,-1.3476201816934514,2.4700354985002146,1.081009868609201,-0.27828073914835727,-0.019711651799546315,-0.8915424223660733,0.30637187371272434,-0.26297244835122,-2.45485130972985,0.7701022779532347,-0.6883286300537561,0.20295899347461543,-1.2442983023669925,-1.0697225823491534,1.4369705259792924,0.22711702730272476,0.2186824072618636,-0.4586308369900496,-0.5653510505796885,-0.1526342687570057,1.611225167422336,0.7773273742503786],[0.0,0.0,0.0,0.28280329893040335,0.6595195601141308,0.2615757683163432,-0.6454722003081614,1.681645979084196,1.3013357261636753,-0.034216374100818125,1.1670477689430216,-0.36345160319295167,-0.09488555724973297,2.130676239675594,-2.3401935497794053,-2.507069935923152,0.5041921061059722,0.36547501423360507,0.22578316290893552,1.6096752997969168,0.04565409954908839,-0.12382811146186307,-1.435257450801731,-1.2937436671708475,1.9211855467884758,-0.2891826746165087,-0.7088381757721463,-1.5265422579154575,1.456576529657956,2.3983217036625177,-1.914401953077596,0.11063870930389763,0.8384365728255054,-1.248400089911423,0.7147429860432959,0.013724575766882019],[0.0,0.0,0.0,-0.856576277430812,-0.04121006644914245,0.16432181657193162,0.487418867058437,-0.8114110363772753,0.7218193835928745,-0.9721544619038902,0.3314239171590421,0.4364617966603018,-0.21485327309585558,-1.9787176095948034,0.4482896234674021,2.034717884075529,-0.05499945892771368,-0.6188162007329008,0.06973380247322825,-0.6480270580331142,0.02247249470762258,-0.32145001542666285,-2.4118515603619386,-0.8078701710214249,0.5261963685390558,1.4134246633000929,-1.3152599202213162,-0.9928432841179464,0.5600685009504505,0.847300084283416,0.9129965032410325,1.1537699591116257,1.418930275776537,-1.1680660786107726,0.3872789471304594,1.372110171008323],[0.0,0.0,0.0,0.036371411092048174,0.5147265105932718,0.08609619048449613,-0.8522447921411459,2.755293241207244,1.5501648357236997,-1.7195979991114159,0.3717654160504739,-0.7557742841306758,-0.1694643858159592,0.8978016139688276,0.0686401260743339,2.4266099847366194,1.0732292545019662,-0.38311273873973267,0.11008453538448776,-2.6152267712486577,-0.46840881453866284,0.03500938357844341,0.043752537752482556,-0.6306531995206853,1.4421575867965308,0.8829263537788752,-1.9481627660348655,0.42554004837320325,-0.7302011577222566,0.23072956085036278,0.06875331937267681,2.3334028309035353,-0.8575096963351121,-1.1852774950448164,0.7598606841956805,-0.8405118188787052],[0.0,0.0,0.0,-0.2022061269012739,0.8664679107363389,-0.18309422437974907,-0.4181178624581449,2.107900460595309,1.2794948962843558,0.48601745798638474,0.08179064482306746,0.48603066093350356,-0.03763278415085139,-2.059838982233966,0.32682450415222464,0.7513122721960972,2.2997358963361134,-0.4223093647115032,-0.022038078766091568,-0.8037586018943237,-0.39693152324799325,0.3187433731759933,-1.7240228350019107,-1.1679668764859412,-1.5153506390862284,1.5621715079359497,1.9133530475179459,0.6207883561709873,1.4079001448471014,-2.1272387831361406,-0.5880035434186017,-2.219362644068445,-0.34189418373655023,-0.8787558962032922,-0.41168589414580903,-1.1832143612436377],[0.0,0.0,0.0,0.13468803500583593,-0.5917378683782779,0.19810029277359661,-0.7697153385222448,-2.0612138202823553,2.1797972588363037,-2.6644605652239934,0.6336731557657737,-0.13753826620099596,-0.2258707293111545,0.4875664246622722,-0.5242276101237313,-0.5699192291640323,0.06646345217840009,-0.7847860331171215,-0.17252415930085171,2.3222412150506737,0.34382075205722495,0.48638187666071553,0.5452719127034689,1.974347689339118,-1.5535554995834127,-0.08434479694280728,1.953815540850158,1.133300145282022,-1.233334850382046,1.0852652168952401,-0.2531191618540545,0.3968057716013047,0.030849439885405783,-0.7772780409441231,-1.57954068907411,-1.2105897633436853],[0.0,0.0,0.0,0.1356423397585882,0.46736458947567994,-0.585140250514554,0.6486774108883474,1.8988252127392307,2.4841457644093565,0.8921020286264678,0.876732854476696,0.2947916538635956,-0.0676608393967219,-0.03282722609778732,-2.892913712446632,-1.533449743315042,0.6402548657961233,-0.6646312341565878,-0.17713876568046705,2.3574876105939375,-0.13900682560038685,-0.3634259621185538,2.0114373680609905,0.5198312688233147,-0.6134032410522092,-0.18993424899062272,1.8287961278124985,-0.9904044185877671,-1.034745971022535,-0.9172614718687839,-0.5038407162848182,1.305924280685777,0.7258149858359708,-0.7520628451074436,-0.43378093855651945,-0.758878508515994],[0.0,0.0,0.0,0.03447141577327857,-0.0661730085630664,0.1417092568305134,0.9870923669854011,-0.8176454704792997,1.6338109037170465,-0.9640446122196973,2.547916218166536,-0.011024502243594636,-0.11590871016705964,1.3234354990686277,0.13488322897125515,-2.7062273114567836,1.581900843864413,0.34024264965328854,0.2295683006805534,-1.0640876709787905,-0.224572510152811,-0.451120982705649,-2.6164700173174484,-1.1044683550611605,-1.5946673263012061,1.9067486932229858,0.7243518102605493,-1.3117423681346054,0.14452275300279038,0.8250273459206907,0.9329959985524421,2.2031037527161357,-0.3448583420265878,-0.8204107338321607,-0.6414482922499435,-0.5806704221464953],[0.0,0.0,0.0,0.3069668927217551,-0.8778729632460858,-0.36567187459623185,0.03734256691183191,2.527019672523387,1.2387717751361431,1.0062261433482873,0.9141898929846153,-0.4726377951691134,0.111424312667744,2.5293800876726302,-0.434289809077415,0.5432180947161016,-0.0752611673987877,0.08180956049897392,0.14963524285875185,-1.327673915798121,0.19829906891934412,-0.1484542830410855,-2.02081925050512,0.18273837367466372,0.3374710008596309,0.6412665580967438,-1.2456567119145177,-0.6214874016265093,1.3423628752634664,0.5100632829420371,0.2863304858319826,-0.013014449136853123,1.7630242933701517,1.791321210855942,-0.6854955550772183,-0.7691830040973225],[0.0,0.0,0.0,-0.28529183165868816,0.7359160085854229,-0.5168991832980597,0.3314384307866685,0.48036514560108934,1.825719176492563,-0.80489148768077,2.7079228668928086,0.17897611187164697,-0.21468511301189788,1.4210492041467857,0.36541362100436947,0.2894781641249238,2.42227195904287,-0.7208681585335329,0.016183289296604952,1.3950641526173926,0.2569809014648241,0.04620827845246278,-3.064095626101485,-0.04644180279282284,1.97676089692087,1.2791015567392356,1.381894319892878,-1.1284576178230086,1.127421796159083,2.5579205803576937,-2.0488867502177395,-0.5100451367650014,0.39891751355919647,-1.3626313751971784,-1.1945864890114666,-1.1337414852967593],[0.0,0.0,0.0,-0.0021031764122114325,-0.5068291334561768,0.8595224353755723,-0.06588618379512737,-1.6854242453729449,2.277504325490409,1.9463973295510613,1.6895006510693031,-0.8011328789003458,-7.16183124583325e-05,1.726680365182705,-2.575313523949977,0.7181138244863927,0.07282752022882562,0.12621281270774987,0.20427907039555127,-1.5664523125350323,-0.3565056704427664,0.09556449669016065,2.0371875176958314,1.2063385116493195,-1.6289269021985895,0.7097149981396051,-1.5494300161141144,-0.8795616021634939,-0.7513656380853103,-0.2514299712621062,-1.0795484185499906,2.2066248360467022,0.4009844967202363,1.3068725804079073,1.5001093608111795,1.4716024664493037],[0.0,0.0,0.0,0.36882286105706147,0.6627582464593187,0.25561609847849304,-0.5994844569381976,2.8770463115795724,1.0985878956705966,0.35964810615187126,0.9942854089404018,-0.3190726262733804,0.16443338204114988,1.1651175900969064,-0.6677342209578332,-0.5304594553710027,0.5521631294312352,0.185047959081592,-0.033249795479915606,1.519357106288394,-0.031309399953174555,-0.36748078765056474,-1.0517967656813387,0.3451680618347941,-1.9814690648668984,-0.20991256072424136,-0.6479604401007986,-0.6502269054500038,0.897379895785644,0.45082732984939256,-1.5362867425078433,-0.03486449283689996,1.8649098047923143,1.6907246129174722,0.05218307619652518,1.3426183717404074],[0.0,0.0,0.0,0.5501765881310782,-0.24835384542422312,-0.6037803581843577,0.5206489877126583,-0.8588843081577748,0.33070355240656735,0.5193908366452686,2.0651692345863233,-0.5422029499495631,-0.19970665151491135,-0.7095781721203016,-0.14182470897218602,-2.0724806948996095,2.252559405131263,0.08299051789347833,-0.007960027862373209,-0.33900043474644237,0.25306928429435094,-0.38806297034384396,-1.5507426247189415,0.7825167005902065,2.0829961810677085,-0.13661999842686623,-1.3462019523849027,0.19843821255286986,-1.348476317596474,-1.8562198479179532,-0.6589551527388384,-2.4637346063549477,0.10576218640878321,0.40797164868165847,1.1516095453452422,-1.310477343293717],[0.0,0.0,0.0,-0.4821980934702524,-0.8758856526712696,0.003137015234647544,-0.01730552610282288,1.2745472916124125,2.238808722311695,1.7987556473809594,0.7837213290013131,-0.6512644846759663,-0.16334645194827863,-0.3077797557525286,-1.1719615007251845,-0.9741817191145574,0.8302962614891507,0.3271105695532003,0.09543312020885403,1.3490465732980623,-0.2461865659930048,-0.04146178144422141,0.9512591900916485,-0.8255296522467707,-1.5861595013484022,1.3981889782675474,-0.49093096076930953,1.2157423738511908,-0.8575559024273668,0.7494994300918751,-1.1257416395039102,2.392950131578694,1.645267044576561,-1.7007042375706432,-0.7910807901095753,-0.3994139788268132],[0.0,0.0,0.0,-0.7447568146352631,-0.581555638025604,0.26556772131104156,0.19132201214870762,-1.2344504267645875,0.4769082360461617,1.6574836160187854,2.873083951834746,0.11302516344436908,0.030567736623401787,1.1298467203424822,0.49665094804114407,2.629388307156683,1.151692571584274,-0.33103240880702933,0.2064096577330105,-1.3286376511981068,0.39512615218565006,0.44083412447118564,2.348421108234642,2.182468424466427,-0.47895154961687947,0.5079052637051251,-0.6089128138166722,-0.1903959090714693,0.17091596566538603,-1.4487686356296101,-0.7344607913567531,-2.1429567330926416,1.714972458236297,-1.8136787105142849,1.2859804190530466,1.540493506522142],[0.0,0.0,0.0,-0.2737410981380604,0.38354177413152074,0.8063723098298522,-0.3573866486396536,0.5136582184243852,2.401600965855456,-0.9841241202249806,0.14679006934326527,0.02664072070987411,0.10051150967858813,-2.505062565055145,-2.828702547145375,-0.6043012686204503,2.709612442735168,0.06995999917615114,0.255399059701564,-1.7119555814245369,0.5163059783830615,-0.5089339489744972,1.3345339468357889,-0.7041370911691202,1.6048690342502065,0.40046402096409484,-1.6155763930757976,-1.2178582515544962,-1.0883060570476895,2.32336668473706,-1.6745515171236902,2.441293682278674,1.1790050132423928,-1.0487213582036534,0.17726529696920523,-0.7176103413088061],[0.0,0.0,0.0,0.3560707824352515,-0.27789606344378404,0.8572336518852809,0.24726067599564114,-1.2763576831058934,2.7431614004093348,-1.1571281279294454,0.34730219274278207,-0.5095323895715995,0.2303106486467965,1.9989003438473252,-1.5571437100601415,-1.4747580576927561,2.199931784947385,-0.7677271551479412,-0.25081041777122814,0.9089953301752942,0.3381444334873659,-0.11244943841238086,0.6245379203057806,0.4346720814563203,-0.1946183310553562,0.5786627240778852,0.978861872896156,-0.017276024385645128,0.2693763076359368,1.2342430832907638,0.3650280874666305,-2.43919395299781,0.9507325692064355,1.1588316700039591,1.2967484420439852,-1.1252887867704895]], -"gravity": [[8.781495017406924,-5.705106343325246,-4.279678886343577,3.1766932223408944,0.03006450471592875,0.04670060286985533,-2.5826733255231216,-10.859025649928743,-2.208588549061563,-2.6295860459078977,-0.22643344227754653,0.02957879070925201,-11.158218054345202,7.449301168715846,-18.43671577720734,0.5790320203442015,4.32044501760318,-1.3185306794121665,-0.6779377595957256,0.2996913732746609,-0.28611412944668624,-0.3420283672723795,-1.8949712008294468,-1.0643307934887092,1.577594110580514,0.5252498568610322,-0.1587725537350407,0.9237280366220654,-0.2910388506455459],[-6.73839978388238,13.667774208590442,3.627855091743884,1.6651356564572106,0.2113953197915017,-0.04960774006537565,0.8885911387684057,11.623533347908955,5.4745315896843785,2.5001023424935678,0.11510998183520832,-0.07366380510568016,15.451616483992087,3.3985378279153835,27.528033690629204,4.259601507135638,4.385267897581565,-0.4652013497298399,2.6080795226751583,0.4671866592372829,0.7280636933743133,0.3115032523446416,1.5371261874668363,1.055082681424001,1.2864307490848974,1.064915600090519,0.4004794167087502,-0.3592936425109594,-0.35822771793089736],[5.255661457572871,0.5303646249961751,3.157446654798786,2.8189404083363554,0.10077908272934481,-0.06323283938786416,-1.1107022694085256,-3.59700033728174,-0.5018001377385561,-4.179726115279891,-0.1711892365068895,0.009248602343927713,-1.6739415471272319,10.040744295168883,-34.20686218709169,-2.399360018121777,-1.0952011776525803,2.6484679772217685,-1.4675616796632063,-0.2968196935940761,-1.007656566394535,0.03392571549527801,1.7734049159210767,-4.4803582182374235,1.8811691918693176,-0.6546913432917804,-0.20307718367281524,-0.8128874515986901,0.4466067232282486],[-1.4768145749431167,8.546484469967044,5.176400632970533,2.4955664990598487,-0.014107075921909022,-0.08316306693593505,-0.9778427907225896,10.115525841537576,2.4815310983841443,2.751395682465233,0.05623450457653215,-0.041446654215158196,8.955110663826883,-12.82575576858123,-31.249091479952362,-7.652243206916494,1.4632469661737935,1.4373800309007003,-1.273437792115324,-0.6557908330172528,0.7859481624417426,0.13616582387633094,5.073924647849901,1.8639201803073278,-1.16179739280166,1.5162325938251846,-0.3522820957096301,0.27582635626810836,-0.1431640002531146],[2.901518964976411,6.4250853013122615,3.156720660157541,-3.656162461453509,-0.140550001009766,-0.04957755657044263,4.246957223044756,9.558680380599522,3.8350083219256073,1.0227625585121032,0.15176529864784713,-0.07185395802471602,5.82862243259038,-12.492880172460593,-23.22430499970899,2.9356018724937774,1.69220330640075,-0.6235808114838594,0.4724969687642441,0.18053952506930426,-0.5948650812974022,0.35151323251453487,0.40566243566424953,2.9347485722432958,-0.3380603003513994,1.9563266440241012,0.6452147563170767,0.09549658880662838,0.6893855762659327],[1.8080381435086905,-8.550449195488351,-2.6523932292043786,1.3195976992214193,0.03474346308759832,0.05915210215558362,0.6686801709382139,4.435022612426185,0.20107731845849777,-4.255179355520557,-0.12940893029957434,0.0033872477226649816,-1.1907086535085027,32.90628703938809,-0.9015634304210132,0.2159045079811967,-0.07774105938552059,0.16419493245255443,-0.2728833713784066,-0.086259384562878,-0.34547248399561997,0.14977612597987813,0.7347776660340155,-3.7510567202438727,-0.2842637158944805,-2.581640440426242,-0.30016717866357595,-0.1579101764460376,0.0029822898785071238],[-5.774613121392449,2.1743177163374265,-0.7604932738307727,1.6173849098944364,-0.010873794618848093,0.018430088408800244,-3.5210299981851003,-4.834241120688657,3.1910915988206234,-2.935520053699716,-0.18777035300115882,-0.06701286294630784,-7.433518531606937,-5.4418787202547145,9.412520323079743,2.735220115283015,-1.0947399141908625,-0.6644795278997395,-1.9136262562883644,0.704212785698549,-0.015146635704298292,-0.7172888080660339,-2.740370238053183,-1.517205036843726,0.31492777287926044,1.4457865407967294,0.40605545102909096,0.4282334867424032,-0.06718774220456356],[-4.510208084117235,16.916272172876518,1.4664507394880328,-3.739106511765864,-0.2380413343220419,-0.01748059863035526,-12.241858857262937,5.827690322239998,-4.253493129886232,-1.2454272617705482,-0.1296521794434093,0.05498378018304363,-0.04658228731234768,3.2642431992759975,-1.2560338802935251,1.5446574011937009,3.3436066970313427,0.08166425961678106,-0.957693698454509,0.16009789109657202,-0.4172112694874222,-0.07126331004644386,-7.561512829814149,-1.8120174627739773,1.3855823198668742,-2.115243232480303,-0.7246120860126072,-0.7908817953598829,0.51401724159812],[-0.13570982387672262,-0.9156261072651892,0.4615095243145447,4.361558225088809,0.09134501811977279,-0.0011426449673575042,-15.013708272239272,-2.1793844427655658,-5.594493714327532,-0.8281580648279376,-0.15550621742633408,0.07708607376538311,-3.3780751075554276,-27.328558433046734,14.369718824063035,-2.701502961027259,2.4451274337071904,0.14670752380572594,-0.2568216915241253,0.5506556928203601,-0.854031030695219,0.33160706617834096,1.262488743290653,2.378705864813811,0.9971340108450125,-0.6446491732072788,-0.2750175422651987,-0.7865594485996464,0.2113181882496405],[3.4829146185827478,4.851509468321146,-3.1959919261945133,3.774347442022702,0.1321108157992786,0.046028753344193825,-2.8748559353818544,7.216702729143295,0.5683276209604395,-4.412666499065141,-0.16842282799729402,-0.011344675043847228,2.684770273269174,-24.089667466039913,-7.307454099600713,2.260235780318406,0.6680756197761001,1.032113058071962,-2.040028259070047,-0.40078638765846397,0.27568564788714284,-0.20724109789294626,-0.27299334754152094,4.058854858719038,0.63598830656083,1.9155411628072985,-0.15716760527521417,0.08032049874817421,0.3725112593883383],[5.608384650028313,-3.340613943121027,2.1218441683729146,-4.334836617870735,-0.21659948110432953,-0.02819037152599462,-4.96464028698164,-5.712631123410091,-3.6895106916024867,3.6174784009521055,0.1751956545956514,0.059544265374579054,-8.322484060835983,-5.52523361831624,26.525357701592483,-0.13476614562355155,-8.125675060881655,0.08373939109556881,3.022520668803325,-0.12528837899668754,0.01810202556210237,0.7348084302072706,-1.9550832161112093,1.7412897366611715,1.289482223940305,-1.17179202384634,-0.45299992813472434,0.4537473675205279,0.6165605407265187],[-5.890310257096074,9.414763047157704,5.235587181819844,-2.2791183916199875,-0.042644718706801296,-0.06899494984988497,15.991257493818846,10.024776951906428,-3.9124703462122086,1.070188090096181,0.08353294689638541,0.0897841652912151,-4.7481508506748415,6.519632090171671,4.168103730043814,4.2310367095713035,0.3889721275909448,1.3737786381788157,-1.6307134351129615,-0.9274334384379039,-0.36806522379940815,0.5104662172859131,-3.4316722458507685,-1.6175434505629698,-0.01960448388495388,-1.2342084409082983,0.2897133743813555,-0.5337939035916497,-0.41953760731962275],[-8.215253225273722,-7.121396467747321,-0.4085731077981708,4.511196939395745,0.23944479406166808,0.0171003343269116,-5.875737103000632,-4.109935771958493,5.282086770402235,0.43998971077509264,-0.03435625263932164,-0.09233381294574348,5.496909129502245,-13.053315659570563,-0.25993277249507285,-4.886685768656053,5.726701696782572,-0.765753041463067,-2.263380836449526,0.7145488833409734,-0.3858949505625017,-0.22612407431573872,-7.938453076250677,2.372857184998795,0.693266796614967,2.0992396981588306,-0.6058947873121876,0.7481383113923102,0.4428036217056408],[11.05518490243716,1.546571882620154,2.424664686493193,-1.1370478027550397,0.11529381963020828,-0.0635638749390858,9.241949952488094,8.895883507758612,1.1828288748017928,3.51532650114062,0.2204353470377157,-0.016806553682457932,-3.333948390845995,-8.279275909255452,15.555790328028433,1.881642588020874,-1.8030989500122185,-1.2792057938216999,1.7063650120369402,-0.32790018921113206,0.2958148400933086,-0.5898103596208871,-6.163621420890907,1.4910863875827187,-0.4247192648408018,-2.2933852489468984,0.39136188234212177,-0.6692824375769116,-0.31654224334853426],[1.3381275558158623,14.966810449292344,-6.007996921766756,-2.2643678653366246,-0.07608518746544868,0.08122028322514836,-3.1250524741415444,-2.4594288904855253,1.3034134545134952,4.00276451669896,0.1208390513026906,-0.009167240420394344,5.341792690092283,12.24444472607175,8.534137835874008,3.3355754505990314,-2.6948012365594365,2.0580305487021975,-1.5250398351102137,0.8843155790942114,0.7408437671932441,0.505027223175774,-0.9167288756441957,-6.985792379406327,1.4396650495019037,-2.110178597924881,0.6795315569385696,-0.4259120106897607,0.6366676778764541],[3.267682603739907,1.7550552355929439,-2.4733493876775055,-4.118632387462627,-0.10954008543527186,0.03540812821458832,6.826517383082838,-7.299327038422469,-1.1807691957020483,-4.185191002492733,-0.03974398259936397,0.01794418761116609,12.350572072233051,-19.963233686625475,27.89910810534282,-1.3400099869809348,4.98948089218783,-0.547091175034664,-0.1492470783498656,0.7674317110272872,-0.3581358096161371,-0.6471950235073505,0.18530052243988915,-7.321154792836008,0.19248267439299288,2.42637378518079,-0.10908051113171473,0.4144262264133958,-0.11914829488722843],[-4.482851829843599,-5.930199050091311,-7.166871168811099,-0.4022129453203079,-0.032596517611016895,0.09749628644119589,-7.954483966829202,0.7825843334500883,-3.406992670527801,3.1865247103189267,0.042455385477618804,0.05315747424257877,-18.57408933931433,-25.04340432886888,-3.7748769655954657,-1.9756551835166514,5.009877547202596,-1.2764268583715923,-1.6368265067931749,-0.17364668834572153,0.7236947870892578,-0.4248825134049796,1.0846163296350875,4.046071291063613,-1.1726532095346012,3.1001008374461625,0.42837558501115586,1.0821392672941887,0.2115738921083083],[5.074145375159986,-4.08484819913097,6.315798386398743,-1.0128722591567259,-0.054979273877891914,-0.09548435405337234,4.713257326505597,7.131182721724299,0.9214066625041156,3.7218062773770413,0.025052129711289315,-0.006157069261111692,0.2879987961044618,17.67772148709909,6.442798279471302,-0.6685460815581347,1.232250474904557,-1.0285580196680797,2.0257065789660818,-0.6281083096826368,0.9633173203101209,0.2861307631048834,1.003033781257977,-2.3061781556962377,-0.35265468929749494,-2.3093821785701634,-0.5567854370327915,0.17747098793195754,-0.4977477493085202],[2.7681011500604114,-7.0255529732481214,-0.9878374612517833,-2.1630320295559335,0.13685004057990746,0.02928443207884225,14.378832570619767,7.8058088512458745,0.24184115412899027,4.007196608358755,0.13560280252237528,-0.0061578684915788075,-23.17092036276537,-16.641863195538804,-24.146269636668862,-0.6960734660061689,6.894358989570712,-0.47989285473086085,-2.5526580999838964,-0.7487990443133733,-0.5402605069871941,-0.4955816560365214,1.4504809801518637,5.256733381194581,-0.17066576075933285,-1.6950611790883163,0.2863691730455546,-0.20643838563336359,0.534475846225293],[-12.639596444697059,-4.987781320137259,-0.40422228633021273,-0.6073881106946324,-0.2467934705673866,0.004354844394669689,-3.139735601815172,7.15287337532449,-1.2926949066995619,2.969810075473907,-0.011109134519134999,0.01311122277745342,-7.979658325118854,22.221051450654898,-21.044603441572708,-1.4184370551674745,-0.21324813528038958,-1.2914899305589187,2.866198630282763,-0.4931538355058295,0.7610052285034276,0.4320334995845537,0.5854865981961446,-0.8394272970160579,0.8608292321318415,1.7629585410126913,-0.0006405751059276427,-0.43531169357421745,0.42440749533288835],[-7.379054546974794,-7.490481763165838,3.6054496171280443,1.0076879330630648,0.009040990042127934,-0.058183574553111586,-2.4123507274468756,-0.5621740165363226,1.3731876410778292,0.4159118646760035,0.11086752138627769,-0.02473428587741636,7.570891275860677,24.229508571928264,-17.526919281307478,-2.9640707821540437,-1.2852187006845288,0.07131235630300348,2.1094372766038445,-0.6468794242624174,-0.057512851595016956,-0.20028758260623739,-0.27559394083019684,-6.2026764243824655,-2.550443411888207,-1.3979846997863883,0.2898501746262797,-0.14150308020007982,-0.36650084065620847],[-0.49093063367588247,3.569214685608957,-0.45666605979510894,3.06250332460929,0.03847171219515692,0.017249435356076526,3.231360717336823,-15.462549517917724,3.2001483625456864,-1.9747507140915932,-0.17702046369525637,-0.047298163603358485,-7.328357583612918,-16.887840460279584,-33.32050603719882,-2.4980459860325053,-3.4828790640694227,-2.3115593012691202,-1.1487247542680543,0.8538058654530594,-0.36677822969414753,0.631956502226671,-3.4332564805076093,-4.104487945141903,-2.1979977831166653,-1.0026973269278123,0.5615004596178822,-0.32869646810648956,-0.41066190889544024],[-20.186882056258824,3.3985008013084883,3.900247637804437,2.3537776494653215,0.08631064568093068,-0.08302071431798372,-13.851332663376109,-1.3074745789695446,-3.9179527091436954,2.63540583667701,0.17471385597283337,0.056505675208087254,-2.474988519863001,0.3087611688366836,12.788604458206457,-3.2585072211973722,3.4175203876118605,1.3443583710122187,0.45758008927470334,-0.7002663547685393,-0.668360283629869,0.03886788889500767,2.308952197865529,-1.0509689156478121,-0.6292118988735378,-1.8669551916985214,-0.2579610485658751,-0.36625437306155684,0.3930483120240448],[9.24366536797695,-6.272274322381279,4.3921467997568255,-3.650508270290644,-0.1931419054319786,-0.05939913907823684,13.506886454252092,3.5578638649244727,2.11387482057606,1.2697001424264238,0.22409725646808207,-0.02565821506598655,-4.146391619594163,-16.401647855336094,17.02926109028806,0.8441923958428069,-2.634314210251525,-0.8929092709398434,1.5263780437407108,0.9515121309228223,0.14468721130197182,-0.5121235083878438,-1.2140607050455194,1.035710659329225,0.7862522048813132,1.7896810923756172,-0.8409375544826794,-0.0204025391402222,0.5519253381114216],[-3.9296208840733544,-12.197015424273374,-2.537675425143961,3.0022454268676286,0.08376798954802052,0.06340797162068379,4.100642529195812,-10.533224128712176,-4.503445565921329,-1.3046567672292817,-0.09315525185612754,0.07570457644725781,-7.4173579886101155,38.409679906792825,-4.016817213562444,-0.230282428036304,-2.156569332168471,-1.1684466816659074,1.9118035403363585,0.3519396597498571,-0.05872453353594549,0.08986023777855844,-1.4320771051459718,-2.2691118420961174,2.5181541701615986,1.9402956761412185,0.35357075461561993,0.9429075680696033,-0.10597530544195381],[-6.828977912735844,9.819686890332974,-0.3557729977410615,-3.0713905498752516,-0.1875922930714017,0.022768567452208954,4.221830848939264,-0.1425582045744792,1.5096771641896618,2.3387722468238525,0.1827915416924619,-0.041816259555654454,-10.019396557440958,-17.745006329716272,29.15983900869724,-2.6526486896206434,0.7831858700006094,1.8137248758727889,2.5320481514513755,0.3560419447548949,0.9007447014160408,-0.05346477592414423,-0.35996219899674636,7.957333749854675,0.5557962002114254,1.178031848552414,-1.0470003862890875,0.30278680738920194,0.734280078307389],[6.2988104718417635,7.41420718718998,-3.9347298515540055,-2.402523916785496,-0.07017678150135258,0.06918020519483026,-8.223233899138203,5.773556229245147,-2.409940788075419,0.5045097078982947,0.17480320049583534,0.07270293389998267,11.204937944718795,3.532722144829636,23.651126552617306,2.2344285274029265,3.7093514195383044,2.90724320775652,0.8714200854431713,0.7236817521161384,-0.3290343788393479,0.6034268131619277,-4.801185129039322,-4.710967796590012,0.16253136355150852,-2.940244459584305,-0.6746501494238532,-0.7949050599245308,-0.07729498836496697],[-5.755887189951388,-12.226274501642921,2.222860209754508,-2.401035313938663,-0.20227499544302768,-0.030674946882387904,4.279277633416629,-13.059000677483837,1.3274212376232148,3.881142699348801,0.2255108008289964,-0.020352965391795378,1.2852476515937277,-21.33104757597895,-9.185648607742031,-1.6735661103685788,5.758397002199771,0.8568891872487009,2.270868245551364,-0.24679036632346651,0.20672022238129012,-0.6956864301910535,1.3096543449306133,5.364715580166527,1.2134025878978099,0.9391370620807183,-0.3944499897219936,-0.3866606000011401,0.24971269705157992],[-5.829453725782758,-5.7140054909309175,3.82527494121142,-0.9139581757403894,-0.12397227836286542,-0.08929306095056046,-10.432713174642833,-8.573135109938764,-5.885491277563151,2.6168635832555545,0.12924676166007637,0.07899752115277484,-6.100218101060029,-8.05257306864741,6.091878569981061,-4.658981201798576,1.870399311344705,-3.2535525337765208,-0.025818800655699435,0.035221784215401626,-0.6175330986204158,-0.2896033857393283,-0.5177599755399706,1.6982125345900125,0.6559806082408843,0.690496090753348,-0.13100013771182215,0.154486091862771,-0.5488327255696451],[9.404053256097047,5.95418150399293,-4.843239651818824,2.3476963748657838,0.07710604759885824,0.06889825080770168,-3.1835440280606915,4.8058535603285195,-2.5184245001353776,3.518651190261643,0.18099371962243668,0.038943999023482875,11.031656589647497,-19.908362519847174,-5.5933633513800585,5.404288960209379,-0.5367904174231759,-1.7455548330102717,-0.37968615888611945,0.5753443354829489,0.8360890307058066,-0.11109745768972012,-3.3253321237747766,3.4004293187461316,1.7020125950841498,0.6183376788658521,-0.49861057973952155,0.6623744383866552,0.5199898028147486],[-11.635283880133064,-2.863305189757938,3.5746607213772688,0.5207695730231954,-0.1906234448368525,-0.04139609549630895,-6.562959512849306,-4.1022217983454725,-3.0204253853900402,0.9529267037040619,0.10989388687871383,0.09118844793747283,-5.506691573500203,9.089316525899294,-8.666302940903105,-2.058107569536452,1.1847889990310416,-1.8378017980584476,2.705181170975598,-0.24111770279058367,1.0040446645198284,0.3792153708458086,2.105186085972431,0.43045206459285973,0.8664460857144364,-2.0561509499865207,-0.2871738599732118,0.08818153837353802,-0.029688492521931616],[12.633037266142173,7.706404750229841,-6.808947913401657,-1.5882084598274098,-0.07361808292258108,0.09015907627173465,5.132384851799409,6.239330906432985,1.4180157257502495,3.1011917607593755,0.18462874431473317,-0.0210010425553323,1.7617202613145575,-18.581502979889716,25.48698912811076,-1.5301889333719068,3.6862583556091892,0.828705441211071,0.8523766880050307,0.26238501742872367,0.2288095709638185,0.6809535593097525,-2.708502518271676,1.3739543577948656,0.486029346880557,1.4703663191724996,0.4139575035618394,0.6566314965350158,0.21014743234638333],[2.5215334124981874,-4.072375865815306,2.7444547057278323,1.1521054969316284,-0.0028040733469032087,-0.028216197852931238,-3.524717226543572,-13.834896451469726,-0.16004365682087385,2.0826039626423904,0.144652997038582,-0.021226565360885657,-4.700359514804184,31.517671705740838,-1.2947414663290189,0.35340931938751224,2.586024421075372,-0.17473378384270694,-3.022470411132623,-0.098298169748748,0.30044452974228675,-0.7244752761061674,-1.7814121318547111,-7.509144556763153,-1.9210061049837222,0.5584373788750494,0.823326479727887,-0.14816031305275923,-0.718146054544368],[0.24203800430603062,0.744770364320119,0.4473272811627006,3.8541166621919487,0.24534566424417978,-0.016396091382942502,-4.066588783628367,-7.709043177025427,-1.5453311491873603,-1.0183102657522967,0.14378149378685243,0.023770059827832556,8.701870407722188,18.490363010939642,19.49637565432539,4.32817808331032,-3.9625021914863794,-2.200212860308473,-0.7731888952259413,-0.18465137752765917,0.06404315611811581,0.4726805523984264,-1.6300968433749379,-2.5322013157270393,-1.0554821921100002,1.2558832798028647,-0.6161377614075805,-0.48810392243332945,-0.54297208074735],[5.450047297489621,14.304328355792459,-6.891314812193887,1.1242700514450843,0.007177197941896418,0.09136733912773995,-8.059531656460297,-13.60010228819938,-1.8961208515993535,-4.243652469017941,-0.21295291346309347,0.03641264312857983,1.8056217957870504,31.221116497970257,-1.8217701725800417,-0.6365244220425491,-2.638923224628624,-0.43017450135797963,2.0206531851375047,-0.8215045458608423,-0.2362149437994206,0.6645481190079361,-0.16731568811117895,0.06081565423755497,-0.07032316806766215,1.1323778635184603,0.34059318132339816,-0.45021314023054004,0.29668946065138724],[6.537267692696362,-9.82412322381115,4.119054043202208,0.0052560531901906585,-0.027920471524347235,-0.08878474693192598,3.9137504536809176,-12.392517601970443,-1.5084135278655326,1.8452061358784804,-0.13992687617399163,0.053421693617585274,8.47867034312203,26.306232811348476,5.84335533454847,1.6797102944083595,-5.313940113072586,0.3906446897964977,1.6055683443200157,-0.684384280212513,0.9361211012540427,0.3642920695301578,-2.5637793833485056,-4.849317615634124,0.15872923550750423,1.0185226783113581,-0.673975453018299,0.5656006024137201,0.0935338460770381],[-2.569287793163429,1.1917481079152281,4.916057649188443,-0.33464032533956617,-0.006816189284581091,-0.09561290139765431,-12.292495805598897,-2.9497028152694984,2.1325664822451436,4.160767597441051,0.09540936416808334,-0.02442554341723437,8.227870469319944,-3.6575842200634843,-18.26921945526673,5.72016148704046,0.24288185112864472,2.091386813525468,-0.6644738241608464,-0.5392968681246233,-0.37526423207241705,-0.6613591202959125,2.766486116067148,1.1678825918602451,1.9095606168628847,0.9190604497773049,0.11941019335857042,-0.8207690963446477,0.4094951296347453],[-0.17775518844222593,14.924582350850732,-2.5397884317762376,-2.4087885050333417,-0.14966510796892943,0.06064895552602942,11.664648980763552,-1.5190378940305205,-2.835384092123836,-3.3958882920144537,-0.011981708904688635,0.048794809582033624,5.805472536653726,-16.282177255874686,25.61080506467752,0.41536022867018246,5.49998436899092,-0.28752008348423674,1.7486660372347298,0.23212968545204257,-0.11660318148419206,0.4822414783629272,-0.1533501936111103,4.127574216187021,-2.4010611276396756,0.40191815696369115,0.6113832062534741,0.3645826994796026,-0.6706881654892967],[3.9904215454301717,11.576437819308325,1.4845821754438686,3.376641206008398,0.2364540179989651,-0.012810874094057578,16.646565802462938,-7.204225204639873,2.955235852009262,2.203805558288206,0.10131704496915521,-0.0787121482495612,10.372727056836123,-26.77405347065599,7.897502733293179,-0.7867040473855582,1.6301234142736967,0.8076379112206945,2.280503981619385,-0.6518972712382722,-0.0636140279713168,-0.22520437524183898,-0.096232760392958,4.333301181039498,2.3628764545663494,-0.648708647384691,-0.9207977836790778,0.1439257718543931,0.7433424968850594],[-5.523161937233965,-8.244402262915667,-1.233575453179202,4.42712612479029,0.21944541352017952,0.018772227329200395,-8.87753497092483,-1.1273303568028943,3.7416506407914523,-1.4764473782347038,0.13734655535297363,-0.04773137743879303,-9.942392368055733,-21.46997204320675,14.85040957280328,2.2126067007453063,4.780126966775618,1.0568999296170174,0.1151598058888546,0.24316927026006077,-0.33985430084088575,-0.5426665792686391,-6.547028539111765,4.123713941233513,-1.9246956022424224,-1.669573878327583,0.3886212710369308,0.20727260172833498,-0.3078725007649255],[3.1058817438950483,3.7277962179230233,-4.931036658064505,0.5299265558200157,0.00268472636358603,0.09398657348980198,8.00017325679704,3.177079857879146,3.415321994625866,2.8094257882289053,0.1012088234385378,-0.03646795192295286,-2.258245018470876,-0.8394400960942586,14.871012518021825,-0.7741322462766664,7.78073207296251,-0.6903237466941603,2.609387318129858,0.6634938030087248,0.6314282010075588,-0.41328785436890125,1.720498084094005,3.9849209728988475,-0.7367369767179156,-0.4838754417330443,0.20967300193008948,0.5439193360477631,0.0700156155137622],[-7.025415070156466,12.355527213077464,1.4696405483296964,-3.822449012518854,-0.20107754407030085,-0.01991108402886749,-10.59513790879738,6.854890904441334,-0.19905337624740316,-3.2996454838703757,-0.24064128126444062,0.015751417856509204,-3.6696874570436075,3.7095123153816867,-24.26121916462907,-0.46767842207508553,-1.5275887679043219,-0.5039190850419063,-0.9719433841731614,0.15674023571096546,-0.2260058586933663,0.372686910827051,1.9706107176984862,-3.1099671318353566,0.2631720136435945,-0.06278509084746306,-0.1839479714134354,0.1485896149840722,0.4343088851635371],[-0.9088303947683518,2.631307192345455,0.47151693926842303,1.1641134929751378,0.15842631634402557,0.010254001763656873,-3.1423903810049634,-10.0449813816063,5.459665214636381,-0.16524152781594825,0.032166236611512376,-0.0970684797056373,-18.08740801232588,3.705086846730332,-39.25034802370112,-7.6226436013864936,-1.074834306797673,-1.6235837265570712,0.9233506508862964,0.8875477118307885,-0.5589167876352303,-0.49383513316769667,2.422815510934076,1.4110867935973608,1.6601857579790003,-0.530184940684032,-0.35270561582979915,0.2263453954937588,0.5549375121862151],[0.8090092860340079,1.7518989310247273,4.627610501699316,3.275546404781573,6.767810484252209e-05,-0.06268073290232978,-7.174978536984239,14.888224376391593,-3.472824173054651,-0.9773405442606256,-0.002574093510315031,0.07954967760845534,-8.7339672308922,7.415121473082452,30.695491858469538,5.483835542257917,1.3211122598177627,-2.233182014518945,-0.4225140955384926,0.5664196116837403,0.21901204341538857,-0.5835477691520117,-4.592492026144913,-2.483682463584553,-0.6952337860801542,0.6919659591575132,-0.01326958173960348,0.3652956749284022,0.2832504857837506],[12.348899990688604,6.134966221666014,-4.9344115418304915,2.6041972555994453,0.16448766925854347,0.0647321433783924,-8.747564466772863,-7.696089893099813,0.7035441017180346,-1.5116030334141044,-0.1719310504057209,-0.01578892164386096,7.518909102827982,31.14521547183535,-2.931725484631614,-0.19422481129634223,-5.214255094491244,0.7158351718715643,-0.1841934690779383,-0.01975958302903263,0.2912912963026876,-0.3486619245343093,-0.1698803704633747,1.2900626231300365,0.019275302918113094,-0.48094095358199596,-0.11406061327998669,0.06664068324871004,-0.6485153806660111],[1.8582518751428014,10.39636014189215,-5.527758670173351,1.9010790480265265,-0.027529866981005655,0.07138618219973929,-1.3991027671050948,11.48715842372774,2.622112290813643,-0.2980177688816886,0.09877747745195968,-0.04084182773773501,-8.824885484494846,-26.477630832712467,-23.96257514649206,0.43959266285103876,4.623523296237174,-1.5656974350277792,0.35731757181808,0.4020093933530197,0.22583565252917687,0.65829922531491,-3.0167429279705535,1.7232529058487769,-0.07572389598824145,-0.41024351228123795,-0.1303763210068579,0.41096330823759203,-0.4741756394369367],[4.143659072284751,-8.137085156244112,4.8123249303158815,2.5773576390319675,0.10700889226372924,-0.07563047603458882,4.462665513415599,11.236511546680182,-0.23892958598900826,1.2193045154318423,0.13347906968791853,-0.004935506361063591,-0.3071228495333386,4.8358143676402445,-8.323992989297484,-3.8065824772102888,4.215478681141331,-0.39878376867378806,1.380116661228159,0.9451943943296918,-0.10117103267364055,0.5797218678617665,-3.388984720396644,6.853566958064369,-0.24661496685576842,-0.5044728833416968,0.2893442194446819,0.8416786041648436,-0.21070337377626533],[9.467399547000435,-5.779727010549511,-3.0817915105283857,-1.7413456639470328,-0.039542866133558896,0.08175874174223588,-4.772267189030222,-11.229415402198585,-3.0785392544035073,0.17491036165405546,0.18309354028829408,0.024668850348639224,-2.5759115112237905,13.58815324248231,27.016026593110595,-2.1953569825551202,5.108611573673192,1.9132296349243003,0.05561650364899085,-0.06961845578794236,-0.5478993062463109,0.3992807849496935,2.7155505229548815,-1.3137270566553199,0.38230662700434725,-1.6517767307506317,-0.41568680286760545,-0.3362389823591415,-0.4451490711457454],[-4.0857382627959815,-8.187202119578671,1.8427928973558119,-1.266618356325619,0.058028748132156596,-0.05130815246068702,1.4027220679093344,-4.3547688217375216,1.2696769525021177,3.8724463420272794,0.232025204893314,-0.04155578137199057,-10.732661487321012,-6.786443474688598,-7.716063862844923,5.719077571547131,2.118568169505541,2.3900181811210506,-0.5319576447281407,-0.4152152904484409,-0.07622870373608348,0.5541243699291103,0.319270664201961,6.33688324695821,-0.30372420424881796,-1.9685752274143131,-0.1336091663029553,-0.1091264196038472,-0.12680079241588915],[10.202625611343356,3.5467087529326036,-3.044197080878276,0.3787459230789988,-0.1605579916403993,0.07626702245572191,6.054677782037783,-8.052992514856902,5.041203035304773,0.4990675369116572,-0.1310983706322073,-0.08962134905680257,9.56424499722998,17.25817292694453,14.247401133777613,-0.8142699395236113,-1.359620217297683,-0.6191743309363481,-2.2204664821770637,0.08178451141526652,-0.65690886435926,0.44405004138057297,3.6814519957349128,-3.1452200018067957,-0.30702579483171094,-1.8290111478883402,0.34575237312257995,0.19004584123107837,-0.06408053531758126]], -"twist": {"left_hand_palm_link":[[-0.08908840020147969,-0.3851403756602039,-0.15488756556135208,0.3568663989958384,-0.39315291511365485,1.6994286436735302],[-0.4334316909923182,0.255129349992903,0.222145983427238,-0.32110809803261764,-1.6190265782035478,0.6672538551183356],[-0.005479316023530523,0.5282866186446702,-0.09637718875113385,-1.091938220986208,-1.0552156590173247,-0.7361711313250112],[0.5408382220099258,-0.21404967991521895,-0.13222592612618406,1.2560425282123717,0.30343568316432823,-0.17768575557246724],[-0.0029112977255348668,-0.21638204741364178,0.08798335774550241,-0.8178086421653075,0.4981638263195702,0.08509135286327867],[0.2376056846664021,-0.3057469437108155,-0.04857694832032697,0.23274379418314128,-0.301659955169275,1.2893573835158738],[0.3819036962487651,-0.044737535353170294,0.32150419082082154,1.1035705492304204,1.0761087135127396,-1.432936326627801],[0.01526539811673433,-0.5787352520888029,-0.2240014865483665,-1.7713013435179612,-2.213555437949296,0.7454657488459265],[0.4900061450841459,-0.27064477635084666,0.2786527001601313,1.0515521531436856,-1.505606031129556,-0.7978977162761728],[0.3600435408164172,-0.4001315582478805,0.040252611675082026,0.20584847303945236,1.235625087437428,1.0507504344889662],[0.13136498782940947,-0.09201346176296182,0.23144731244153754,0.16263977485774345,0.7851873566374457,1.1141885827414622],[-0.15955076501423254,-0.23035953128057446,0.4804290004438738,-1.0729444183157326,1.87033343311478,1.3055257785222376],[0.11652520255119707,-0.09091245351062602,-0.033721107379011794,-0.4436851982243033,-1.1598772244582276,1.1094208349360826],[0.28223486595195857,-0.27492592189124176,-0.0353894940371015,-1.3977280612736491,-0.8150048648036903,0.5317021331633456],[0.08109964306480599,-0.14895720729539746,0.15696363654441084,-0.10065977538166965,0.098355997847284,-0.9702808547555873],[-0.1914151179208406,0.04239206160765109,0.020306523425707095,-0.7109428211644515,-0.4338005917660525,-1.3652937740186633],[0.023638885192883402,0.04329985051682953,-0.08951744306028753,1.4124160722303203,0.34728322560900704,0.6118588362374248],[-0.05946991626652898,-0.384254900021083,-0.05400957724659365,-1.8824060769661846,0.32666358372417104,-0.8111174472453424],[-0.3466970391701543,-0.3636802168284874,-0.3573249554505914,-0.46607910898850513,-1.7779669816497616,1.3123433130154893],[-0.11359262627237175,0.23921902695495711,-0.04020848769831509,-0.8424310173153552,-0.5992254858564221,-1.1486608117807353],[-0.08524670471360843,0.12459811620295913,0.08415420071518893,-0.5085886166018883,-0.1724860205895356,0.5430848459844292],[0.13139443378080098,0.007684417594314527,0.11095821554639478,-0.43921363795943197,1.232726573694168,-0.040675812766160835],[-0.23185874046725394,0.2493806776355121,0.09440582808103307,-1.6075186102167167,-0.23131671535094067,-0.11902542813874328],[0.04105727109620916,-0.2592053366506527,0.10730558198281186,0.7376460832968551,-1.1579456823977532,-1.0471318939375873],[0.10117389904246477,0.27374298853536105,-0.05898408604163657,-0.9929210850241839,-0.25116492318878575,-1.249432578995441],[-0.08268968589963117,-0.021722870751874887,0.10051012282719154,-0.37739148425403973,-0.8565514164152706,0.416518055258144],[-0.008933322611252768,-0.44570612214400096,0.3214647914207174,1.2319604273004514,1.2869282167551752,1.209426646530093],[-0.5922567935493573,0.06773743552521932,0.6202547268953251,2.233940337558928,-0.4657617255565946,1.725951223677054],[-0.14536810165649577,0.1548184284338056,0.014449515132850088,0.15594579775925765,-0.43849266971646583,-1.1868185204407042],[0.05202640220286735,-0.306157970464888,-0.08167509226999844,-0.18332180729401912,0.16688564917529525,-1.06535821658316],[-0.3266271877772622,-0.02101685296594486,-0.054900906727953586,-0.09259666606392658,1.3107137868900027,-1.8075585017816342],[0.008904712112143519,0.16467013354101576,-0.22169986508996908,0.5155602243832628,0.17968696513674948,0.5362069163077706],[0.35136186297629113,-0.19771629629351567,0.24229838359696565,-0.5173459406840716,0.5721191421194625,1.9862747149340418],[0.01786619952512189,-0.2578098015337901,0.00748678712067706,0.9818776446293499,-0.5008480598786454,-1.805678375411882],[-0.023488006368805853,0.01421140994979965,-0.1420349778499449,-1.3878522356161065,-0.16961303364203,-0.978960806101322],[-0.5198138756703361,-0.08803236568568903,-0.3042219686716093,0.4949382556159111,-1.2432937672022288,-0.0346580389695863],[0.5108015415221856,-0.36491321539331306,-0.23919893236584022,-2.262204412246682,-1.7946259327853675,0.4340436052767003],[-0.014242732352182978,-0.12594814339140478,-0.08608516674942075,1.2499308263583244,-1.3232971531825872,1.0234521874401625],[-0.04917006352518583,-0.03485475614768762,-0.1571539987347961,1.4451436382232252,0.3314041243869714,-1.3729030071844832],[-0.00996142908371335,0.3304281269481816,-0.25752083667003184,1.0924721243609485,0.37692239647640036,0.8716900557090059],[0.061749985500833056,0.28503659826626015,0.2380861514691746,0.4513546744653524,1.2725494497506205,-1.197658230971818],[-0.00022635487323907633,-0.14676616416217741,-0.09486222581203092,-0.24091931076423423,0.35985536038373167,0.9251739345670573],[-0.3402267615930421,-0.044565949373814334,0.09588445304471258,-0.0026878226247794013,-1.8798404857730744,-0.35503359322049033],[-0.02310886360333564,0.22074143246755426,-0.3788783385107897,-1.6258707508518235,-1.1452467038140866,-0.728946792565036],[-0.04360512829237621,-0.42330585479457034,0.017960959559590407,1.0904864921332442,-0.0010207623768387908,-0.6348479175890149],[0.037819234431875084,-0.21252559544667018,0.3219232081055509,1.459348424187543,-0.21242254531685434,0.12097851617817584],[-0.08885248033916598,-0.15864255422363183,-0.46802611413700423,0.9834076019081384,0.7529519866714882,-0.520492348294985],[0.39282129796200677,-0.18574234531412753,-0.17225857104220613,0.7529968567797323,0.04400989624595081,-0.4862592870365707],[0.08861364667608498,0.02995947936000037,-0.18963395577531586,-1.028679680435557,-1.697694389128428,0.11189821113287746],[-0.20907655180077572,-0.33454947645345645,-0.4450117532696878,-1.9650678711539493,0.44951103720110663,2.0405320605511106]],"right_hand_palm_link":[[-0.3053969366380804,-0.024510982070385816,-0.3404044074675382,0.3327939412500183,-2.060688939922748,-1.047080000661842],[0.20445970415685727,-0.046883894097393033,-0.5233406732079683,-0.4612896581831676,0.487538922465349,-0.8944386104055515],[0.1710656410718625,0.20587140098258608,-0.023766003703087457,-0.622976659428376,-0.5586046801570781,-0.8335114954751899],[-0.02189276957579544,0.06322075711041597,0.09272891200329214,-0.6292592778073989,0.6467851275282281,-1.2311501713876614],[-0.005493237052403017,0.06160732388421171,-0.06405249289169357,-0.14386444833392964,-0.03086889845056781,-0.3394090321302659],[0.013280055398696228,0.05919375137064316,-0.12359979793435637,1.7271374322358308,-0.9672838208487808,0.16961529286578675],[0.3283500736600812,0.15875541765159998,0.35041001275069383,1.879211764571143,-0.5312185693395887,-1.0689241510331182],[0.08250264191680173,-0.3769811151739872,0.2174198820780181,-0.36293449041280196,-1.4865178990351284,-0.4372064892681407],[-0.010554163758474022,-0.24041326018491802,-0.09864784898439984,1.1487125365400375,1.354528648731132,-1.4491443399694761],[0.2409282229505381,-0.4092444936881197,-0.05211217053246338,0.6059723893586619,-0.458030547052103,1.58563706813856],[0.08302740033872133,0.16500642163647072,0.044051422166160495,-0.36186006366039575,-0.2712904569126554,0.5494263980584408],[0.4355972054868569,0.08488638571020407,-0.00035949732892193165,1.6824327729774764,0.7745662484865938,-0.7075901834795941],[-0.20847183731007893,-0.17384440874802393,0.15719921325806432,-0.04634178158330926,0.7641101913427254,-0.6689146592770474],[-0.013360595428886929,0.5800789886794359,-0.21484626523020067,1.8277572775143125,0.6834022298306773,-0.019465874610525166],[0.23198426812771605,-0.0056158533372059755,-0.3181060340191166,-0.12667527526141176,-0.2377262562839545,-1.0617243064454507],[0.3630150169275009,-0.002710185040669999,0.23555716085202816,0.28716112296291296,1.8687577208466235,1.1267732912734938],[0.23458914812496942,0.2284771261791028,0.0825168724262485,0.5617768045382874,0.9645649834745128,-2.224053606111304],[0.12673610381105516,0.220158604917716,-0.08679461076462464,-0.3612852515925007,0.6751093793531943,-1.4516224606952626],[-0.5399296685614123,0.01105214461589668,-0.3876184498297094,-1.316660340299593,-1.5470673045804815,0.008635081442277912],[-0.2696917642264406,-0.22706007839814787,0.6194780605636814,-0.7546700431290367,-2.401401412178622,-0.5988695930065411],[0.13527950476236517,-0.03274981189417689,0.007769253440558954,-0.5935392572285423,1.14857842774901,0.52764679144384],[-0.19616251019624284,-0.04652564399936253,0.363278414046765,0.23933470036531765,3.202051327200635,0.8723369968639262],[0.08591945097105412,-0.34634616791535505,0.12783756206581398,-0.49516875366917223,-0.6212422343115389,0.46789568583596675],[-0.042023975162506416,-0.12998184391042752,0.11728530084901975,-0.5754721458597389,-0.38117615396548454,-1.4351800305768734],[0.09542729692471034,0.556873355054074,0.03965186462269746,-1.7235340955550948,0.02600551571138257,-0.7239950720231172],[-0.033557641234576385,-0.20803075330252763,-0.023320696378505562,-0.5339650707308115,-0.13700537147402292,-0.7286824097637317],[0.285524036028726,0.06294628680003175,-0.13957073870564277,1.4702491974776444,-0.9578204054255022,-0.3024854621106735],[0.4608232434843329,-0.38241301566831987,-0.5513459661000153,0.04849726410459543,-1.2466050089179084,1.0594630031927714],[-0.09957774839175998,0.0831806616334218,0.04547281216158764,-0.38635017366270097,-0.19311471784264303,-0.8759202620001666],[-0.0214167579154204,-0.09547708464860868,0.314805366828951,0.2152065766030956,0.038082629933675505,0.8684519278698707],[0.10274687226417901,-0.022387061879094658,0.06492663702263737,-1.0371921133256419,0.9620792557472402,0.48707289797857134],[-0.13465418938537704,0.11153495974633006,0.266359394395684,1.2443696760478962,-0.9990752550357889,0.2063573670012086],[0.25273690957157363,0.25589934558773747,-0.11486279234740254,-0.46244656788613064,0.8650908111099884,1.4735455451011292],[0.021273282943601937,-0.10636353942240238,-0.014107653799352304,-0.9996219170713595,0.5677740586812561,-0.18413286823341213],[-0.19064445976409594,0.041532047482795764,0.10189794041491325,0.051097234934415287,-0.08450172846338067,0.5868481755313555],[-0.28164405122264935,0.0005109632834012917,-0.04560683067062907,0.10220592240394033,1.1791614627529672,0.8602027478191675],[0.058818980439369575,0.16472489062117163,-0.03334926320281363,0.8138235981108144,-0.07696969806585463,1.1039195657534129],[0.11883588724496195,-0.13921547189074468,0.14250520638266034,-1.4511358223344066,-1.0688884390785889,3.236958719208733],[0.17650544568540463,-0.23634650212201974,0.08258421216352926,0.9647192749353615,-0.3707428338471712,-1.0774851322428047],[0.08141574564047113,0.19495789922918633,0.17284408236384835,1.582764311438754,-0.759477966325609,0.17517986869162017],[0.19470525577828757,0.32003688557369736,-0.15394654097823826,-1.2458997042890094,0.4205961827980416,0.4928980707609269],[-0.1614094152821404,-0.041289791659755444,0.033255742156524264,1.0965169671248385,0.11210132586818443,0.7182488711974253],[-0.03131104689279057,-0.4444506371090362,0.14682669530665737,-0.38538655608650074,-0.5380496881102297,0.3007980067063737],[-0.09167661462430989,0.007119256416190125,0.2671756489780571,1.1838597953275154,-0.23358059072341558,0.8099153568740669],[0.09795827349299707,0.4036581025654363,-0.08404314577554298,0.775357148414132,-0.11619405793634852,1.5027320097822303],[0.05259958701705286,-0.2918511004468995,-0.36992419064378224,1.6265666770102216,-0.25658065023996185,-0.7978007014921195],[0.17836631304051703,-0.11252068575911944,-0.1476957443531322,0.3148211382169379,-0.06316965168764047,0.6441485162975401],[-0.2050128634929097,-0.12172080490847786,0.080996561615928,-0.246532081114715,1.7364679099374143,0.3285428117352627],[0.501313259622825,0.8764423750521437,-0.4264806745318056,0.11684363386371877,0.9136357249596251,-2.2343090437653537],[-0.02257500030017222,-0.22852572022801546,0.23166447749550828,-1.455798123573202,0.29429674910560943,0.6398231889716632]]} -} diff --git a/core_ws/src/golem_upper_ik/test/fixtures/fk_h12.json b/core_ws/src/golem_upper_ik/test/fixtures/fk_h12.json deleted file mode 100644 index eb87c853..00000000 --- a/core_ws/src/golem_upper_ik/test/fixtures/fk_h12.json +++ /dev/null @@ -1,18 +0,0 @@ -{ -"urdf": "h1_2_magpie.urdf", -"seed": 20261027, -"body_joints": ["left_hip_yaw_joint","left_hip_pitch_joint","left_hip_roll_joint","left_knee_joint","left_ankle_pitch_joint","left_ankle_roll_joint","right_hip_yaw_joint","right_hip_pitch_joint","right_hip_roll_joint","right_knee_joint","right_ankle_pitch_joint","right_ankle_roll_joint","torso_joint","left_shoulder_pitch_joint","left_shoulder_roll_joint","left_shoulder_yaw_joint","left_elbow_joint","left_wrist_roll_joint","left_wrist_pitch_joint","left_wrist_yaw_joint","right_shoulder_pitch_joint","right_shoulder_roll_joint","right_shoulder_yaw_joint","right_elbow_joint","right_wrist_roll_joint","right_wrist_pitch_joint","right_wrist_yaw_joint"], -"enabled_joints": ["left_shoulder_pitch_joint","left_shoulder_roll_joint","left_shoulder_yaw_joint","left_elbow_joint","left_wrist_roll_joint","left_wrist_pitch_joint","left_wrist_yaw_joint","right_shoulder_pitch_joint","right_shoulder_roll_joint","right_shoulder_yaw_joint","right_elbow_joint","right_wrist_roll_joint","right_wrist_pitch_joint","right_wrist_yaw_joint"], -"ee_frames": ["left_wrist_yaw_link","right_wrist_yaw_link"], -"planner_frames": ["left_grasp_frame","right_grasp_frame"], -"imu_frame": "torso_link", -"q": [[0.4189000808417968,2.2871373785168285,-0.3090922181450222,0.903638890970159,-0.3527103564167966,-0.18750795902859696,-0.23424867825464224,-1.140222721280617,-2.139803602024264,0.3498724880490446,-0.19640966686612138,-0.13178048349257032,-1.6152526191406191,0.8004940171036221,2.9346488535298167,1.9704848873353376,-0.4109335951308114,-2.871893514353677,0.21478926377614505,0.19601538138491925,-1.6092712532372437,-2.0893129936300623,-2.2178873674398805,2.9702498282140013,-0.6809311575681383,-0.4187804924461013,1.0290064604176123],[0.19856725652810053,-3.0874695776558005,3.0319501577314565,0.7506480576569451,0.3080031738163792,-0.0992566119044068,-0.009696367483391588,-2.760246096800696,-1.1131055334026216,0.19391917461712532,-0.15901435656721574,0.1667568837571029,-1.7891346165394584,-2.8447710694291946,1.1023603086376996,2.535866627792416,2.7609350166676183,-2.587382915843323,0.45858439781050153,-0.12434543820709365,0.3238888066759462,-1.1423939111090062,-0.647532903270668,2.669490494753881,0.7713420061102285,0.3398490167647391,-0.7608118634358956],[0.19559687995688418,-0.7190517269164123,0.9362979385740473,0.24724351894216062,-0.7907265895095392,-0.130053158489718,0.31104966153158214,-0.2491717893927503,-2.775423842510102,1.164123541729856,-0.23793440855043835,-0.08525938841649972,-1.6867882442840703,0.5972938951587476,1.3976649408335917,-0.6633889200182115,0.6651273352406872,1.4143351000639068,0.1489062998737879,0.6910745259432831,-2.2005646319139993,-1.2588947251413383,-0.5338885540738598,1.586254916941239,1.266896827151002,0.3673557695816718,0.744292930687291],[-0.10504552995405037,0.08046298945782127,2.7826200602581017,-0.05778581493619775,-0.8887167791353145,-0.2552992482645734,-0.23936241109116388,1.383074828958494,-0.6281354135839643,1.6134840215050588,-0.1829310913947778,0.24424245945849787,-2.2518871333509725,0.04083956658288823,-0.18144350157217753,2.502397495564984,2.0370039780811693,-2.0195879745025733,-0.2747160106981532,0.24656869472151977,0.3513420213225946,-0.4385084794566332,1.0457114818222228,2.066977902385342,-0.8310338411588787,-0.22446550240369884,-0.6446000950409725],[0.31420133777579823,-3.00035538645568,2.33186553578499,2.1115482255308087,-0.2847868949851696,0.2409089933235552,-0.18111993238127905,-0.3427633049235612,-2.320385771757055,1.9479025584362062,-0.07247511463981315,0.14765620370540755,-2.143040614906868,0.8807276001686168,2.109696986837284,-2.546694401909443,3.0044204199449336,1.6074308838020412,-0.19466507371159736,-0.811703657183605,0.4376711446765418,-0.8538896762529204,0.10974950265892147,2.829922285956508,0.4794513521672519,-0.3865250717766136,0.8095682196847158],[0.2565950626916425,0.4815911472935528,1.4204937387412107,0.6214963154309354,-0.06040585694680356,0.04074186956459169,-0.3124138142035602,-2.8219875997063752,-2.0900352844201944,0.7382423910381969,-0.2641885128650857,-0.11189033864882522,0.16380161083722378,-2.6842973334013207,2.4808800638890025,0.4879792844322566,2.256574147644799,0.7769577983090223,0.35062279915408845,-0.6467470048179236,-0.3035620967787591,0.10946754612078191,-2.2969020257800477,3.0424053475486597,2.5763703591389095,-0.24094301249543534,-0.8393951286611969],[-0.22461955193016353,1.6839245818956248,0.5992391749023864,0.479063485383296,-0.06878133257643293,0.239912003415634,0.4285494255973666,1.2346360475664304,-0.24317428499528537,0.7201015413001561,0.18646023855609994,0.11648889467610796,-0.9541071239811112,-1.9139919751672552,2.497367489206354,-2.37000725046287,0.43709632941182086,2.0682447220263835,-0.22584384105356906,0.524124540577894,-2.3224658488463827,-0.6985358782901847,-2.5254192426793933,0.40204756920930396,1.707788318363626,-0.42736763772352027,-0.984465237378298],[-0.16215816115457848,-0.1728273161878997,2.3875046775162647,1.1378137030187725,-0.5839958351502803,0.12886142926206767,0.11558586453020209,-2.4102403928859846,0.07062606157385343,1.17779592373068,-0.4671944666558577,0.12469884934925907,0.34332340331411526,0.8440207792605259,2.670968523953277,-0.6321344996316762,1.6885515425901698,2.56072925034034,-0.2353894127602544,-0.07807586078918605,-1.79909797005996,-0.7309834085407187,-2.5492904524393705,-0.27098885783468984,0.26698452438794495,-0.3229935630796149,0.667071137580683],[0.28357474730366244,-2.1105229349605117,1.0392907669071567,0.02417470954603798,-0.3338565260165094,0.20150736252947532,-0.2252377218122311,0.5824822902474889,-2.5891990166310537,0.1482285294773421,0.23037563949285722,-0.2325114769319446,-1.3619246622930476,-0.001131851817910423,1.8042216065359984,2.0551957752569434,1.8648111679117052,-1.2276249188805355,0.39559137180097514,0.4347660125907389,-0.2572521431456045,-3.389498432138258,2.040089771485439,-0.8977649794312533,-2.2230086065871353,-0.41143263305794525,-0.6643765527786258],[-0.3545608273393621,-2.35676362096139,2.6290560558263634,-0.04318889918245933,-0.5355635152965809,0.1323400872901071,-0.2961873175340124,1.985691339973838,-0.6044139785846276,1.4446089990490574,0.38912709046404126,0.11964524057131071,-1.8775505389040013,-0.7779230001222484,-0.005076692081322609,-1.8720749265955514,1.8002148327373788,2.0894068679322997,0.20685586469551964,-0.394466269462601,0.2152918137258535,-0.3731352808604589,2.489113254012553,-0.8564623241421322,1.2280978758854153,0.1706439232647574,-0.5888514751931757],[-0.36134079024767995,2.0339591433427997,1.5112082011641568,2.1684497439406134,-0.6287253150711185,0.2206283693259402,-0.09106596516374199,-0.9271151331170797,-0.5960207391792682,0.43855662774739046,0.23769863782204992,-0.12219080049726683,0.9828060339369533,-0.8078409217185878,-0.1260954028917391,0.9441820397767899,0.2478906755417032,0.5760723712489058,0.026888084084436492,-1.0686148768075094,1.494442784338871,-1.2399677079517883,-2.422228985478398,0.34946773460743374,1.973996508144146,-0.06793718307563379,-0.860318968449531],[-0.395144737362453,-0.8928684982004667,2.7543144164892155,1.8051342677071198,0.5105639767682034,-0.20473575967594776,-0.030948037308885123,-0.6101048469555472,-0.5874321231999478,2.0081460173545804,-0.7333311291893425,-0.20863523890067084,2.0055883767324256,-0.9984201477700667,2.7208239075092036,-0.03507405022712984,0.5985033750182289,0.5174328565611694,0.45646948896686734,0.4823851027987829,1.2574562013182011,-2.6152800612490044,0.7451279992088091,1.7088332850606516,2.3954546356581936,-0.09871917450893825,-0.9834680983788542],[0.10667612721536784,-2.6335675504553877,0.6706672736083745,1.5104324780360572,0.0837166086699197,-0.24979705934321222,0.33475728403812094,0.5284217765995685,-1.8176521688028726,2.032566572068784,-0.17668871299716937,0.1627554148487972,-2.2617812576935314,-1.412361180949608,0.17945036323896907,-0.10640404397813397,0.14468922893892722,1.2946277340176628,-0.1794557938228124,0.42525955899295,0.9713160936095258,-0.7393446117283267,1.3028296044476342,-0.7816208115067418,-2.370735060776926,-0.2068595420883113,-0.049304150806216196],[0.3939875348042126,-2.1429919741009718,2.5098205325937784,-0.01469544060173765,-0.6192489995747148,0.16223419277604312,0.14741778694143387,-1.6305672519475263,-1.7159819842098976,1.338317660972931,-0.1973668119595262,-0.03198876776138143,0.059018093616034495,0.029310571501142135,2.9216003862205433,-1.3312110118691451,0.06886203370276522,2.2054787316137707,-0.18461239127575502,-0.09795347790018494,-2.645063947150777,-0.39484628506497277,0.5264837139447063,-0.07160863976412646,0.9140021729207759,-0.4197391088891395,0.5789051598570296],[-0.0026864990193083416,-1.9045938719498983,0.029743044629437776,0.13191618203962857,-0.31583818119985596,0.1317417494211745,-0.3348057473464753,1.3102829488356131,-1.9517362340988158,1.1855475560323798,-0.5021039340319591,0.06891105513287515,1.8581610789164533,-0.9545181275046661,1.3401915667974225,0.5461354892012094,3.1535428742408325,-0.5655461637447154,-0.44556179751716507,0.2017763802620265,-0.8715151397599872,-0.00464395098856496,0.34532715892864463,1.5781796646233393,-0.3027169043205533,0.34364279984766954,-0.30779491537968384],[0.006064735113411046,1.86943594383763,0.29340632457371213,1.1397578176784404,-0.31210380580543895,0.2270487446609259,0.08326576607018249,-0.9608065344363275,-2.029041423288475,1.6702416977662908,-0.5768885081469584,0.25491390284184345,-0.15315111148280902,-0.42262394094634903,2.0743981305469403,2.7140499214380274,0.2973803269236692,1.1987081404718287,0.07466059204005349,0.43112133119605467,-1.860081490623698,-0.5935090282424502,-2.2513037264533615,2.874376059467222,2.254405624722269,-0.18231638734408762,0.08490780886017335],[-0.27146248018879265,-2.29891710727597,1.5569951035545258,0.6447242823012176,0.1929007651432838,0.24872599185860123,-0.4147210993177517,-0.0685397899169704,-1.0162899004124664,2.0493639160491894,-0.44720600891228046,0.06809182590560531,-0.46764975699318523,-1.3037528349946033,-0.10616584503777517,2.1810617689625618,-0.7614930147842234,-0.74504702323136,-0.2539249234354976,-0.1178316781814257,-1.1730544165886285,-0.4861547942577724,-2.6685728646979845,0.4325428720587632,-2.2017540887343054,0.4111835729322396,1.0234792075422061],[-0.19367681908679388,0.2566686557227973,3.1219962966740913,1.308447403632588,0.40074055662708175,0.24696512410431437,-0.2770335568727506,1.430698840609749,-1.6150979108781034,1.3598947248881155,0.32386979529498994,-0.13722408075107995,-0.03680965229942812,-1.3973136163994022,0.4354381921044161,0.7202942944124415,2.85089117523174,1.3235640023203947,-0.2741817400608452,-1.268388706444282,0.01719879606447705,-2.1717408903790094,-0.12867229507432354,1.7560332722738223,-1.9035825894209193,-0.21471566067606448,-0.9348130368743077],[-0.40948401183797956,0.3945097362367851,0.9425828369661324,0.8199525855591943,-0.15875940264854171,0.16012816318883327,0.24936486536876962,-2.5253174987918237,-2.5406176066864745,1.8137034726500367,-0.8161310096077018,-0.2515512525080076,1.89416159037162,1.3703404953597622,2.4808935459661083,2.067196155954636,1.634019290339127,2.444745044777142,0.1915605087806147,0.6199649129344842,0.19882720443116364,-2.7449716213810538,2.394919212246732,1.9794128558785447,-1.0960099093138544,-0.20154911229690425,-0.24308314014327292],[-0.3097421967067537,1.3098693847189566,0.6813650796421367,0.577306193669155,0.0280772775994248,-0.17338717544375534,0.2144388990192933,0.055617105624874874,-0.1746705129353492,0.4231869465539243,0.28736514365269794,-0.10859883693209355,0.8852567477598035,-0.5437365429783134,2.67875279657489,2.8922921253551275,0.3992255229780244,0.24315229269403593,-0.2910672665268096,-0.7853782524559294,-2.748879356966032,-0.2541592999199058,-1.4549996003255508,2.039155135424801,2.729527714127122,0.07096104852694152,-0.6268763926626492],[0.39497856807942805,-2.741308051803605,2.0872787988753636,-0.05058805830577373,0.3982862303454765,-0.035885252885613894,0.053618001357354506,1.4705199093007342,-0.8527503361091022,0.1478079277885923,0.030990676457526622,0.23655737918612396,1.588251454110917,0.3659159859862955,-0.12891498587875205,-2.632599929788563,0.8312955723780306,-2.894041614145575,0.23673682748471025,0.11764172729016353,-3.109309024032635,-1.096818529895783,2.1024725495014813,1.6036921906327153,-2.4589462473637838,-0.32928773403722433,0.5093045882044218],[0.15502885396834182,1.4967779517736202,0.3191620277779979,-0.09242685846160607,-0.3140066899858285,0.10065826855718657,-0.35335098900358036,-2.348329696195475,0.4205996400831218,0.4771863264192857,0.284496352139039,0.07573903331808668,0.029608532729266557,0.49840659595061565,0.9162827239800084,-1.5120035493211743,1.4026830839366735,0.8358203842432648,0.19817653204644714,0.730142140156504,0.8908371530427677,-3.146165954474042,-2.0039165848890947,1.6022907685141565,2.037008343535904,-0.3986700895308648,0.560296642602172],[-0.12193980272728361,-1.2109615597041616,2.758448291329414,0.5032312013455464,-0.5252751106354168,0.1473637162830032,-0.058667603326634066,-0.7242477155631684,-0.5616235581715086,1.0391232771680348,0.4916116702516825,-0.21826434572073955,-0.30897642442242423,-2.4317719944208567,0.5922180366780914,1.344480478445524,1.6054303709912736,-0.638496669764375,0.011539403470648135,0.18080711762399004,-1.5168711693331924,-1.2935908910077814,1.3133645881917646,0.3885575880654357,-1.471807360355262,0.021989103786424546,0.48488890913167637],[0.043913329992909256,-0.971624089193301,0.0751223643112609,0.06822101806256872,-0.4460839435717325,0.08469273802370086,0.1139345636305456,2.4427853311787913,-1.0435366368170764,1.5793760406086776,0.01132237389898294,-0.061142583046055715,1.2452786963079583,-1.163299705173275,-0.15046872230716854,0.767527666914269,2.4440481002492893,-2.83378604413649,-0.30789902430987803,-1.2568835942252756,0.7251173022832393,-0.7534387073439004,-1.321794567054091,1.4075713891857238,0.1507725002476854,-0.3900775891888231,0.8401902299743562],[-0.3250719862712815,1.328681733028279,1.8518369656548221,1.0802418306302366,-0.4180717827449538,0.2145565700996665,-0.0898960782289367,-0.6728060300089838,-1.8792152600428742,0.223204106182613,0.3531369845462303,-0.018228588606277074,-0.1706439002681739,0.5082441877996016,2.9542944082016147,-2.042376758511538,1.8433713362293338,0.7234419514669344,0.36263652031644256,0.7403735658633077,0.4790819765668779,-0.39093433365426034,-2.555914917252824,-0.1959440666246871,2.972122867638631,-0.24654396107180107,-0.6720424250119743],[-0.08139899165376718,-2.670723227762906,2.1852281139120024,-0.07864827318798909,0.29439916759907436,0.11222205976601174,0.3820562618320829,-1.158293491922788,-2.736947952422575,0.9990469103604277,-0.8671942137980524,0.26068488889439967,1.0035461711943778,-0.17259551611754498,0.6588543024516189,0.23713549470244688,2.3458123669457915,2.185337497712456,-0.08756030089521705,1.049511725657037,0.11576784852160715,-1.5313346705291715,-1.2659110253722898,0.5438037453283784,-2.356949879518992,0.33795737084651434,0.3771294136250509],[0.04466973390924911,1.2940719558796183,-0.1725172952832918,0.4597911630080983,-0.16652351971987656,-0.15563927260714389,0.18128435603178278,-2.333181053193693,0.25820733130121765,1.575909694494455,-0.22584462295864272,-0.2186598078110915,-1.7229064702129364,0.7132839241742217,1.6721453628813232,-0.9726200135805934,-0.4673673341704629,-1.8649997319342486,0.43868444520806826,-0.14613477805809305,-1.5963004025260323,-1.9690563430938817,-1.4871245179439696,-0.18799229677933904,-2.407547263861679,0.23905065381128376,0.12204483511603059],[0.37651865504248666,1.5884905007691477,1.9487662868009028,0.9486424106284063,-0.6313548203517005,-0.19539753280056565,0.03496880005497238,1.4000359506501021,-1.8710384221681708,0.16102579516894744,-0.07985492870247624,-0.09961836433094326,-1.061953802734136,-1.751798600679261,0.8124089877934184,-0.44511911385847425,2.8273411298307787,-0.4560024054878853,-0.31042955269072225,0.01206151998224203,1.408313782841884,-2.1843785206116957,-1.838398741071168,1.086940524128303,1.1092709070833058,-0.26450100898538936,-0.8538694381664571],[-0.41938749712112305,0.9603069821758754,-0.07816524550315895,1.331433323140896,-0.7659933546297171,-0.049919328708884964,-0.41499982236275973,-0.6565943797071925,-1.2769906461115934,-0.04159321965834985,-0.5101186727800698,-0.20719752669755762,-0.7010013568337103,-2.553746898516334,0.492406028550403,1.7838797124133423,1.661612782835119,0.6699433679503688,0.05048113084783146,-0.2915876486846084,-1.0276246204879387,-1.6469976420206835,0.41125209523664763,-0.3537765803777815,-2.5850426121543637,-0.44514872472447153,-0.16444163850708726],[0.36919823244768996,-1.8161465053232235,-0.0088261501031629,1.4286322854072728,0.3503392093359007,-0.08951776787778781,-0.1919143966588155,1.8544596856714297,-2.234180207534127,2.147277776909548,-0.17506809347735064,-0.030729306285313923,0.5039631458565994,-0.613385944272518,1.0451575467998415,2.088385613066677,0.05023241641381393,1.252178220951322,0.44911321074455823,0.15784868805951868,-0.3774755200379407,0.08787407257508262,-2.4364942600968975,0.43249690673354513,0.7249541159337731,-0.08462047869265726,-1.195517788081012],[-0.014853663946441553,2.3684028386178677,0.051069354145372614,0.25503801526697684,0.3835567565690966,-0.10218784746795243,0.3715053939605814,-0.5819060422867657,-2.546840326663928,0.5056381595137407,-0.6651956715369656,0.16246596936014773,-0.455210921626948,0.34442399601892193,2.003994567636644,2.0250545632967585,-0.35355361901651006,-1.926513917747858,0.15942937439180505,-0.9532072181406179,1.4322013129467472,-2.432685528206835,0.7311869867117378,0.24608662300192008,-1.5559848210445002,-0.38936718430260053,1.1521555836645105],[0.3170255119386823,-1.9180195889082468,3.0931937295941156,0.7277352149689604,0.24953589534381393,-0.005868625847204489,-0.3153061903246797,-2.63498150831663,-2.6369488951579347,1.703555869418289,0.12081608405693667,0.15359001782277737,-0.6916402092124956,-0.8788363461512896,0.788131051774108,0.02580099802959257,2.223563368350212,-1.655711372693832,-0.4257511179183749,-0.6255146109137089,-0.7275956865392139,-0.12075371002590707,-0.8116300533698015,1.9845980025221055,-2.2253624997400676,0.142028439289555,-0.8009101837403749],[0.18595925452786105,1.855737697151676,0.9080648262522097,0.587029026367775,0.21963918029400709,-0.24954391634616624,0.12962711873547844,-1.0370694119088313,-0.8840700469809448,-0.01527146888700387,0.08437652583299993,0.16753307208249285,-0.5016405531019761,-0.20483792519092914,0.9765238096856785,-2.0370935241958223,2.3122832365039283,1.4087011520062882,-0.06507698006096524,-0.9398899940884617,-2.991842445666708,-1.2281142972575605,-2.1258628693390405,2.767109001761619,1.96763021113615,0.2357196072623029,0.3407910830570744],[0.022852615334948312,-1.1283344778250397,1.7224102811642543,0.23522898650085938,-0.45799687770279457,-0.21116117015906244,-0.25429886123561085,-0.7713743087299698,-0.36958873781622703,1.8554188598173056,-0.6811450012352143,0.11524532254477943,-1.0612366903456008,-2.8452545878145807,2.2380610574444644,-0.30718767468940955,1.0118564949723101,1.2676409943424067,-0.009855932554584779,-0.43563855230544446,0.08718927826089695,-2.611392384721352,1.0585810321946063,0.2684581088127933,1.4373413190328108,-0.4553132670865099,0.09367159551319348],[0.21576547506593285,0.046330153773920024,2.5112699955015825,0.033970460600811325,0.35002051160743364,0.13349827758322408,-0.025777168946689355,-0.3077114846712501,-0.5948145673103684,0.12922564390102592,0.3512531161382467,-0.19894752382363967,-1.3928010513989277,-2.5465973517436744,2.531464273200042,1.3074272992045946,3.0413233676454174,-0.5097512613889275,0.25560262379551835,0.022174024880984078,0.5654804035391758,-0.3096303818960333,2.1086384822111306,2.6178562375059213,-1.2213114344413578,0.17990622525846067,-0.16503751995494742],[0.06132803499599099,0.9895184501504279,-0.3882133383958233,1.6951998344321115,0.08437566181066036,0.049421661383512006,-0.41403526584969236,1.9297375796878886,-0.3816855480959904,0.273681297295772,-0.10512327744858163,0.21454877313013476,0.025622791883937346,-2.431008814311854,1.4266632910442332,-1.9618780474604443,0.9588459854199982,0.693513644039399,-0.2870053273482065,0.2388150330576555,0.552152723422227,-3.3638971178644104,-0.9436256937142145,-0.8874042684233887,2.2350554038856973,0.12904096845730395,0.7409566828465075],[0.3820054387539273,-1.7661518166255536,2.7597382904338033,2.0111431718486985,-0.22103053217168156,-0.09782314739089318,-0.15564357889458652,2.296635595878635,-2.734543958122301,1.3730338100487023,0.23673764657913476,0.0016751591579155511,-1.5272105318901352,1.2440563745245918,1.1125258056616032,-1.1512259531809057,0.8396339889552942,-1.6832745704192937,-0.2201291384404004,-0.3992649847634251,1.4966259853760424,-1.089538078310194,-1.0506265994642026,-0.9299310089921488,-1.1759185401837637,0.25765895585488985,0.057910374554025035],[0.1833304370929904,-0.8481695393442648,1.090698438874884,0.7373462504680851,0.3076282221586618,-0.11863939632840564,-0.19033170170675745,-1.2030859875180442,-2.642237259288126,1.6855452760980896,-0.8609912662460829,-0.09764621282990144,-1.2432046689975367,-2.210751618372415,1.9766858772762679,0.44186779987572455,1.7570728890190963,1.831880388174179,0.36726213228042015,0.8731886933068362,-0.5509602106212559,-1.988792344076842,-1.9840030445497858,-0.4468486627143332,0.6159791667291299,-0.06512159703293963,-0.6443252418560615],[-0.27779627745627106,-2.7296749416790185,0.486081026264443,1.125318921773609,0.1396441246433543,-0.21252520677417575,-0.3977776153440632,-3.0593666800713435,-0.8878043161361897,1.0608105822803884,-0.7613218873606484,0.019744010091857334,2.0832113919356536,0.6821924342965424,0.8912937332981112,-2.5635303916366023,1.2261680644066992,1.9915669552679578,-0.3367521324406133,-1.0047841604329244,-1.8631579502130202,-1.3221467265896552,2.097921739091948,-0.8746969730981659,-1.1402385519292468,-0.2936368062675605,0.3480703806216714],[-0.378073064313788,-2.0147800219794023,2.195142770285732,0.4336785326158502,-0.052151712151849994,0.07001355564918521,0.2816934546133239,-0.7777575445584937,-2.333343033889948,1.890402388004273,-0.5048399604481634,0.10073230615797191,-1.5698419280709048,0.23698200265082292,2.931213525676738,0.9065299508481659,3.13497648642781,1.9864096683041241,-0.041042719709911524,-0.10322761385152535,1.4757737803232724,-0.7476617798872947,0.23989309217844745,3.0030642055696193,2.7718383726351066,-0.2107285895823232,-1.2331765245629962],[0.37499131460045626,-2.1980142261420643,1.1166526942197548,0.05586942173450957,-0.5783272505363966,-0.11664884002464648,-0.1096516931355006,-2.3867761452785903,-0.29349506825493776,1.06991877386873,-0.7909746289978192,-0.18819543414758405,0.41090419875619233,0.9988385102061339,2.581966849711,-0.8643285018277131,0.512661833575772,1.0493080608318426,-0.34610573696456787,-1.0683092058633947,-0.34474155873725953,-3.3757358711026746,0.6810178904233641,0.4001100926497594,2.555717438994627,0.30012528494399704,-1.2539472807177148],[-0.25516763362141615,-1.4114211404244437,3.123340807945114,1.8457889633923164,-0.7296247114771139,0.11769638833181367,0.017589009767823793,0.10742256528482841,-1.9149438183120389,0.5797996662091569,-0.881980369346081,-0.19631156273317776,0.9558099565898028,-0.7374839947460035,3.3512890859496998,-2.329206532093402,1.646523064252097,-0.35714539592030414,-0.1868889063219012,-0.26066928475550566,0.7789790946889559,-0.11429817342837678,-0.7145881102970559,1.523995697821826,0.5035438165736625,0.09727232688027121,0.4398331391014194],[-0.11494795193878143,-2.2908869484121572,2.73155208764111,1.1482116391858366,-0.3533317689294877,-0.1189219205828215,0.3987291052569146,-2.049985954839352,-2.499070702324954,0.7510983401595787,-0.2505894623204987,0.13059192924341317,0.30252432945921326,-1.683026668262797,1.1227016049721308,-1.1576196618377592,-0.03411377718672115,-1.4288331718303573,-0.09019519181879804,-0.5098846644505663,-2.6111405352600836,-0.7091874832408158,0.4438337013588258,-0.8525254944169527,-0.17956602034478308,-0.07290038559528644,0.30244965666206935],[0.3170653087882918,2.152813628415434,0.6294856788780976,0.5361899053336603,-0.8032259232672145,-0.21882371118261362,-0.32165580262848603,-2.0377088847094735,0.2167598786624847,1.4592049414030814,-0.7641297013241779,0.023436087394391902,0.8441225998033275,0.7662885520176062,3.115531411812629,-1.392404761679002,0.25599522316597545,-1.2742872019565916,-0.16634982239513363,-0.6484182653880263,-0.6368167536305434,-1.1013470631706004,-2.7828708635263193,-0.6632305426026455,2.946658189613138,-0.3146920468555787,-0.39498806622570504],[0.37430068854975734,0.9523165774209699,1.7066261600914714,-0.11065289954396028,0.07400431627044068,0.14963467129556296,-0.2180671443060321,0.7553911045241359,-1.8645978754391108,0.30849495300596796,-0.24197472579048163,0.03374697118183745,0.17603540331509304,-2.2724195076609206,0.914000581418007,2.5322405079639454,1.6309009928728757,0.7970431539943799,-0.0022991530656205628,1.0020810113191323,1.353989087465981,-2.3671667450687286,-1.525714749869507,1.6960523658674187,-1.7472279990613824,0.22641732774155676,-0.5033435428071519],[-0.1255088989348459,2.17328441497123,2.2588614805028966,0.08785979553574458,0.14049372389736636,0.23807136036630067,0.04513911030378487,1.6303000063705295,-2.7518712182921017,1.1063968645438451,0.19172086487909434,0.1293795058126721,0.20882587377555284,-3.1194705186016383,1.2514959314461458,2.3156100621736693,2.108195005517267,1.8879551451722527,-0.32327929432220015,0.8868926327735358,1.4780182536087119,-3.210562917228347,-0.7273216053203893,0.9510903141709581,-1.8598258173356141,-0.3422238204386266,-0.8918640514180206],[0.42362952821853955,1.8499305366540741,0.12773670529869458,1.7336542790508624,0.3146003511520251,0.05174699965709739,-0.3859383757112144,-0.3207714424794976,-0.3308607515567399,0.1392679862708202,-0.0018536917509610618,-0.23354713693025045,1.0123505623743432,1.052575289463419,0.5309809392664695,-1.7686414917408513,1.4945013185868883,2.116743541552884,0.21071616878287902,-0.7901975423194074,-0.5059696838434111,-3.015199664527651,-1.7195437862414957,0.15393605332133942,0.08796710978718458,-0.18017189810629508,1.0704406194726173],[-0.22867126060404153,2.3455900223786377,0.6364208029264173,0.6578680485549352,-0.6552434182182261,-0.06643045165968056,0.4295623007038966,-0.5189956938201039,-1.122199060965662,0.7220389781060987,-0.33395856155033543,0.1644327539533652,0.8604875101109806,-0.0374667489934386,1.2597089054282893,-1.4380630387264248,2.1786034728290193,-0.49577315119234866,0.2684120174401766,-0.07646718834717636,-2.4492639517636157,-2.1300719474956606,-0.15503817730466363,-0.44792346025597607,-1.2148664214959353,-0.15195129926613193,-0.5115046152552571],[0.2525424966046255,1.6592162916091264,1.0098141984432958,0.5319666036587657,0.10668337181415855,-0.06222783807678109,-0.32829505867880887,-2.608437076560054,-1.9557860964054077,0.7790887505880966,-0.45827357773944055,-0.13365518029822848,0.44261983830736185,0.2768337502663685,0.6596757143540064,-1.9875987664047892,0.440117946426976,1.6519834635804305,-0.3475929508960942,0.7330417476361024,0.0837038962938994,-1.6595769671999998,-0.5420994012628584,2.119977061668913,-2.01927183575052,-0.2154215445282695,0.29825283733081354],[-0.09628184426601066,-1.9178345507223702,0.8705801265834978,1.5937237516983425,-0.36638462448604303,0.07909280597610552,-0.1586970217802513,0.23257988526168516,-2.8671229096705324,0.9679472730743129,0.0806361955005116,-0.07086008450379647,-1.9629564392390881,1.4197739936969458,2.334875293760015,1.8270247960315436,2.461979406673078,-1.3191407187476512,-0.31582342478551345,-0.7923987546765237,-1.2048822936132355,-1.553795166471886,-1.1765249396902269,0.3271994262179425,2.1994267445597435,0.16859365201143994,0.6544267181392434]], -"imu_quat": [[-0.15885537418841272,-0.323058919251674,0.11050004112974353,0.9263841782401753],[-0.46827051774816014,-0.5253107893149768,0.6370715449590447,0.31450142041116774],[0.4492237749548284,0.42575442140208525,0.06427039048781849,-0.7828157443348036],[-0.5833624738420777,-0.5686998148608221,-0.579670036893478,-0.015855378205263445],[0.8954806410046503,0.08572134386312386,0.16966801706043094,-0.40246619333675976],[0.002583311211892455,0.6250241799252721,-0.4254739757881105,0.6544539685409373],[0.015645075180544844,-0.14301087093653536,0.8688084937309787,0.473787846654467],[-0.6884579794742672,-0.5090868944932472,0.2437983033877473,0.45543224701231555],[0.25898726029553637,0.27174336434396623,-0.46370978714347894,-0.8025299846404395],[0.8913606312238138,-0.17653744131279617,-0.40851736451684384,0.08616449272889143],[-0.6361802767469219,0.3172035787459691,0.3382888119092008,-0.6166175677403467],[0.11997042713687188,-0.25626006590225336,0.5304131932934488,0.7991243455286642],[0.6764834243013776,0.0324674536462747,0.7317064320483441,0.07695283229544582],[0.04373138881338093,0.9711507357884035,-0.13258418792473853,-0.19332678842424536],[-0.2636725922682446,0.8604841732023885,-0.42464184312773645,0.09860556180979246],[-0.4322518479054208,-0.7459343424762519,-0.38853008386647575,-0.3252455543548289],[-0.6866108360989455,-0.020260104590563034,-0.6667402177473118,0.28915838212258965],[0.2826604031875753,-0.8962648695602347,0.33394748772916427,0.07274239136427164],[-0.4711327378481973,-0.23445295017542808,-0.530553515158851,-0.664513901310038],[0.1960434400510175,-0.15089218321905523,-0.874424113829921,-0.4173499584390869],[-0.5779728831974373,-0.5481380139955393,0.5591595541513057,0.22985355534074348],[0.6744822884158239,-0.12993628544006647,0.6027367573276956,-0.4060770933030887],[-0.263719378843658,-0.054374531046687276,-0.09314067480540285,-0.9585511537171932],[-0.10121290108181044,-0.8647484372036577,0.4106662960438976,-0.27077533547578037],[-0.6062694349463066,-0.4560003753832903,0.4014646876863044,-0.5131541039893595],[-0.2502469765810043,0.962465558853857,-0.009543279811309945,-0.1046203830170868],[-0.24241780754593736,-0.36825691136133154,0.8970649147145129,0.029916427050654323],[-0.30892978887937017,0.8025333028039474,-0.04085887328146075,-0.5087565585893757],[-0.6332200604113756,-0.24917120048427535,0.5752706235973689,0.45388299986641173],[0.22870004333126576,0.9234216937865227,0.27535879255776,0.13844204919690775],[0.2098053335338353,-0.48673506110821607,-0.7889704010329166,-0.3107996277389922],[0.42904333645827425,0.041706822394263335,-0.742567233173565,0.5126170701639595],[-0.44126271169096254,-0.7647763631435353,-0.05156692080408915,-0.4666317459484199],[-0.02918142966716888,-0.03774414734343095,-0.4832137425373483,-0.8742015228349506],[0.5677962087427668,-0.17402119384543271,-0.23375561676185375,0.7698586890217165],[0.4283132375301734,0.43996506622206094,-0.7620678326613322,0.2055021398513862],[-0.3237329800310817,-0.18114406245109577,0.741288060265287,-0.5593530173218315],[-0.1717970352295556,-0.6285286629945287,0.4970625203246711,0.5730325901457358],[0.2927583595203891,-0.5653208124613975,-0.4697562840875248,0.6115831550077198],[0.1703878008022685,-0.39777416765405094,-0.7343275186274456,-0.5229787799436532],[-0.21775208790242231,-0.003772651202378117,0.9450111896159983,0.2439746847883944],[-0.5294773517551613,0.6495934663887636,-0.28249425263029415,0.4667751703274528],[0.1399208449897781,-0.6935034168437826,-0.012807256731157417,0.7066195172356672],[-0.8106881964458396,0.5704781114659301,0.0186997938649027,-0.13034450579547907],[0.40945956531765926,-0.5048149671872673,-0.001368033062018244,0.7599360774164714],[0.3314384307866685,-0.28529183165868816,0.7359160085854229,-0.5168991832980597],[0.1159391718802656,0.30303342794819854,0.4994185941551828,0.8033118434218078],[-0.11332886720144332,0.3258074305054205,-0.20712797503470237,0.9154802499477075],[-0.18504157659900575,0.7251673427729558,0.3728140744389578,-0.5485449897734807],[-0.1099413123014627,0.3534163018833983,0.7175222865463283,0.5900606695262984]], -"dq": [[0.4647148717504217,-0.0012683031034861347,0.3468532030987812,0.7576510969559009,0.6956300336464714,0.9789647489881865,-0.57520851142901,0.13547532261195405,-0.29365507745723596,0.36899078083211423,0.028330301403755476,0.7608588995354935,0.8287244324292202,0.6084222666543444,-0.5950600525993226,-0.9787187505251616,-0.011507019975504518,0.7628240489608462,0.9175655414772164,-0.8929660717734405,0.6155792346305582,-0.2243678867373402,0.9956746327012398,-0.9106566040346991,0.7016856715175628,0.5153046591997688,-0.34612973930718494],[0.7848844918562594,-0.6584822905457688,-0.16062374299446214,-0.24825878108853217,-0.8515700594385414,-0.22975106240955645,0.4080467419023268,0.29642007669403614,0.11895423389808268,0.6664530220232523,0.9539484365037774,0.2834150007909628,0.7304348078674345,0.005838898972253981,0.41033057769828796,0.6813751462973137,0.8656138995918985,-0.5287541662429311,0.8402928753646197,-0.8848968807727637,0.7586589312594316,0.3545444967335212,0.860442712463918,-0.9197969480364727,-0.15389789225896222,0.6203694308325707,-0.6441083507773231],[-0.39631903263549817,-0.5730065682599137,0.7547115270757525,-0.3645438587573049,-0.10663432960540309,-0.5392895584413393,0.43976645383359814,0.13721895332661305,0.6415316984911765,0.7678993509325955,-0.8835396329087868,0.7395869232458798,0.024597636002641643,0.11038192145344716,-0.8065749528405126,0.08271091337989045,0.8802936883184005,-0.00042290616741458287,0.48370419826919453,-0.7251692087368877,-0.6046812557614274,-0.8479340450092512,-0.6684694190353178,-0.1549657413927521,-0.8672890050404802,0.8800328033990448,-0.7193775778166598],[0.914917539861094,-0.9841904742855838,-0.19049468158439442,0.7082155467690292,0.18903419981349323,0.3376623733673443,-0.3403967783105055,-0.557735197474249,0.6927378817031702,0.4897986411644184,0.255933896296926,0.8183259543008832,0.13164315887197486,0.31338205706574596,0.07357407090854928,-0.14185757648482222,-0.49072373470796826,0.20481891277286834,-0.2219829780805047,0.0700769548822151,-0.07436163478862912,0.22261695731246478,0.8558232828449113,0.7225104102030488,-0.9239480398893847,0.24341089059998344,-0.12290324395649299],[0.5076784866972732,0.24677678304756578,-0.22072724098396312,-0.8616677900212741,0.9660231678080575,-0.27846272712557196,0.5345245275260604,-0.2542197047049599,0.6530104210007956,0.8111691726886818,-0.8951712341806868,-0.637048605753118,0.46021821989468337,-0.43467904463879314,0.379688106965802,-0.4158781403003453,0.3228988519559861,-0.39322623726445105,-0.22518103034415038,-0.516254806738699,0.5129659555013122,-0.7442220560678818,0.6070084589340301,0.31922234513673664,-0.28565646426199076,-0.6878618082915904,0.7373038531000116],[-0.7916837969318891,0.9865879941034963,-0.07719599650254039,-0.10686209111705192,-0.7867886479554773,0.9827383811604617,-0.6860778557536718,0.505014708171543,0.7883323422911961,-0.31916428864983803,0.07487135246820409,-0.402932752372412,0.7683211400846173,-0.7196637176521821,-0.3141193694644493,-0.6019876151351158,0.6889213022015377,0.8522344083094782,0.7998664390086172,-0.5923277905082531,-0.31809626152711146,0.0342355823639553,0.4978873885532811,0.4103758586839208,0.4682242286485119,0.4862643800809612,0.5905323890321619],[-0.05085507559691105,0.5472788455517439,0.3863852350053849,-0.23057026152545257,0.914250116101182,0.2180292105842312,0.7495788169356907,0.6538847898307263,-0.4320928483927091,-0.10548394303210484,-0.5734073321822806,-0.8821162339892687,-0.45596329727628304,0.32280401037626505,-0.09235472780015508,-0.9158449301712188,-0.6856400471024653,-0.4655841511413983,-0.26434006116838504,0.4109959026826515,0.7122295120645232,0.8255769837097657,0.9961907196386082,0.20894680515883457,-0.955034934177045,0.7652189336227764,0.2069332895240774],[-0.888670580284473,0.11939127200928001,0.6494812822614404,0.7223330388038358,-0.34804135835677474,0.08533544857461628,0.2724263657605872,0.7345978354670721,-0.849760793176118,0.7519374336956908,-0.28202654160548235,0.6194145779232361,-0.18290432561751668,0.21528225228968045,0.8844141297022419,-0.4091408492806292,0.32048487319009755,0.3679885107265026,-0.4343946091799298,-0.35907477084688155,-0.6060904381021031,-0.2202235299693338,0.22017093418393885,-0.481186557642729,0.34299611220058157,0.13761802777396204,0.9472348797921601],[0.31665098641896017,0.5947023814600041,-0.4966414246391444,0.3468257890985784,0.8638802951120055,0.9271652361968759,-0.9958258981536932,-0.5703584441302227,0.36019314731632535,-0.4040016096724637,0.39122966062897535,0.3653178962464294,0.13536394174545396,-0.3751825408836622,-0.14501080067501015,-0.7270066870686906,0.09218254396445502,-0.41860427783903487,-0.9289276681559648,0.25622352762997114,0.050627386486848946,0.22943620168553935,-0.28223347895263906,0.3561058598531037,0.0177898080073271,0.8329934908156711,0.07567274321860706],[0.9848119844936332,0.6511475424314501,0.12194402337240029,0.9522305640637763,-0.49054024306595423,-0.22135416641930816,0.6902644421675459,-0.8255912306503159,0.771402725879879,0.28321136833885463,0.930172164363642,-0.24394148681521854,0.09142894415650393,0.34386390862636196,0.8140066700979689,0.4569565208423034,-0.12691725096867712,0.19312636072621236,-0.7265710140234407,-0.12620062298076906,-0.28650639322236704,0.8551252236174887,-0.5152096930959114,-0.23415129934332146,-0.6588306387278628,-0.4012665568163072,0.49799175381451666],[-0.07447338719470076,-0.4205176419848673,-0.7126647510755246,0.23002964633762613,-0.5869799872770405,-0.7901348546607976,-0.46861327047600754,0.6410155970497806,-0.4896433858578775,0.12253971474093528,0.3650807377944001,0.4293584787054401,-0.2551965053581655,-0.6644157126659593,0.9243211096730959,0.02206404407079865,0.027654194234368656,0.5139784683971624,-0.07698186690288567,-0.7229825049478729,-0.2040421681023732,-0.7871530289247393,0.5720014620463869,0.45485525886096156,-0.2971630415093729,0.9138454724576213,0.305004429501488],[-0.09224635905653877,0.9394226506113679,0.08610938962108539,0.18220586356367785,0.4542639745397212,-0.12852022013667352,0.1591483785445138,0.2320554730998492,-0.988944542251091,-0.33970993827005413,0.09896595036423617,-0.1928070357380478,-0.8525296020949473,-0.8218688334223136,-0.9553771186682205,0.09624603895974237,-0.3017287793464156,0.4049727253569231,-0.31400701258683417,-0.353889715646281,0.2745599300186441,0.7222053460012292,-0.3854268614356693,0.11634443226533975,-0.8058787339395235,0.5639489854219117,0.9758034786373007],[-0.779780024030251,-0.08234346156013306,-0.10949672788432374,0.5008229852715405,-0.49481453595725977,-0.5061648123279248,0.4919997702636858,0.11257871661765528,0.9757223672783353,0.9313991431771695,-0.024257629522673962,0.6539698110205638,0.9517789991920032,-0.20735718346379217,0.5343880245420243,0.09734070486947766,-0.12606825133666266,-0.7120560693036371,-0.49137859282845553,-0.7389446750390201,0.3164688460465537,-0.8594973764569702,-0.5795230884313483,0.9141694657663804,0.5976129185346875,0.18040521585938785,-0.9853613960371657],[0.30448263280133014,0.6394028137529331,0.27278189292994415,-0.8683824926312618,-0.6003463894216952,-0.3184126682663686,-0.20939768678018722,-0.5299194744736555,-0.5287744777201446,0.2498834568552284,-0.1355632243707805,-0.18425967167203505,0.222554746146034,-0.3436496534244562,0.056114496568907546,0.8791789730799027,-0.40801655640527734,-0.9049357091926133,-0.7198183010185439,-0.6524106111889005,-0.2697476856391303,-0.7963001207026501,0.33765008668921537,0.3434560897019283,-0.5404530907291045,-0.7587448640738299,-0.752252719605794],[0.40757339768966694,-0.45507443721285434,0.14971778951442505,-0.7457217184764897,-0.003901958256206761,-0.3196055568751448,-0.635503599664532,0.7690396343250729,-0.22693131712637094,0.03558143353421017,0.8324729762876251,0.6437193315244218,0.2296501406546967,0.20645658494614394,0.5243863239555893,0.5342809173632808,-0.597112951948015,0.9174314485324644,-0.6865375803708009,-0.5441541355336781,-0.7800345761714542,-0.04919799562060101,-0.28095259261648287,-0.6427788314745444,-0.9753504620978835,-0.5881604447328015,-0.20068079678938577],[0.29028265784773755,0.48031228858233854,0.9937858442839687,-0.031747204073761415,0.790015681290402,0.3363624279963868,-0.8255606342292718,-0.9687020771261501,-0.34027469055770676,-0.6237080118106761,-0.9546163679137842,0.17140691189176538,0.37263863390994767,0.33700807494043405,-0.25574154724150255,0.6933495274389989,0.5557708465116769,-0.5182023939068947,-0.2060827684163391,0.8784508386411498,0.5555577631146196,-0.014206632120622231,0.2850928573137952,-0.5791006073986862,0.2647272856115821,0.15592130086325096,-0.7654050415211384],[-0.8110080956853856,0.6569919549555543,0.9854528824353097,-0.346422831466328,0.3931138330804156,0.44830401115734686,0.784311071858437,0.8615523845610571,0.5177113954304677,-0.011106019735131145,-0.3968509879323878,0.7412307361353885,-0.84263029020194,0.120194882057574,-0.8749386657328326,-0.7378520716592574,-0.8939118116895781,0.14520010493726865,-0.853760950246272,0.462185600631569,-0.566389495484261,-0.8858047131699278,0.1949600031881964,-0.3297804201175989,-0.15374634205469562,-0.6726940597888942,-0.966589533093271],[-0.9520078725107972,-0.32557887782023176,0.6114573515858375,0.18052406648326658,0.783915548713308,-0.8587235446572556,-0.3881647832212729,-0.2978998461243567,0.19214863640554714,-0.33503765040938105,-0.34574079274180836,-0.08861614804142914,0.8510952093529447,0.6954896819528473,0.07412545297031992,0.5498202495805153,0.8224946081449747,0.013164721704958637,0.7112384311101176,-0.1984662195809448,-0.03409042774213211,0.37151503183701573,0.5512907402235896,0.18376510350069486,-0.2662193185436441,-0.9870991015285722,0.0288140884048409],[-0.7241277383240021,-0.8437874662965723,-0.9326634888428962,-0.16480907874863004,-0.424898469212458,-0.5840285259348215,0.4391319963487579,0.39175119653207546,0.9365671971537926,0.1479203230330064,0.381145781347362,-0.529251090715503,0.4600747089540105,-0.615389624446472,0.22223824992533725,-0.3417463596808996,0.987588448444036,0.2498611572125058,0.6633822842609853,0.05317039369534693,0.15682241959587717,0.9972847627059358,-0.729546322591849,-0.4850058040025498,-0.4465513667373957,-0.045340859855014015,-0.5300538620878867],[-0.9750429675109855,0.3888384338389297,-0.03783114668913545,-0.8273292608811695,0.6064279350526232,-0.5662366191600681,0.7842109352562283,-0.956946872308241,0.8675545150724837,0.06447404853806349,0.1637332446492492,0.5360211324343893,-0.7144486546983033,0.6377979792667647,-0.6215019645623432,0.8143887345869014,0.023786792286157876,0.9599459943130826,0.300702160407716,-0.2768605760066398,-0.000795140882134282,0.019145313397321395,-0.8873707084557703,-0.7362454233061886,-0.08257238808453238,0.10982130465897222,0.8384328992644621],[-0.9164766763483736,-0.11064958037428685,0.268304672611936,0.8759624855978116,-0.4788449690326668,-0.5365844507255997,-0.02831198585321948,-0.40988780160394667,0.7316530423753731,-0.2812643807203823,0.5843633368383894,-0.8879610684497161,-0.9694224035353838,0.3399876716159209,-0.8190773618371545,-0.5243318681121525,-0.5246818690569306,-0.0648604707422964,0.036635395560239425,-0.9493191044089404,0.9892085851874601,-0.03213189643576242,-0.8228352454388173,0.4345958170174635,-0.8961240685706293,-0.5891659323440712,-0.19010802290990925],[-0.602749674841293,-0.9498524178375263,-0.05754309920701428,0.5784156302129781,0.8161441384843504,-0.034001035651368916,0.32648457160458455,-0.5725165765682412,-0.4342245891748422,-0.8757991284653937,0.56599820764067,-0.36137653204471154,0.3184462625006981,-0.9892392187123138,-0.882628177115222,-0.7110859478051765,-0.6411028653968147,0.9807551485213419,0.2832875551228138,0.5402062649298325,0.9618515536930612,0.9622673547075402,-0.19628895762382714,-0.012334609351041248,-0.2935945034880594,-0.18485962419871682,-0.0824687919858691],[0.9895777237955152,-0.09586202643003716,0.14112926031572903,-0.05149385771094028,0.525931144257205,-0.039699333494956024,0.2777251485066079,0.6374660847811644,0.25307587767151074,-0.1045353985492723,-0.49699514872779327,-0.11192678432588532,0.5433797003524219,-0.1965281459083721,0.9334311354577773,0.5081229404448979,0.7106863889833717,-0.29456950480484023,-0.7740975705325497,-0.0925016622132413,-0.4325144330868125,0.7646460422650441,0.9301493199697934,0.37518938630421284,-0.34124833915448005,-0.5406266065063239,0.27629197267955075],[0.10118531764097893,-0.9671568782901871,-0.8046821793346384,0.43081735290883705,0.1440767996108212,-0.5689545455033334,-0.40777621481929427,-0.5162702085173507,0.8444419379133241,-0.7580486638005799,-0.09602266317572083,0.06616388250805505,0.7355222506282577,-0.7738445409528625,0.24079872875300024,-0.2881114713353168,-0.6987831421953032,0.18768078501778396,-0.04122314669483407,-0.7461006188864745,-0.5044075149203273,-0.4973843196018082,-0.5220133327538528,0.9204556914868336,0.8747094917863101,0.825503445525819,0.14867794161743997],[-0.18747357266128395,-0.7236213762157002,-0.8249811309435828,0.2317932634895572,-0.5462126313617959,-0.24266576999353262,-0.7749769739035453,0.9790828004166128,0.06080233738658558,-0.6862526620415481,-0.7537428988039632,-0.6599265680585531,-0.5259264132800148,-0.021786701161925492,0.955398311232331,-0.2678782595755047,-0.8638361018179694,0.3975370201496755,-0.30231038166670543,-0.5444788864836665,-0.9711906651391404,-0.8603119920929934,-0.23766874055627096,0.7768810260795362,-0.9647023283920568,-0.06763874998831376,0.7707153673727862],[-0.43214502741757,0.6167526867992152,0.5519144098266977,-0.4657932243948919,-0.30126117363660354,0.3025755866282871,-0.7194771767225601,-0.7650082878677114,0.030185859391824277,0.5117366414336868,-0.7093465293557115,0.6274560527051083,0.0334958969787813,-0.03512286477945037,0.21330596840969096,0.5739220389213009,0.562720307968247,-0.9472065103476843,-0.3209209536151383,-0.9841513084989235,-0.916875994999929,-0.8322038287404196,-0.5144231314510326,0.15008470057601597,0.9866740290516338,0.937767635943703,-0.6856338584929453],[0.8167949967047716,-0.41571213182401023,0.147554796847569,-0.5184169442755961,-0.6878162218386301,-0.3101360653094982,-0.08229982375869982,-0.6177450855945528,-0.5066093085523953,0.6698367805549654,-0.2727802527846255,0.06613253081215165,0.4905616174976466,-0.5512690786040324,0.5774813881733072,0.996647695740873,-0.1557702238738483,-0.18257827707470686,0.4192815409701989,-0.5019033474032255,0.31764126286135497,-0.9836792908458669,0.9865479173395058,0.4623045796548715,-0.49960298883905563,-0.6525739221830811,0.7238513059632554],[-0.13019826961577263,-0.7544879648250102,-0.2773653718293252,0.6966305007835829,-0.7166109640251939,0.8496565673342535,0.4166729995303935,0.2086226185367217,-0.8271212980916423,-0.37815029299376146,-0.4844736242930281,0.3822589881558476,0.8026399707816163,-0.643645282488148,0.9963248925208514,-0.15766255673066976,-0.9373936579575757,0.7225170136220835,-0.1431173968804904,-0.8223057177033075,-0.5499441240272056,-0.414083866763969,-0.24721799300325054,0.7014731316393654,-0.6137858355111818,0.5618753030385206,0.6139516239648835],[0.31438600947959916,-0.8291770525041904,0.8383831062246545,-0.6865133442868812,0.06828440316591688,-0.037610198138696704,-0.1036958979798166,0.6757929561829725,-0.2755983434793585,0.785739425107868,0.35416025544460417,0.6288179320937646,0.8753746517209411,-0.25154318013115917,0.10423625840781159,0.7360457965164524,-0.048023302054906125,-0.2075347039304738,-0.2737731661597582,-0.240629881976139,0.9324583241118929,-0.7059392168633558,-0.7896735745870487,0.9241264281400272,0.6900566060860402,0.005573073287169494,0.9823962973634284],[-0.3308595269297112,-0.7792429188240955,0.07621670636544153,-0.7125442140420242,-0.9404874146024285,0.5361717357851337,0.029714159805963813,-0.1946000790447231,0.40592230194968093,0.00017985869857994885,-0.9740039190599872,-0.4975096269289725,-0.005429477805248828,0.9083013978012446,0.9676682400174887,0.33325623594953835,-0.09516219118450464,0.8581968275021343,-0.08405908494887004,0.03871030985153556,0.6858341854710615,0.5053508196470344,-0.4358327629436545,-0.17045861628774528,0.005037181171792859,-0.5566327887790032,-0.32902655946439685],[0.9353371097396914,0.30109992762328,0.7246998394309552,0.02590665819788085,0.7523739981333288,-0.3146859647314455,0.45003929400039744,0.008043712828731397,-0.9801632988945037,-0.005015399615963645,0.465347045748,0.8798739202838048,0.9312955524282478,-0.3430907131291592,-0.01978155581136254,0.14288037400825426,-0.08171635652265596,0.4253877052059418,-0.20908792032185564,0.9007220566811625,-0.1858213154313504,0.17179358358111552,-0.6846024394222272,-0.5923968814401668,0.6608644492639828,-0.545879987180935,0.09160554038813018],[-0.0029283075817754245,0.09835933591797485,-0.14471156432550458,-0.7149698229390862,-0.5987715929567528,-0.6894632208228737,0.18061667619111588,-0.6595186683889576,-0.47732281532592435,0.9023686492152299,-0.4690258471850093,-0.5338092123817297,-0.9353789262404764,-0.5873715478202002,-0.02995544432665298,0.15731921372551727,-0.2170311829823277,-0.6825480025435136,-0.7629760647166888,-0.26728458906453123,0.42555204022048754,-0.18678299024309264,-0.30528445110562297,0.48044919077973924,-0.6764909198545952,0.16944675827317668,0.43770919509935435],[-0.12569100862164584,0.15036575409738595,-0.6054446916992955,-0.02858986267870889,0.31901283837028194,-0.9560634976764297,0.5430599916489536,-0.265193582267905,0.8949227585431265,0.07610290604552628,0.9186257849347859,0.15876967000850017,-0.7403812772780329,-0.8262941634786765,-0.9530543078210707,-0.8000185243982405,-0.4009127385300113,0.14407010254376806,-0.029595105260078913,0.7896339878807084,0.2030993176317717,0.05944042130458316,0.5905907983432872,0.6103841144660247,0.5178249807327298,-0.7608658827454595,0.0007413280416082024],[0.4115824440965503,-0.0004887047293755664,0.1234721588718759,-0.23498130426522112,-0.08817496586146789,-0.5351723138320341,-0.9112540211658537,0.12369265032118215,-0.3851503292907261,0.9830888074366286,0.7209618982203199,0.015875526700289466,-0.08009192038671187,-0.6534082851715501,0.44050155637480093,-0.6170664329793247,-0.5168387768184313,-0.7710275309843964,0.23509605847394432,0.2859618644959223,0.2129761251068898,-0.6568453832203072,0.277631698801484,0.8169430928617805,0.079263266003057,0.6477652504120033,0.14699116928170808],[0.7800810902537128,-0.49554040635844165,0.8246331723839109,0.2448288868912607,-0.6049726330235639,-0.9328996898881161,-0.025364268969672477,0.4530987632887755,0.37741830301068147,0.07974220226292195,0.15193950778523035,0.45945686898105276,0.2296070306432645,-0.49279422378229865,-0.03952722767826944,-0.15021889853990977,-0.16573662092141994,-0.11631385705978037,-0.05706461211730285,-0.1776982204707711,-0.8872837739850536,0.04554355300640767,0.2056445569842038,0.0471239505406591,-0.3763107978098852,0.7197174088845639,-0.27897268475143333],[-0.1486041226243595,-0.07529278762240765,-0.3405433240512121,0.5891766802167775,-0.5057162468292693,-0.7797247412076707,0.22837840349674954,-0.262921554642382,-0.20412592725069034,-0.6309118690987889,-0.6626009645625885,0.8900794208447542,0.15157893857306926,0.28665385694812806,-0.17518366577160038,-0.6932461488284241,-0.07739203019631469,0.998015156151109,0.48148732807726424,-0.9594860860825489,-0.16335591372524205,0.4550693612469079,0.8797759166594299,0.12601759794993073,0.9509745794763069,-0.2651455934641558,0.837824143631503],[0.7230852008781421,-0.6408133324430088,-0.9091492369898289,-0.6013173237368943,0.773503712367386,0.8624261379256515,0.6148492359804876,0.08779634528670854,-0.23813098358050588,-0.42567450892483194,-0.846625344256503,0.7338371072530465,-0.5174323113168504,-0.3594109366815479,-0.9455613906168692,0.9022236507255252,0.9055218083417313,-0.8244143829349146,0.042260775135178674,0.20043071863095596,-0.6329916488761782,0.4427216923163493,0.008501192076713737,0.9113252803100627,-0.16747067290546713,-0.605686070015055,0.14730749598288795],[-0.3525364309196004,-0.7177745449232915,0.8751843077059622,-0.8206771317641544,-0.7959588243581197,0.9568508456569753,0.737858240526374,-0.9782495366149222,-0.6363829355825175,0.2663628818687098,-0.24752752407681977,0.04321633597619723,-0.6181731065897362,-0.7647932357670459,-0.5935464598908378,0.2009917374098762,0.5664786499236651,-0.6668924107981384,-0.6149808637968126,0.34691417669797797,0.36693523310070697,0.6483040358575058,0.44070663067670957,0.5699836234890101,-0.5922589072775741,0.23988593693132776,0.8499040197877268],[0.5127342445323504,0.9601659821997965,0.9539758769826241,0.18816560330231757,-0.6235850011283057,-0.6905816983233113,-0.8325600122192354,-0.6473047586553065,0.2673511187390547,0.739330292534067,0.8799717089993542,-0.2177485344941883,-0.29876419136661636,0.42049096785518625,-0.9989407071232459,-0.9007861818051208,0.0673257376508527,0.08790629132317607,-0.5012387423604372,0.5508623326189956,0.2287537266226607,-0.9878009233715148,0.2635853922919833,-0.4522966976811611,0.15283337761315474,0.20855968923232382,0.8912921432022671],[-0.26245374118404974,0.20714948368021036,0.3740485609892714,-0.021614711161792144,0.5233943148549012,0.4346388697171435,0.7146660976232544,0.03308499514960084,0.17624653973976812,-0.8860439317190563,0.9461861401211851,-0.14374631073663635,-0.8259454698212609,0.8872572860184058,0.27245150572820465,0.6089983113533421,-0.3549089570233597,-0.08417906327766067,-0.30701245297720536,-0.763329852399987,0.6129680253384486,-0.5259472307111295,-0.7811114808375348,-0.5788199538144494,0.6611736108581296,0.9701509237447894,0.3845248949356681],[0.07675519460079827,-0.9101273684281515,0.8922940119684188,-0.9308342602141224,0.24698350028578275,0.5376905975378272,0.3328108031833812,0.5504890238562261,0.8681789693125808,0.8469061588285371,-0.8264536809102137,0.5489141197102609,-0.9662244579431365,-0.5140385061976871,0.052993667125991406,-0.862760616161782,0.11570702325562232,0.3996461396718529,-0.20667218928199604,-0.8963754764025214,-0.8741160851656506,-0.6589922051216643,0.8870287299658801,0.6611937539562018,0.9353497628090681,0.26205705698432835,0.8556385599599541],[0.35354701442184133,-0.6482889278835073,0.27785573534051955,0.5573084307556555,0.8116891470584877,0.5870377955767097,-0.6862801410404475,0.60864030643986,0.8421728696051414,-0.9395162762085361,0.6685926562754883,-0.3854477469945954,-0.7119530715486251,0.3509900401172503,0.637381097029565,0.7232908954704538,0.32350668284974904,-0.35020014413109246,0.6722577350563941,-0.258444764693361,-0.07665732412819048,-0.9574948935437775,-0.5560812820260523,-0.5096019969915588,-0.7020078267907894,-0.6766186618810812,0.9004918298678142],[0.3026691158099848,-0.19858487468186925,-0.1376027879995556,-0.7478159539361076,-0.7342645047710086,0.08842512376326028,-0.5752283750370275,-0.4745902953070018,-0.6499944096732122,-0.6650097067475058,-0.7015657027529623,-0.9035140102362269,0.7377701062416038,0.7232436913485971,-0.5375289416647726,-0.9376077337106301,-0.384969181823166,-0.3667666465130024,0.23609070027045953,-0.34959552227288127,0.7762883131163105,0.23421042886605803,-0.4427376247786847,0.4246873668121718,0.7772843435249406,-0.9813705074908556,0.1251298631708775],[-0.7559619801199584,-0.067295230772219,-0.8148856261240363,0.6532303426924513,-0.7494600318435942,-0.7307997930500585,-0.2605804276762014,-0.18582058779324617,0.9663642833461674,0.9826176894729393,-0.9689156527259293,-0.2762128318779349,0.03977235732685136,0.3395456713658247,-0.8187752553793688,0.23526446544325164,0.6549734660656417,0.05697311268290606,0.5702623657125832,0.38320791572626733,-0.20339707055052259,0.8695941863130532,0.009752642814417234,0.36489198699894376,-0.3249516151912881,-0.4269987827126751,0.42560852814264316],[-0.5191963742049333,0.6269376203149875,-0.979417617510894,-0.4344899635552817,0.6202883101888701,0.17397011974579923,0.5863022979368679,0.48919790474856684,0.5178162170575462,0.4036416573795951,0.15347167926043004,0.4054124376213184,-0.20886496877320604,0.39531769559769003,-0.36565995978068533,0.47972599731706467,-0.592979674196427,-0.34799305408786374,-0.02691112497741166,-0.17685618097703149,-0.03604414148641122,0.046319831581020265,-0.23492435316835603,-0.29035207342830627,-0.9910529474331198,0.11304505890438565,0.34604473400324465],[-0.3743136967101459,-0.25651699504952363,-0.7344834813529211,0.3911001447316358,0.4594638646660236,-0.756420517352304,-0.40593698999485617,0.4231202168621635,0.24489152877511455,-0.45164107208389637,-0.2520964719840739,0.6777912857256772,-0.27518575470997053,-0.06557829216428468,-0.41806693902611425,0.46558456933663717,0.4116044651873465,-0.5136474337869588,-0.8367702779532269,-0.674513481605514,-0.7761794885883835,0.20085846188787237,0.8847347811221569,0.21514330730143327,0.7255172103475294,0.2355846343148349,0.5078827740579341],[0.7910395132327219,-0.24851744855854885,0.16896777051956668,-0.5906692037639472,0.7585998816422888,0.5891165425381928,0.11666569553177064,-0.4886148125139327,-0.9378253349422432,-0.7474545248197797,-0.18534143958137084,-0.10661074568486817,-0.15719890624758603,-0.027097249493483133,0.7357777679643882,-0.5415694285220696,0.7250712780525539,-0.025162274381356875,0.35258624032181185,-0.7564841400977356,0.4804939068980032,0.7822883690962503,-0.3901010555897415,-0.7895168330029168,-0.33332934481574017,0.7579463176742371,0.3477423805242046],[0.853902345806159,0.857268890938724,0.032322919224280655,0.8316363913726159,-0.7285734680037406,-0.7346086511533387,0.8617039611294575,-0.24265482330264265,-0.16303119046155734,-0.6345301558962531,0.8217260413779897,-0.7146581937219905,0.280401435425895,0.868214097716971,0.39348959658851923,0.7390596910865999,0.21938775139777822,0.9175187488249072,-0.8254909099881502,-0.2095615545948153,0.8557175634986838,-0.4790569874804895,-0.9438343003754808,0.03139948751362365,-0.7529130209637853,-0.28695812102486196,-0.7662121268394981],[-0.8352648840665453,-0.5718521591492303,-0.17048475283946063,-0.9410750979201481,-0.26600319174383547,0.2068588817644661,0.7133228821528068,-0.8117277937584173,-0.6196300439399429,0.4947015917589639,0.10499271272123822,-0.6059183472108987,0.6630637431073012,-0.28960038964900603,-0.19891448754008678,0.3869847042355665,-0.02950503178842223,-0.8587942614766331,0.1208603721311221,0.0945132718770274,0.17907100080130767,0.36499370655229146,0.33160714268081626,-0.15933479728269617,0.37395481897104976,0.5066698048571243,0.2188755568911891],[-0.6058668836319336,0.5567793342676011,-0.24892276190382234,0.7530476432538102,-0.5311819882000492,0.3329743142203887,-0.4136217097710291,0.9140374834143217,0.7140737487755038,-0.8623289827437672,-0.4900063841060913,-0.24740254342321277,-0.7398912386194061,0.9007554711841941,-0.602437155780932,-0.749140600582086,0.5325120631215818,0.7113489841187146,0.843512653582001,0.46849967242251767,-0.9949276048023175,-0.3595023102462007,-0.13340353916274106,0.7399735955016966,0.4486989583500569,-0.642618454422101,-0.0015342594976903712]], -"frames": {"left_wrist_yaw_link":[[0.0641286696195158,-0.16120012461039265,0.6821365951031123,-0.9550922642055143,-0.07686924354090241,-0.2861640897321557,0.16599267675493845,-0.938799092499668,-0.30183223019672767,-0.24544897253505538,-0.3357787713945735,0.9093994823850716],[0.33110414715323705,-0.01434739590631804,0.8818194734421836,0.2157247510254601,0.3749209874945753,-0.9016080550500291,0.3474996915586871,0.8334217247040421,0.4297117558991812,0.912527696106601,-0.406007982590777,0.04950476654774566],[0.6698336558826642,-0.024007050914575338,0.21927023394196657,-0.03313025221397181,-0.9816523686399952,-0.18777916160140418,-0.5253781243830669,-0.14272438318243164,0.838813195452316,-0.8502236251763235,0.12644515644611343,-0.511010185420358],[-0.0035896370668951974,-0.0812202694719797,-0.07300424211669021,-0.8028066469822482,-0.43374031132859037,-0.40910980175216705,0.03059494443038943,-0.7152136356107197,0.6982359234612597,-0.5954539754980408,0.5480317488659525,0.5874485214029433],[0.26863124018682555,-0.5038591742353536,0.512144822284118,-0.8916328355616439,0.0500119616817018,-0.4499885445620264,-0.16224312868711166,-0.9631701428360929,0.214430508890218,-0.4226914403464071,0.26420083201581407,0.8669082227202783],[-0.09857756842560014,0.6901582964480737,0.2867810898736498,0.7405880557017444,-0.13993009810733198,-0.6572281943096954,0.6460286998763577,0.4173569562183008,0.6391088248743388,0.1848681981520166,-0.8979046378874918,0.39948843609316026],[0.4960493587268476,0.4645773895935958,0.3642224693462169,0.677176108948669,-0.5027567582260295,-0.5372784748407422,-0.1877367137653106,-0.824059472419014,0.5344912648688456,-0.7114685121449451,-0.2610778197674984,-0.6524192886876565],[0.11521144267944626,0.5436906214172289,0.8205636636513207,0.26472246745576233,-0.7379103984513918,-0.6208141904639753,0.40529456824203874,0.6692981377092442,-0.6227168825497164,0.8750190444656805,-0.08676546960770654,0.4762493308196053],[0.7413962562630018,0.11591171979269063,0.4993759912039567,0.8532175761471469,-0.41643207251144904,0.3140128926292788,-0.33908725170096554,0.014548938325586819,0.940642420969565,-0.3962822270648452,-0.9090504152011688,-0.12879339709437296],[0.08325176177907186,-0.4438602385573847,0.05146388019021086,-0.13572093109459565,-0.4400159440656107,0.8876743760134485,-0.9547879647248585,0.29728443858300757,0.0013802148195914965,-0.26449909504453556,-0.847353486771864,-0.4604696484853116],[-0.006619631419468684,0.495710574414725,0.24216605323565898,0.8400218533009509,-0.5413015556923665,-0.03682270742164129,0.5262155021643308,0.8293785033707539,-0.18769268347069185,0.1321383035261806,0.13828927634315002,0.9815373374404618],[-0.2833768857415974,-0.3543023908287376,0.8516358742668201,0.4499745399898606,0.7895288716383948,-0.41733328912311385,-0.14712181092836316,0.5264692227742218,0.8373680972073546,0.8808394212965677,-0.3153954950865815,0.3530546637123178],[-0.17383923112411703,-0.3806714985146895,0.5851529790492166,-0.17800926174633652,0.9077843582739821,-0.37978975974296436,0.3624902349641206,0.41930591012552665,0.8323360999556971,0.9148297832319305,0.010493455431744458,-0.40370330083481],[0.028121276783603624,0.5110727186910934,0.7304569788863436,0.0768694966434214,-0.5809473251008996,-0.8103032061789569,0.9964227885850746,0.07338066618254206,0.04191544129494426,0.03510992557853992,-0.810626599174192,0.584509888570889],[-0.5162284218163327,-0.05029484277719066,0.20667053743162497,-0.26102336923054886,0.4999533772481249,-0.8257804921973676,-0.469791322779167,-0.8130906227983553,-0.3437728205643718,-0.8433047573427612,0.29821176988103165,0.44710941227790035],[-0.17046439040125688,0.5792911880413765,0.6132967760990063,-0.9621671407232236,0.12816336708193293,-0.24043407547749554,0.08222478311637971,-0.7047247918080026,-0.7046999736431324,-0.2597565751890809,-0.6978087984178684,0.667524832868898],[0.3084679047810981,0.19785348525936544,0.3045025941656194,-0.3492796726764318,0.15689155048194672,-0.9237904262560939,0.9370181743743835,0.059331329068159425,-0.3442044948618529,0.0008069368884841277,-0.9858320520141232,-0.16773346140374465],[0.38227413677860694,0.17811436902995526,0.20119751292014182,-0.72038021398894,0.5982152682237966,0.3509855269910433,-0.643404486214089,-0.38740832620401855,-0.6602616571537268,-0.2590038887986695,-0.7014650965313473,0.663975680228903],[-0.6951984728188826,0.09504833509472216,0.6116093997427372,-0.5751767549273341,0.6206477086309999,-0.5328865942790746,0.7988925344622375,0.5662635633524284,-0.20277153449119234,0.1759045734816583,-0.54234859508484,-0.8215324597590387],[-0.526330202798911,-0.02806036769669768,0.683166608753431,-0.029405328726813007,-0.7307750253483394,0.6819847424756674,-0.9596075436992846,-0.17034671651904149,-0.22390926342294842,0.27980115925044724,-0.6610218290633947,-0.6962481258745264],[-0.04649971102040228,-0.2848080017034182,0.022258342389044002,0.4065570390881882,-0.9136156383338471,0.004223666615966917,-0.8795032500599846,-0.39011709988681603,0.27254849386819957,-0.247356841631214,-0.1145212371909132,-0.9621327762479008],[-0.23594766501077966,0.6099703176767254,0.12439841496286982,0.11839144507278179,0.544479971170942,-0.8303764367606229,0.2773437357230301,-0.8211082745325764,-0.49886035495850867,-0.9534484348871586,-0.1712389047145108,-0.24822030441404688],[0.4150667200745169,0.14635336501632884,0.8731988069482591,0.7317632276977217,0.5348527578111978,0.4224394702808541,-0.19119437990573007,-0.43383431711089593,0.8804728811209267,0.6541920877554817,-0.7250657299574148,-0.21520315880452454],[0.10354964582651693,0.2777401889651975,0.10224269806549635,-0.5260147547472901,0.5831940223648601,-0.6190260172771767,0.41468730199823445,-0.45959510398920655,-0.7853704743307937,-0.7425246927541017,-0.66981868641226,-8.93497680440336e-05],[0.32116784138582244,0.22037533962706252,0.8978533214922908,0.838973801854854,-0.2554767191904381,-0.48047331429841345,0.4411484072392403,0.836266187797259,0.3256485005912368,0.318607976346314,-0.48517059793306677,0.8143085706965003],[-0.44386469964122627,0.19402012365154228,0.008893281053339322,0.1355688034513473,0.6705229597712269,0.7293970523318584,0.41555201185281665,0.629827743994195,-0.6562267430852382,-0.8994095980007366,0.3920662868888583,-0.19325217129268413],[0.4685588341501468,-0.011933038548998628,0.25940720919724447,-0.5208724881678857,0.8494246234392284,0.08467384581392787,0.4469648026808699,0.3558929395931914,-0.8207086454474377,-0.7272649560058884,-0.38963832542864923,-0.5650377501750758],[0.608820239927144,-0.027931424983089467,0.3834804555978535,0.8965642375105777,-0.008621239776983924,0.4428298118259252,0.11801166338479249,0.9683176551427661,-0.22007763640127004,-0.426902582941625,0.2495728209631868,0.8691763869976256],[0.3505137440276886,-0.05191010540753472,0.9110091005386799,0.06841030998405406,0.9753341390957218,0.2098650676084351,-0.42109046999076216,-0.1624733213698768,0.8923481584706721,0.9044350974895017,-0.14941799408855766,0.3995890607508009],[-0.33018293365266876,0.5325851753269589,0.3798435499128618,-0.9964548134289911,0.03583333498705152,-0.07611686342657731,-0.044737938506443656,-0.9919211234149262,0.1187055254883458,-0.07124830982127325,0.12169000380876151,0.9900076875058272],[0.14708203091355396,0.32244563579032315,0.7426438532684941,-0.6673645422132051,0.09975299821643246,-0.7380202620141211,-0.7258429253606378,-0.3088960507957508,0.6146017226681851,-0.16666317979827133,0.9458501832476547,0.2785512795696829],[0.5431940594504047,0.27026879484123206,0.1318483048686904,0.9068698568350878,-0.40507942879321257,-0.11617968468054946,0.3806615579039271,0.6691586774129971,0.6382189614678052,-0.18078672821328023,-0.6230066779830321,0.7610380004247386],[0.44021660220877934,0.5424761224996182,0.2952473367896339,0.3356828841559224,0.6124367003659077,-0.7157082431617517,0.3159820582656275,0.6425616618889518,0.6980471685529042,0.8873963827619421,-0.4604734506086406,0.022177942771877047],[0.5542956037764918,0.4890102963466463,0.4438656993548574,0.7835400399422743,-0.6017646559415188,0.15473947352516332,0.617922963407262,0.7285907968632922,-0.29551084924262994,0.0650862281995723,0.327161656655694,0.942724261548972],[0.6008918699484683,0.08723069699768986,0.47544997722337873,0.45333875575677773,-0.24551244374004888,0.8568591555779084,-0.5254567259474524,-0.8501237704724337,0.03442098247169288,0.7199855565811585,-0.4658467718537095,-0.5144002172122378],[0.23474506229626688,0.5737412286440678,0.7511217654179473,0.8394309546722222,-0.19995794581321397,0.5053439346071561,0.0897388942124133,-0.8660903867982869,-0.4917665836157715,0.5360061596694715,0.4581530986490803,-0.7090790752762566],[0.5232929809042013,0.41911635712367423,0.31888420313848026,0.04925807916881281,0.8631340601898229,0.50256664809439,0.5795343277786409,0.38511151188025394,-0.7182124242474953,-0.8134578074877382,0.32663238902095504,-0.48124596401393915],[0.7054157038880079,0.2630120738422371,0.5120118373623448,0.3471544263378271,-0.8176588823624129,0.45925783212472704,0.8467843311987148,0.06283732362603106,-0.5282118582499475,0.40303858464221454,0.5722634208591069,0.7141949848859438],[-0.3344334177639068,-0.5806529937688154,0.20336107500367173,-0.761295116335113,0.12378000064721151,-0.6364811523400224,-0.24062404710078222,-0.965447650427667,0.10005449635342764,-0.602104487444785,0.22932367022065933,0.7647750260536617],[0.4403593953289799,0.08659758561673338,0.7333258671973192,0.7001156851834597,0.529858231647967,0.47863167646425625,0.7050360254253016,-0.406934289399643,-0.5808000404298378,-0.11297004121707956,0.7440797930771249,-0.6584702205276364],[-0.04700628910099953,0.6636030163943146,0.5453571056477287,-0.3019992423668747,-0.4335494568597651,-0.8490178596863769,0.6669211208351846,0.5402936683849326,-0.5131266612514824,0.6811847592201213,-0.7211918055273197,0.12597501119853108],[-0.3015834064854023,-0.22554870068387065,0.844282543157328,-0.418858746531251,0.12341264836814947,-0.899625849271256,-0.7313063306619645,0.5414421641897363,0.4147667218705814,0.5382828263914825,0.8316307480248035,-0.1365353351762941],[0.27022885311502154,0.3774941792387718,0.6015133820728075,0.486533367086008,0.6997907637814843,-0.5230470051898573,-0.8640825305683397,0.47384651047324644,-0.16979653965538255,0.1290219480498026,0.5345674620087296,0.8352191122591602],[-0.18741370677057878,0.4958460174979219,0.7102130951723988,-0.893732405387593,0.17257673599342832,0.41407687420811295,0.31043634055933594,0.9042584087103118,0.2931654971800447,-0.32383895074594543,0.3905560145107862,-0.8617391330960986],[0.2529237679415,0.498132179900686,0.8181273110220563,0.8753091319843829,-0.04286096436437342,-0.4816605248497145,0.4439028069781138,-0.323840852829,0.8355102632499121,-0.19179213076061807,-0.9451402222806635,-0.26443475339969996],[-0.17765613319078735,0.4587570480673684,0.8887458622592149,-0.3239550847634409,0.5792548714267154,-0.7480086209291517,0.8915832502438292,0.4513760416514049,-0.03659203339196798,0.3164370568384201,-0.6787661327375789,-0.6626795048195695],[-0.5956470545249437,-0.18588236301459138,0.21247959355776308,-0.7516630226218013,-0.09163234407948168,-0.6531509886248026,0.19106606528370196,-0.9780901221690763,-0.08266481483851545,-0.6312657595023363,-0.18693107401869102,0.7527020090621236],[-0.5643324225931586,0.4664463796094916,0.4069323979458802,-0.8391112806202784,0.5431786892983861,-0.029140525525184813,0.33566437756484313,0.5592069586835405,0.7580349615907644,0.42804402153260257,0.626294251013046,-0.6515626038825466],[-0.3324069566131872,0.1943231306064131,0.01748410882134903,-0.23130475083345217,0.6374315291339264,-0.7349688142417058,-0.9722765443685983,-0.1780903644465484,0.151532647841818,-0.03429917655763004,0.749643160281797,0.6609528717920731],[0.608066262780664,-0.32828345431094247,0.5382678069556021,0.6248337709749242,0.7216979992750251,-0.29788379696063,-0.7144245740705849,0.6823942761956447,0.15471127877270546,0.31492901837290177,0.11614667303822436,0.9419817746266816]],"right_wrist_yaw_link":[[-0.554546777418899,-0.049391110630126474,0.6747037550608797,0.5335789307830141,0.7775646135930188,-0.3326962523269457,-0.003352498656634173,0.39531556086210495,0.9185392577854454,0.8457436286353048,-0.48899783131238816,0.21353883860920636],[-0.44758513861381943,0.37417163195105413,0.21307314148483725,0.6238180999374654,-0.14656527844622366,-0.767704107937682,0.7693872219520262,0.28791150789302433,0.5702194896000214,0.13745646906331638,-0.9463749694369279,0.29236986222232203],[-0.6303767885692858,-0.1871979143024647,0.7336762987475871,-0.28065750588422744,0.43175363585142706,-0.8572165200927466,0.07313087972691656,0.9001356260710045,0.4294272104887259,0.9570178884541354,0.05783297162375839,-0.2842043429857354],[-0.24715166368282385,0.4938601155293653,0.04774508279411952,-0.31899409578678234,0.3485471843890971,0.8813385428469617,0.3104116374115335,0.9170521401973604,-0.25031977053147847,-0.8954816481719007,0.19372721134077636,-0.40072732047298987],[-0.23856456502187343,0.5390860468364574,0.30996785601956955,0.298956093791548,0.27836015504635564,-0.9127655109980153,0.7743964315593639,-0.6296946068636939,0.06160250705045722,-0.5576158361928243,-0.7252587994428933,-0.4038121507057834],[0.21150658189781352,0.04301939864245577,0.17206843299780633,0.6455169653080393,-0.21360914061728523,-0.7332659698528528,0.28004890138290034,0.9594199297031369,-0.032954685898340885,0.7105494073839835,-0.1840775204449816,0.6791428466332373],[-0.15601346763443752,-0.5664128690804853,0.6164546289944409,0.23609077790117694,0.03643710995644239,0.9710476206695733,-0.781564229222997,-0.5866839333550247,0.21203612414430564,0.5774240211396472,-0.8089958586798196,-0.1100327244496896],[0.37437773613095665,-0.365626010018538,0.3646000932433103,-0.5855872661791104,-0.1840300924523888,-0.7894431447297777,-0.7616013453521748,-0.2085864372913076,0.6135593605641401,-0.2775805188528778,0.9605335096966086,-0.018012004421567085],[-0.3285184231893542,0.0469499921262206,0.5994087333213195,-0.22279711480946585,-0.9716702370948591,0.07885680678671994,0.40510645894662534,-0.01870517908673569,0.91407815486153,-0.8867075067643804,0.23559937737471537,0.3977973489458452],[-0.11154827350310043,0.21132532628569425,0.27721626528462134,0.8887076038477384,-0.10426850317675845,0.44646038358233964,0.43432620132123045,-0.12040049239367698,-0.8926726568441706,0.14683169177354538,0.9872344203261141,-0.061714290195060376],[0.4071350530454864,-0.43167527870133565,0.6977615131038183,0.6893297212066987,-0.00035867010045292913,-0.7244476563678388,-0.6357598507327439,0.479133196731341,-0.6051783142068836,0.34732398082679905,0.877742132488674,0.33005272487416343],[0.32225149419612836,0.5620184921888143,0.6800141746974675,0.016146975724939755,0.09587956852748217,-0.9952619672799329,0.07557432976295453,0.9924273117786584,0.09683259532133921,0.9970094261619689,-0.07677980968177717,0.008778665582631524],[-0.03024388714355905,0.2522664593654328,0.25366126596961397,0.6423139021665158,-0.14558106978901197,0.7524885402467671,-0.7438101354454447,0.11842157300275888,0.6578167020197809,-0.1848765358076913,-0.9822334158267265,-0.0322208525819588],[-0.017105236513811812,-0.041920092255713005,0.8004246762997052,-0.8412385736375998,0.48951536291501363,-0.2295481903530482,0.3659186016515954,0.20292936812765625,-0.9082528549455,-0.39802165668405254,-0.8480532890222718,-0.3498347892777237],[0.04777078823727838,0.44016906927741173,0.057834824636337503,0.15469891127927252,-0.45158719779354173,-0.8787134058599451,0.7725437751643355,-0.4991111478790353,0.39251009861875996,-0.6158281921963663,-0.7395654567729949,0.2716589274150716],[0.3399575384569442,-0.1598305954965867,0.6683100563961043,0.46278522113159726,0.8644919267327152,-0.19617223992230895,0.8111848897315359,-0.3237364315509095,0.48700492559923736,0.3575037255378104,-0.3845106389953869,-0.8510832243241229],[0.05508563921342821,-0.5539894694423885,0.16741767114180664,0.5590813556727108,0.822738662554494,0.10261155333200231,-0.13678549260218015,0.2135919044695164,-0.9672994507176482,-0.8177516534698923,0.5267633163794985,0.23195396475190866],[-0.06884138979887403,-0.6657490090886287,0.7363529166740207,0.6147934660993692,-0.7673652220972733,-0.18215271054015647,-0.7766322194502523,-0.5488003973173543,-0.3092903484043582,0.13737317697898266,0.3316153491922781,-0.9333594540297883],[0.2926828184602531,0.2609532034446129,0.9291928975306804,0.2064862457600185,-0.9779814216091248,-0.030261680378472024,0.2790563729902742,0.08850591310004302,-0.956187347772308,0.9378117993528586,0.18899482091772576,0.29118720208970483],[0.11762187829756643,0.18060419316433096,0.8814628459732454,-0.735493511031404,0.6142205561399632,-0.2859762291621667,0.3232029604354694,0.6890327614811116,0.6486707176768981,0.5954738798607764,0.3846647397680579,-0.7052970270621635],[0.5209858508605519,0.015539782257311015,0.9012102572245821,0.757906227566307,-0.281360415445469,0.5885698487321307,-0.43127014484484055,-0.8930331766408834,0.12844379153623295,0.489473403140273,-0.3511809534115512,-0.798177753122219],[0.4162774090960714,-0.2623286763881708,0.7986957056196587,0.21771536336012054,-0.5738402278853101,-0.7894982035555913,0.10894083276706162,0.8181348206622412,-0.5646125310121813,0.9699133546420473,0.03691623058354909,0.24063515205911296],[0.2469978430035332,-0.5502516988634725,0.5381049490373464,0.8020298902511103,-0.5767297541479067,0.15534106290445823,-0.5722732686085522,-0.8164742589864996,-0.07663609103112032,0.17103029316432744,-0.027433102138497464,-0.9848837818378283],[0.456285257911109,-0.47912895097028235,0.16725482832943217,0.7643296980580889,-0.3916623855631587,0.5122506109331705,0.11619144129157027,0.865051875416887,0.48804180334097824,-0.6342709686222587,-0.31350570740052247,0.706692655820251],[-0.36905420469916767,-0.35234847875403363,0.32334412327943224,-0.4593347188025493,-0.716105351062849,-0.52555184547458,-0.8026480998144447,0.5880527569968674,-0.0997496007643364,0.3804834345498427,0.3760147353063934,-0.8448936470737713],[0.5956706923388347,-0.309535960189481,0.5985093125031504,0.3495731164389035,-0.08819836977051186,0.932748456891255,-0.41268182813922827,-0.908274775387197,0.06877965628731669,0.8411256416177416,-0.40897185717988094,-0.35390630829074204],[-0.41010800177565887,0.32817916403285974,0.5153795041345841,0.4666584798915226,-0.8106123141768979,-0.3537478469900661,0.8360236337381252,0.2737762507109105,0.4755113546257102,-0.28860760034454136,-0.5176429663884662,0.8054510614381949],[-0.430112326271194,-0.53983746180517,0.6328261782116669,-0.7324182637433763,0.5426481281016133,0.41121344336360527,-0.28218297105740175,-0.791586987837028,0.5419989036264913,0.6196259014763308,0.2809324647578415,0.7328988282598959],[-0.09309762137120023,-0.46936042173197445,0.5976478040203742,0.8810408112090629,-0.31333498420585704,-0.3543843628841318,-0.4300382920964107,-0.21844328922924444,-0.8759849295059313,0.1970638381714896,0.9241773190591858,-0.3272034942076521],[0.16862087485887045,-0.263856910257568,-0.03427633342552568,-0.5202348743724162,0.04220848669256849,-0.8529795537630661,0.29515837826359426,-0.9283455571591335,-0.22595587676292656,-0.8013970347009785,-0.36931418894218104,0.4704995458217337],[-0.18435294863953572,-0.6266745546213002,0.40383017806780486,-0.060653101640751717,0.882553950784619,-0.4662828811095438,-0.9534660792528726,0.0869916793992247,0.28867781942757065,0.2953364809425195,0.4620941155974212,0.8362089400112426],[0.11242962464866642,-0.3326716274381589,-0.017558317848556916,0.7531207158102117,-0.2798739980009457,0.5953820056572765,-0.5450028953790501,0.24147845097116719,0.8029072186404964,-0.36848477776696076,-0.9291709761715184,0.02933028459632067],[-0.20779518202858013,-0.2623820214343451,0.8775533739328388,-0.27053434280044075,0.29335769419178337,0.9169255327582573,0.13654640000394996,-0.9311202950353138,0.33818645274361847,0.9529775705710469,0.21669393030685113,0.21184308003018346],[-0.440305620480136,-0.3922860835865006,0.670443886717051,-0.11415936799803596,0.18098999653013456,0.9768368645041561,-0.9528946561160925,0.2581644661883159,-0.1591944808813926,-0.28099717622012355,-0.9489961694094049,0.14299250820442694],[-0.5149089236941288,0.051407548353474275,0.08490175057726132,-0.7240411726920543,-0.3770878007532584,-0.5775544742877383,-0.3491902521745353,-0.5217122900101043,0.7783845156724903,-0.5948365725415675,0.765258830053913,0.24606579402980172],[0.16336194706322488,-0.06652247355692456,0.4888866924761994,-0.7228964804471233,-0.6809760475645916,-0.11701410684387026,0.046191217871758186,-0.2166005098725307,0.9751669552002281,-0.6894106540725096,0.6995397356592878,0.18803485922840962],[-0.42327198695230633,-0.01324962325484947,0.42236680611940336,0.04129897390205992,-0.8715978652751211,0.4884788183764869,-0.994924818281776,0.009022406981183666,0.10021577789570363,-0.09175511278186135,-0.4901385084036123,-0.8668017315731842],[-0.4099105001657834,0.06429333967828933,0.6341301959192713,-0.16252017631402804,0.9118671718324739,0.376942241232132,0.986705204744128,0.15012519060883783,0.06225163512317055,0.0001766966482162069,0.38204801802771543,-0.9241424569293354],[0.14179080719620651,0.32798875728341625,0.4362956752790795,-0.5947063095153033,0.19913684273254775,0.7788895449864512,0.009830087891947059,0.9705655024157664,-0.2406366033928909,-0.8038829360017073,-0.13545155365274847,-0.5791589607511325],[-0.36138926695656987,0.30391319809626394,0.6737094599019361,0.9395147382109174,-0.31826440353832414,0.12656945178385834,0.08499299277798206,-0.141341570672953,-0.9863056075975358,0.3317954910228791,0.9374061712193372,-0.1057422446176978],[0.10776573983430167,-0.21096715259824517,0.9053039877136685,-0.3002929451136,0.9184393679837634,-0.2574748035295602,-0.8096863501658884,-0.10275578038992347,0.5777969054533786,0.5042144003001531,0.3819821683456481,0.7745046556321752],[-0.03284922135486089,-0.4519689624844963,0.034014793590291165,-0.25481318725180463,0.4248397891772592,0.8686664452679169,-0.20537673896355496,0.8540481611913227,-0.477935281663131,-0.9449289045718055,-0.30018809419685966,-0.13037052353651418],[0.012977138336805385,-0.15773307566551378,0.6653495472763556,-0.7721583771556656,0.495849648693105,0.39737207561585536,0.5294179919008849,0.8478574089423144,-0.029230189073282842,-0.35140863740090905,0.18780559095125351,-0.9171919262441678],[0.2689069294457644,-0.3645587540036133,0.3144410447614912,-0.500856810708943,-0.5393776719988932,0.676915195660077,-0.8561701608784384,0.1940405334754119,-0.47887464642635497,0.12694530628524633,-0.8194022201078246,-0.5589855909542868],[0.3535432124779357,-0.6150082697926679,0.6235431531724027,0.8463015932279357,-0.4962692983972744,0.19362437028467072,-0.34743375011812694,-0.7897407475302302,-0.5055683346188211,0.40381109764184664,0.36059164599123844,-0.8407795562825114],[0.5650447060623797,-0.12727937595985603,0.3958487949388051,0.21688926245026835,0.974112663248039,-0.0637461146548601,0.05619413162709545,-0.07765068125035914,-0.9953956958275596,-0.9745774814782328,0.21230848075658004,-0.07158101420333207],[-0.08310669957088694,-0.2248934532678543,0.7619210642353137,-0.28085271600300876,0.9531582337826474,0.11229930225107226,0.8980598483686584,0.2197212666445056,0.3810657078932667,0.33854137215678043,0.2078748333889371,-0.9177024534029133],[0.15918109822871956,-0.43183609437706894,0.5584882926025011,-0.9190311857171937,-0.08882175383102414,0.38404736130538636,0.3354855269699132,-0.6877703084745352,0.6437557486924383,0.20695685751617054,0.7204739404220137,0.6618807749888254],[0.20514705303822972,-0.7061843219807638,0.42130709860567256,-0.3518154268434116,-0.9358719426567297,0.019225305794891845,-0.8700048414908863,0.3344954877119501,0.36222140257411994,-0.34542362573716073,0.1107089682377345,-0.9318937939134109],[-0.5523191435758049,0.2702086822847625,0.6593357830382731,0.4645085914578987,0.6467710034894134,-0.604912421352956,0.7624979131653131,0.05528168751054152,0.6446245941976845,0.45036507511167145,-0.7606781211992913,-0.4674827216575759]],"left_grasp_frame":[[-0.15762299240186828,-0.12287996268121068,0.6257630171922229,-0.9550922642055143,-0.07686924354090241,-0.2861640897321557,0.16599267675493845,-0.938799092499668,-0.30183223019672767,-0.24544897253505538,-0.3357787713945735,0.9093994823850716],[0.3805868428339783,0.06653797488551033,1.0935486266687424,0.2157247510254601,0.3749209874945753,-0.9016080550500291,0.3474996915586871,0.8334217247040421,0.4297117558991812,0.912527696106601,-0.406007982590777,0.04950476654774566],[0.6620303735652918,-0.1453657891211697,0.02170662354779837,-0.03313025221397181,-0.9816523686399952,-0.18777916160140418,-0.5253781243830669,-0.14272438318243164,0.838813195452316,-0.8502236251763235,0.12644515644611343,-0.511010185420358],[-0.19008924753304904,-0.07368612026646594,-0.21077705002057937,-0.8028066469822482,-0.43374031132859037,-0.40910980175216705,0.03059494443038943,-0.7152136356107197,0.6982359234612597,-0.5954539754980408,0.5480317488659525,0.5874485214029433],[0.06149920419169294,-0.541364100834549,0.4146260299859148,-0.8916328355616439,0.0500119616817018,-0.4499885445620264,-0.16224312868711166,-0.9631701428360929,0.214430508890218,-0.4226914403464071,0.26420083201581407,0.8669082227202783],[0.07282142758325966,0.8404305835766362,0.3299185283036925,0.7405880557017444,-0.13993009810733198,-0.6572281943096954,0.6460286998763577,0.4173569562183008,0.6391088248743388,0.1848681981520166,-0.8979046378874918,0.39948843609316026],[0.6528123223711828,0.42135821867101064,0.19876041568976915,0.677176108948669,-0.5027567582260295,-0.5372784748407422,-0.1877367137653106,-0.824059472419014,0.5344912648688456,-0.7114685121449451,-0.2610778197674984,-0.6524192886876565],[0.17623666375793604,0.6373261155466742,1.0238578626277206,0.26472246745576233,-0.7379103984513918,-0.6208141904639753,0.40529456824203874,0.6692981377092442,-0.6227168825497164,0.8750190444656805,-0.08676546960770654,0.4762493308196053],[0.9395313130288478,0.03783443069643791,0.40736158519177224,0.8532175761471469,-0.41643207251144904,0.3140128926292788,-0.33908725170096554,0.014548938325586819,0.940642420969565,-0.3962822270648452,-0.9090504152011688,-0.12879339709437296],[0.05232052373851392,-0.6653615906476071,-0.010185322898569306,-0.13572093109459565,-0.4400159440656107,0.8876743760134485,-0.9547879647248585,0.29728443858300757,0.0013802148195914965,-0.26449909504453556,-0.847353486771864,-0.4604696484853116],[0.18823486415753368,0.617670527050161,0.2734344112449014,0.8400218533009509,-0.5413015556923665,-0.03682270742164129,0.5262155021643308,0.8293785033707539,-0.18769268347069185,0.1321383035261806,0.13828927634315002,0.9815373374404618],[-0.17924767554051163,-0.3879099718070649,1.0562033516176523,0.4499745399898606,0.7895288716383948,-0.41733328912311385,-0.14712181092836316,0.5264692227742218,0.8373680972073546,0.8808394212965677,-0.3153954950865815,0.3530546637123178],[-0.21537314636575075,-0.2960568163526559,0.7971329407279537,-0.17800926174633652,0.9077843582739821,-0.37978975974296436,0.3624902349641206,0.41930591012552665,0.8323360999556971,0.9148297832319305,0.010493455431744458,-0.40370330083481],[0.04544786867554575,0.7422600349885428,0.7389674843115915,0.0768694966434214,-0.5809473251008996,-0.8103032061789569,0.9964227885850746,0.07338066618254206,0.04191544129494426,0.03510992557853992,-0.810626599174192,0.584509888570889],[-0.5772996070751203,-0.15949705955290513,0.011310866853594143,-0.26102336923054886,0.4999533772481249,-0.8257804921973676,-0.469791322779167,-0.8130906227983553,-0.3437728205643718,-0.8433047573427612,0.29821176988103165,0.44710941227790035],[-0.39382877884195167,0.5979261602178015,0.5534527914848594,-0.9621671407232236,0.12816336708193293,-0.24043407547749554,0.08222478311637971,-0.7047247918080026,-0.7046999736431324,-0.2597565751890809,-0.6978087984178684,0.667524832868898],[0.22686079522080993,0.4150181407413644,0.3045849628479384,-0.3492796726764318,0.15689155048194672,-0.9237904262560939,0.9370181743743835,0.059331329068159425,-0.3442044948618529,0.0008069368884841277,-0.9858320520141232,-0.16773346140374465],[0.21537177650946815,0.02843765533294146,0.14152592655399276,-0.72038021398894,0.5982152682237966,0.3509855269910433,-0.643404486214089,-0.38740832620401855,-0.6602616571537268,-0.2590038887986695,-0.7014650965313473,0.663975680228903],[-0.8289673574926542,0.2802574808480941,0.6519042198619712,-0.5751767549273341,0.6206477086309999,-0.5328865942790746,0.7988925344622375,0.5662635633524284,-0.20277153449119234,0.1759045734816583,-0.54234859508484,-0.8215324597590387],[-0.5327257339515258,-0.25082062465667776,0.74764280441043,-0.029405328726813007,-0.7307750253483394,0.6819847424756674,-0.9596075436992846,-0.17034671651904149,-0.22390926342294842,0.27980115925044724,-0.6610218290633947,-0.6962481258745264],[0.04782050282634056,-0.4886744973794165,-0.0357275516429779,0.4065570390881882,-0.9136156383338471,0.004223666615966917,-0.8795032500599846,-0.39011709988681603,0.27254849386819957,-0.247356841631214,-0.1145212371909132,-0.9621327762479008],[-0.20900090054987533,0.6739997805489978,-0.09694817858529575,0.11839144507278179,0.544479971170942,-0.8303764367606229,0.2773437357230301,-0.8211082745325764,-0.49886035495850867,-0.9534484348871586,-0.1712389047145108,-0.24822030441404688],[0.5850932277002646,0.1025482851783192,1.0248309816044883,0.7317632276977217,0.5348527578111978,0.4224394702808541,-0.19119437990573007,-0.43383431711089593,0.8804728811209267,0.6541920877554817,-0.7250657299574148,-0.21520315880452454],[-0.018867934402859873,0.37345305429661313,-0.07001640377482549,-0.5260147547472901,0.5831940223648601,-0.6190260172771767,0.41468730199823445,-0.45959510398920655,-0.7853704743307937,-0.7425246927541017,-0.66981868641226,-8.93497680440336e-05],[0.5155019168304954,0.32292133008377066,0.9722764473895339,0.838973801854854,-0.2554767191904381,-0.48047331429841345,0.4411484072392403,0.836266187797259,0.3256485005912368,0.318607976346314,-0.48517059793306677,0.8143085706965003],[-0.41195808420203733,0.2900143087188608,-0.1998824336035075,0.1355688034513473,0.6705229597712269,0.7293970523318584,0.41555201185281665,0.629827743994195,-0.6562267430852382,-0.8994095980007366,0.3920662868888583,-0.19325217129268413],[0.3477740259012245,0.09124583008633441,0.090335136194623,-0.5208724881678857,0.8494246234392284,0.08467384581392787,0.4469648026808699,0.3558929395931914,-0.8207086454474377,-0.7272649560058884,-0.38963832542864923,-0.5650377501750758],[0.8170918425838516,-0.0006913297055388679,0.2849861337205165,0.8965642375105777,-0.008621239776983924,0.4428298118259252,0.11801166338479249,0.9683176551427661,-0.22007763640127004,-0.426902582941625,0.2495728209631868,0.8691763869976256],[0.36651548591845456,-0.14904158703211745,1.1210796464033361,0.06841030998405406,0.9753341390957218,0.2098650676084351,-0.42109046999076216,-0.1624733213698768,0.8923481584706721,0.9044350974895017,-0.14941799408855766,0.3995890607508009],[-0.5613990553145154,0.5222805671883408,0.3639333380738059,-0.9964548134289911,0.03583333498705152,-0.07611686342657731,-0.044737938506443656,-0.9919211234149262,0.1187055254883458,-0.07124830982127325,0.12169000380876151,0.9900076875058272],[-0.008201799262788523,0.15444073576965106,0.7041535900736444,-0.6673645422132051,0.09975299821643246,-0.7380202620141211,-0.7258429253606378,-0.3088960507957508,0.6146017226681851,-0.16666317979827133,0.9458501832476547,0.2785512795696829],[0.7535070921045082,0.3589777371718394,0.09038305975402877,0.9068698568350878,-0.40507942879321257,-0.11617968468054946,0.3806615579039271,0.6691586774129971,0.6382189614678052,-0.18078672821328023,-0.6230066779830321,0.7610380004247386],[0.5176446925350199,0.616217395659065,0.501129172237192,0.3356828841559224,0.6124367003659077,-0.7157082431617517,0.3159820582656275,0.6425616618889518,0.6980471685529042,0.8873963827619421,-0.4604734506086406,0.022177942771877047],[0.7361665533536932,0.6321781682696838,0.4595543211845725,0.7835400399422743,-0.6017646559415188,0.15473947352516332,0.617922963407262,0.7285907968632922,-0.29551084924262994,0.0650862281995723,0.327161656655694,0.942724261548972],[0.7065979182074752,-0.03464902119754078,0.6421586463444406,0.45333875575677773,-0.24551244374004888,0.8568591555779084,-0.5254567259474524,-0.8501237704724337,0.03442098247169288,0.7199855565811585,-0.4658467718537095,-0.5144002172122378],[0.42980132886075983,0.5942524903365399,0.87502719598378,0.8394309546722222,-0.19995794581321397,0.5053439346071561,0.0897388942124133,-0.8660903867982869,-0.4917665836157715,0.5360061596694715,0.4581530986490803,-0.7090790752762566],[0.5350345161037123,0.5531142225942143,0.12986853419408367,0.04925807916881281,0.8631340601898229,0.50256664809439,0.5795343277786409,0.38511151188025394,-0.7182124242474953,-0.8134578074877382,0.32663238902095504,-0.48124596401393915],[0.7862394425536247,0.45912828520995197,0.6059595335176305,0.3471544263378271,-0.8176588823624129,0.45925783212472704,0.8467843311987148,0.06283732362603106,-0.5282118582499475,0.40303858464221454,0.5722634208591069,0.7141949848859438],[-0.5114448338178185,-0.6364130730195521,0.06415623724815217,-0.761295116335113,0.12378000064721151,-0.6364811523400224,-0.24062404710078222,-0.965447650427667,0.10005449635342764,-0.602104487444785,0.22932367022065933,0.7647750260536617],[0.6030790780481661,0.24979659816590588,0.7067062904774979,0.7001156851834597,0.529858231647967,0.47863167646425625,0.7050360254253016,-0.406934289399643,-0.5808000404298378,-0.11297004121707956,0.7440797930771249,-0.6584702205276364],[-0.11759803149923714,0.8180020099747077,0.7034645735059629,-0.3019992423668747,-0.4335494568597651,-0.8490178596863769,0.6669211208351846,0.5402936683849326,-0.5131266612514824,0.6811847592201213,-0.7211918055273197,0.12597501119853108],[-0.39931713210772823,-0.39494595843930136,0.9690739797502292,-0.418858746531251,0.12341264836814947,-0.899625849271256,-0.7313063306619645,0.5414421641897363,0.4147667218705814,0.5382828263914825,0.8316307480248035,-0.1365353351762941],[0.38277331110042795,0.1769286860524075,0.6319673247679912,0.486533367086008,0.6997907637814843,-0.5230470051898573,-0.8640825305683397,0.47384651047324644,-0.16979653965538255,0.1290219480498026,0.5345674620087296,0.8352191122591602],[-0.39449278318247183,0.5680476830163603,0.6345467861917111,-0.893732405387593,0.17257673599342832,0.41407687420811295,0.31043634055933594,0.9042584087103118,0.2931654971800447,-0.32383895074594543,0.3905560145107862,-0.8617391330960986],[0.4556865709516579,0.6016358299088768,0.7734679910938949,0.8753091319843829,-0.04286096436437342,-0.4816605248497145,0.4439028069781138,-0.323840852829,0.8355102632499121,-0.19179213076061807,-0.9451402222806635,-0.26443475339969996],[-0.2532783026482235,0.6655734678538145,0.9617422368217046,-0.3239550847634409,0.5792548714267154,-0.7480086209291517,0.8915832502438292,0.4513760416514049,-0.03659203339196798,0.3164370568384201,-0.6787661327375789,-0.6626795048195695],[-0.7704343301738885,-0.14160842097263415,0.06650205750072041,-0.7516630226218013,-0.09163234407948168,-0.6531509886248026,0.19106606528370196,-0.9780901221690763,-0.08266481483851545,-0.6312657595023363,-0.18693107401869102,0.7527020090621236],[-0.7590169005239908,0.5447912660761314,0.5058275319178236,-0.8391112806202784,0.5431786892983861,-0.029140525525184813,0.33566437756484313,0.5592069586835405,0.7580349615907644,0.42804402153260257,0.626294251013046,-0.6515626038825466],[-0.38652693257269166,-0.03114156929330128,0.009940104097437924,-0.23130475083345217,0.6374315291339264,-0.7349688142417058,-0.9722765443685983,-0.1780903644465484,0.151532647841818,-0.03429917655763004,0.749643160281797,0.6609528717920731],[0.7528358967698072,-0.4939268311249186,0.6119172434660916,0.6248337709749242,0.7216979992750251,-0.29788379696063,-0.7144245740705849,0.6823942761956447,0.15471127877270546,0.31492901837290177,0.11614667303822436,0.9419817746266816]],"right_grasp_frame":[[-0.43096920284532114,-0.04959477310986179,0.8710421269857435,0.5335789307830141,0.7775646135930188,-0.3326962523269457,-0.003352498656634173,0.39531556086210495,0.9185392577854454,0.8457436286353048,-0.48899783131238816,0.21353883860920636],[-0.303344768858688,0.5530189301399266,0.24514453636319403,0.6238180999374654,-0.14656527844622366,-0.767704107937682,0.7693872219520262,0.28791150789302433,0.5702194896000214,0.13745646906331638,-0.9463749694369279,0.29236986222232203],[-0.6960225643419315,-0.16996381637718216,0.9555182079935843,-0.28065750588422744,0.43175363585142706,-0.8572165200927466,0.07313087972691656,0.9001356260710045,0.4294272104887259,0.9570178884541354,0.05783297162375839,-0.2842043429857354],[-0.3206045863692159,0.5657163718475222,-0.1602490548222235,-0.31899409578678234,0.3485471843890971,0.8813385428469617,0.3104116374115335,0.9170521401973604,-0.25031977053147847,-0.8954816481719007,0.19372721134077636,-0.40072732047298987],[-0.16977992031145217,0.7187775509572524,0.18035361797116894,0.298956093791548,0.27836015504635564,-0.9127655109980153,0.7743964315593639,-0.6296946068636939,0.06160250705045722,-0.5576158361928243,-0.7252587994428933,-0.4038121507057834],[0.3608024169654328,0.10796762664448974,0.3373339648453698,0.6455169653080393,-0.21360914061728523,-0.7332659698528528,0.28004890138290034,0.9594199297031369,-0.032954685898340885,0.7105494073839835,-0.1840775204449816,0.6791428466332373],[-0.1006356272154471,-0.7475962136045674,0.7503430346298677,0.23609077790117694,0.03643710995644239,0.9710476206695733,-0.781564229222997,-0.5866839333550247,0.21203612414430564,0.5774240211396472,-0.8089958586798196,-0.1100327244496896],[0.23803335869734255,-0.5419271931277818,0.30019265359134883,-0.5855872661791104,-0.1840300924523888,-0.7894431447297777,-0.7616013453521748,-0.2085864372913076,0.6135593605641401,-0.2775805188528778,0.9605335096966086,-0.018012004421567085],[-0.3801560631468753,0.14150234349049562,0.3939491954626353,-0.22279711480946585,-0.9716702370948591,0.07885680678671994,0.40510645894662534,-0.01870517908673569,0.91407815486153,-0.8867075067643804,0.23559937737471537,0.3977973489458452],[0.09490292996087923,0.3115271756458802,0.31124132485948597,0.8887076038477384,-0.10426850317675845,0.44646038358233964,0.43432620132123045,-0.12040049239367698,-0.8926726568441706,0.14683169177354538,0.9872344203261141,-0.061714290195060376],[0.5666005646127198,-0.5795440786790549,0.7785438336928546,0.6893297212066987,-0.00035867010045292913,-0.7244476563678388,-0.6357598507327439,0.479133196731341,-0.6051783142068836,0.34732398082679905,0.877742132488674,0.33005272487416343],[0.3253754085119829,0.5796115768969277,0.9113168751481979,0.016146975724939755,0.09587956852748217,-0.9952619672799329,0.07557432976295453,0.9924273117786584,0.09683259532133921,0.9970094261619689,-0.07677980968177717,0.008778665582631524],[0.11923746267160737,0.080120337672071,0.21075143551818815,0.6423139021665158,-0.14558106978901197,0.7524885402467671,-0.7438101354454447,0.11842157300275888,0.6578167020197809,-0.1848765358076913,-0.9822334158267265,-0.0322208525819588],[-0.2124084820695429,0.042402072025701336,0.7078685874006165,-0.8412385736375998,0.48951536291501363,-0.2295481903530482,0.3659186016515954,0.20292936812765625,-0.9082528549455,-0.39802165668405254,-0.8480532890222718,-0.3498347892777237],[0.08311034748520563,0.6196375910331978,-0.0848619866698553,0.15469891127927252,-0.45158719779354173,-0.8787134058599451,0.7725437751643355,-0.4991111478790353,0.39251009861875996,-0.6158281921963663,-0.7395654567729949,0.2716589274150716],[0.44719693704253316,0.028661376335621574,0.7507157761721439,0.46278522113159726,0.8644919267327152,-0.19617223992230895,0.8111848897315359,-0.3237364315509095,0.48700492559923736,0.3575037255378104,-0.3845106389953869,-0.8510832243241229],[0.18485161421812857,-0.5863270348133593,-0.02214838147035722,0.5590813556727108,0.822738662554494,0.10261155333200231,-0.13678549260218015,0.2135919044695164,-0.9672994507176482,-0.8177516534698923,0.5267633163794985,0.23195396475190866],[0.07367131575089714,-0.8461140007788649,0.7676389077157832,0.6147934660993692,-0.7673652220972733,-0.18215271054015647,-0.7766322194502523,-0.5488003973173543,-0.3092903484043582,0.13737317697898266,0.3316153491922781,-0.9333594540297883],[0.34056685555012906,0.32509415337864195,1.1469387866756555,0.2064862457600185,-0.9779814216091248,-0.030261680378472024,0.2790563729902742,0.08850591310004302,-0.956187347772308,0.9378117993528586,0.18899482091772576,0.29118720208970483],[-0.05318473196334639,0.255989790357264,1.0191666161941129,-0.735493511031404,0.6142205561399632,-0.2859762291621667,0.3232029604354694,0.6890327614811116,0.6486707176768981,0.5954738798607764,0.3846647397680579,-0.7052970270621635],[0.6971811306553446,-0.08443073254567826,1.0142648203967959,0.757906227566307,-0.281360415445469,0.5885698487321307,-0.43127014484484055,-0.8930331766408834,0.12844379153623295,0.489473403140273,-0.3511809534115512,-0.798177753122219],[0.4662919775801269,-0.23740826648559005,1.023857271646459,0.21771536336012054,-0.5738402278853101,-0.7894982035555913,0.10894083276706162,0.8181348206622412,-0.5646125310121813,0.9699133546420473,0.03691623058354909,0.24063515205911296],[0.43315864743709376,-0.6830618442781335,0.5771668854151832,0.8020298902511103,-0.5767297541479067,0.15534106290445823,-0.5722732686085522,-0.8164742589864996,-0.07663609103112032,0.17103029316432744,-0.027433102138497464,-0.9848837818378283],[0.6339230255251364,-0.45186855618652155,0.020551354957673407,0.7643296980580889,-0.3916623855631587,0.5122506109331705,0.11619144129157027,0.865051875416887,0.48804180334097824,-0.6342709686222587,-0.31350570740052247,0.706692655820251],[-0.47594419535231147,-0.5386179575785642,0.4110847972146637,-0.4593347188025493,-0.716105351062849,-0.52555184547458,-0.8026480998144447,0.5880527569968674,-0.0997496007643364,0.3804834345498427,0.3760147353063934,-0.8448936470737713],[0.6773514769801694,-0.4052314428961491,0.7934216997850102,0.3495731164389035,-0.08819836977051186,0.932748456891255,-0.41268182813922827,-0.908274775387197,0.06877965628731669,0.8411256416177416,-0.40897185717988094,-0.35390630829074204],[-0.3020685267715134,0.5224263174440422,0.4489285452364525,0.4666584798915226,-0.8106123141768979,-0.3537478469900661,0.8360236337381252,0.2737762507109105,0.4755113546257102,-0.28860760034454136,-0.5176429663884662,0.8054510614381949],[-0.5997697632931813,-0.6049626221289811,0.7770318724887247,-0.7324182637433763,0.5426481281016133,0.41121344336360527,-0.28218297105740175,-0.791586987837028,0.5419989036264913,0.6196259014763308,0.2809324647578415,0.7328988282598959],[0.11107442723519889,-0.5696729257346541,0.6431603387177365,0.8810408112090629,-0.31333498420585704,-0.3543843628841318,-0.4300382920964107,-0.21844328922924444,-0.8759849295059313,0.1970638381714896,0.9241773190591858,-0.3272034942076521],[0.04739795389723732,-0.19552404534879536,-0.21989917068670178,-0.5202348743724162,0.04220848669256849,-0.8529795537630661,0.29515837826359426,-0.9283455571591335,-0.22595587676292656,-0.8013970347009785,-0.36931418894218104,0.4704995458217337],[-0.19871534914296882,-0.8476896801761111,0.47286821420564795,-0.060653101640751717,0.882553950784619,-0.4662828811095438,-0.9534660792528726,0.0869916793992247,0.28867781942757065,0.2953364809425195,0.4620941155974212,0.8362089400112426],[0.28751896638372904,-0.45860557712838984,-0.1030251384996192,0.7531207158102117,-0.2798739980009457,0.5953820056572765,-0.5450028953790501,0.24147845097116719,0.8029072186404964,-0.36848477776696076,-0.9291709761715184,0.02933028459632067],[-0.26998363629122324,-0.23049311901806396,1.0987679954322054,-0.27053434280044075,0.29335769419178337,0.9169255327582573,0.13654640000394996,-0.9311202950353138,0.33818645274361847,0.9529775705710469,0.21669393030685113,0.21184308003018346],[-0.46617904338105326,-0.6134485643040799,0.6053444411261961,-0.11415936799803596,0.18098999653013456,0.9768368645041561,-0.9528946561160925,0.2581644661883159,-0.1591944808813926,-0.28099717622012355,-0.9489961694094049,0.14299250820442694],[-0.683240930934561,-0.02911495711645304,-0.05294118960196084,-0.7240411726920543,-0.3770878007532584,-0.5775544742877383,-0.3491902521745353,-0.5217122900101043,0.7783845156724903,-0.5948365725415675,0.765258830053913,0.24606579402980172],[-0.004416664148961136,-0.05519704738463736,0.32906714721428165,-0.7228964804471233,-0.6809760475645916,-0.11701410684387026,0.046191217871758186,-0.2166005098725307,0.9751669552002281,-0.6894106540725096,0.6995397356592878,0.18803485922840962],[-0.41338569743630826,-0.24400059191167214,0.40053869466779335,0.04129897390205992,-0.8715978652751211,0.4884788183764869,-0.994924818281776,0.009022406981183666,0.10021577789570363,-0.09175511278186135,-0.4901385084036123,-0.8668017315731842],[-0.447378129488281,0.2932389741040363,0.6335935989158068,-0.16252017631402804,0.9118671718324739,0.376942241232132,0.986705204744128,0.15012519060883783,0.06225163512317055,0.0001766966482162069,0.38204801802771543,-0.9241424569293354],[0.004311101711058307,0.33011885132643637,0.24944009472263795,-0.5947063095153033,0.19913684273254775,0.7788895449864512,0.009830087891947059,0.9705655024157664,-0.2406366033928909,-0.8038829360017073,-0.13545155365274847,-0.5791589607511325],[-0.14335119741691604,0.32301436647907233,0.7506169387569388,0.9395147382109174,-0.31826440353832414,0.12656945178385834,0.08499299277798206,-0.141341570672953,-0.9863056075975358,0.3317954910228791,0.9374061712193372,-0.1057422446176978],[0.03793955745224652,-0.39844597559367145,1.0227612560634713,-0.3002929451136,0.9184393679837634,-0.2574748035295602,-0.8096863501658884,-0.10275578038992347,0.5777969054533786,0.5042144003001531,0.3819821683456481,0.7745046556321752],[-0.09142067095030185,-0.49991322708442987,-0.1852816894874369,-0.25481318725180463,0.4248397891772592,0.8686664452679169,-0.20537673896355496,0.8540481611913227,-0.477935281663131,-0.9449289045718055,-0.30018809419685966,-0.13037052353651418],[-0.16590829819065472,-0.0349311351746064,0.5832526611231788,-0.7721583771556656,0.495849648693105,0.39737207561585536,0.5294179919008849,0.8478574089423144,-0.029230189073282842,-0.35140863740090905,0.18780559095125351,-0.9171919262441678],[0.15313572906987355,-0.5634818224499796,0.3435418473175654,-0.500856810708943,-0.5393776719988932,0.676915195660077,-0.8561701608784384,0.1940405334754119,-0.47887464642635497,0.12694530628524633,-0.8194022201078246,-0.5589855909542868],[0.5499985806239056,-0.695925753125459,0.7166982063027558,0.8463015932279357,-0.4962692983972744,0.19362437028467072,-0.34743375011812694,-0.7897407475302302,-0.5055683346188211,0.40381109764184664,0.36059164599123844,-0.8407795562825114],[0.6153212216258206,-0.11486496547944675,0.1697108522993113,0.21688926245026835,0.974112663248039,-0.0637461146548601,0.05619413162709545,-0.07765068125035914,-0.9953956958275596,-0.9745774814782328,0.21230848075658004,-0.07158101420333207],[-0.14819181494523403,-0.01631348491752757,0.8398860516699606,-0.28085271600300876,0.9531582337826474,0.11229930225107226,0.8980598483686584,0.2197212666445056,0.3810657078932667,0.33854137215678043,0.2078748333889371,-0.9177024534029133],[-0.053785835976182056,-0.35360412414685904,0.606914096418903,-0.9190311857171937,-0.08882175383102414,0.38404736130538636,0.3354855269699132,-0.6877703084745352,0.6437557486924383,0.20695685751617054,0.7204739404220137,0.6618807749888254],[0.12354105616552163,-0.9077912267864672,0.340589492626087,-0.3518154268434116,-0.9358719426567297,0.019225305794891845,-0.8700048414908863,0.3344954877119501,0.36222140257411994,-0.34542362573716073,0.1107089682377345,-0.9318937939134109],[-0.44493540119824115,0.4475042260292702,0.7635242504774689,0.4645085914578987,0.6467710034894134,-0.604912421352956,0.7624979131653131,0.05528168751054152,0.6446245941976845,0.45036507511167145,-0.7606781211992913,-0.4674827216575759]],"torso_link":[[0.0,0.0,0.0,-0.04444165017209208,0.9990119817749842,0.0,-0.9990119817749842,-0.04444165017209208,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.21660766373078613,0.976258736203211,0.0,-0.976258736203211,-0.21660766373078613,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.11573199744264913,0.9932804763851621,0.0,-0.9932804763851621,-0.11573199744264913,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.6296408311822408,0.7768863647330522,0.0,-0.7768863647330522,-0.6296408311822408,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.541520156481188,0.8406877661323434,0.0,-0.8406877661323434,-0.541520156481188,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9866144851969935,-0.16307010026203952,0.0,0.16307010026203952,0.9866144851969935,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.5783373944856871,0.8157976821121194,0.0,-0.8157976821121194,0.5783373944856871,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9416411490989612,-0.3366183986706428,0.0,0.3366183986706428,0.9416411490989612,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.20735622038148077,0.9782655047936152,0.0,-0.9782655047936152,0.20735622038148077,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.3019659621855294,0.9533187072964463,0.0,-0.9533187072964463,-0.3019659621855294,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.5546899530702708,-0.8320571230167438,0.0,0.8320571230167438,0.5546899530702708,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.4212218085744849,-0.906957655010001,0.0,0.906957655010001,-0.4212218085744849,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.6372964973809758,0.7706186958710124,0.0,-0.7706186958710124,-0.6372964973809758,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9982589377639443,-0.058983838247452354,0.0,0.058983838247452354,0.9982589377639443,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.28342602498606617,-0.9589941023596537,0.0,0.9589941023596537,-0.28342602498606617,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9882952735012134,0.15255311329226204,0.0,-0.15255311329226204,0.9882952735012134,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.8926302176895632,0.4507896343833597,0.0,-0.4507896343833597,0.8926302176895632,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9993226012407954,0.036801340319751415,0.0,-0.036801340319751415,0.9993226012407954,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.31775920073354685,-0.9481714456516699,0.0,0.9481714456516699,-0.31775920073354685,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.6330907796783386,-0.7740775572810992,0.0,0.7740775572810992,0.6330907796783386,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.01745424095388284,-0.9998476631331014,0.0,0.9998476631331014,-0.01745424095388284,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.999561699416453,-0.029604206790461888,0.0,0.029604206790461888,0.999561699416453,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9526453215179683,0.30408369142380304,0.0,-0.30408369142380304,0.9526453215179683,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.31979927596404883,-0.947485315502499,0.0,0.947485315502499,0.31979927596404883,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9854756260417155,0.1698169322467254,0.0,-0.1698169322467254,0.9854756260417155,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.5373149147186608,-0.8433816943833191,0.0,0.8433816943833191,0.5373149147186608,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.1515242470054095,0.9884535409261498,0.0,-0.9884535409261498,-0.1515242470054095,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.48716673932193827,0.8733089763070289,0.0,-0.8733089763070289,0.48716673932193827,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.7641967121492539,0.6449832440771391,0.0,-0.6449832440771391,0.7641967121492539,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.8756756416500832,-0.48289975214397785,0.0,0.48289975214397785,0.8756756416500832,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.8981683160921501,0.43965176670655093,0.0,-0.43965176670655093,0.8981683160921501,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.7702009238788179,0.6378013302402367,0.0,-0.6378013302402367,0.7702009238788179,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.8767948582197164,0.4808646135862645,0.0,-0.4808646135862645,0.8767948582197164,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.48779287469140553,0.8729593984832827,0.0,-0.8729593984832827,0.48779287469140553,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.17705687933676864,0.9842006205441678,0.0,-0.9842006205441678,0.17705687933676864,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9996717542271562,-0.025619988298208097,0.0,0.025619988298208097,0.9996717542271562,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.04357199607011759,0.999050289604315,0.0,-0.999050289604315,0.04357199607011759,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.3217636971534524,0.9468200057002077,0.0,-0.9468200057002077,0.3217636971534524,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.49028355601532414,-0.8715629837831391,0.0,0.8715629837831391,-0.49028355601532414,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.0009543985791018283,0.9999995445615724,0.0,-0.9999995445615724,0.0009543985791018283,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.916760025899465,-0.3994384244321173,0.0,0.3994384244321173,0.916760025899465,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.5769473897772399,-0.8167813106512842,0.0,0.8167813106512842,0.5769473897772399,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.954587455739704,-0.2979308465808777,0.0,0.2979308465808777,0.954587455739704,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.6643872969231395,-0.7473884663862321,0.0,0.7473884663862321,0.6643872969231395,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9845457389513631,-0.1751276332070826,0.0,0.1751276332070826,0.9845457389513631,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9782749987599362,-0.2073114246761302,0.0,0.2073114246761302,0.9782749987599362,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.5298687228332744,-0.8480796758341959,0.0,0.8480796758341959,0.5298687228332744,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.6520679347356081,-0.7581605426884459,0.0,0.7581605426884459,0.6520679347356081,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,0.9036326670656535,-0.42830830369234474,0.0,0.42830830369234474,0.9036326670656535,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,-0.3821854341438381,0.9240856529176752,0.0,-0.9240856529176752,-0.3821854341438381,0.0,0.0,0.0,1.0]]}, -"reduced_frames": {"left_wrist_yaw_link":[[0.15819087204817062,0.07122930887081841,0.6821365951031123,-0.12338279667719249,0.9412877379170881,0.31425161882008756,-0.9615256041133797,-0.03507151447659304,-0.27246743201108997,-0.24544897253505538,-0.3357787713945735,0.9093994823850716],[-0.0577129251711279,0.3263510721593494,0.8818194734421836,-0.38597724404061984,-0.8948459988687075,-0.22421464134055244,0.13533207646817202,0.1854943567368889,-0.9732815998967518,0.912527696106601,-0.406007982590777,0.04950476654774566],[-0.05367545198058057,0.6681110767689985,0.21927023394196657,0.5256820639340574,0.25537393273623854,-0.8114447129268,0.02789542704567044,-0.9585383544178843,-0.28359490168159684,-0.8502236251763235,0.12644515644611343,-0.511010185420358],[0.06535910195916764,0.048350857887571645,-0.07300424211669021,0.48171104932679554,0.8287403315192994,-0.28485773268380754,-0.6429533637986944,0.11336077429255495,-0.7574696738693478,-0.5954539754980408,0.5480317488659525,0.5874485214029433],[0.2781190124115013,0.49868489610243416,0.512144822284118,0.6192329660633968,0.7826428704704387,0.06340876155643171,-0.661726892303923,0.5636204908126956,-0.494418307041566,-0.4226914403464071,0.26420083201581407,0.8669082227202783],[0.0152861256742709,0.6969952263213008,0.2867810898736498,0.8360228681802114,-0.06999862101215204,-0.5442113164351969,0.5166135046548431,0.43458883363074263,0.7377282917793726,0.1848681981520166,-0.8979046378874918,0.39948843609316026],[-0.09211726392977351,0.6733583940970876,0.3642224693462169,0.5447914423944159,0.3815027739094552,-0.7467649682418178,0.44386353817369345,-0.8867322062070604,-0.12919424892420464,-0.7114685121449451,-0.2610778197674984,-0.6524192886876565],[0.2915041016277371,0.473179170162358,0.8205636636513207,0.385703176998901,-0.4695487281809118,-0.7942021475144994,0.29253158987590056,0.878632884171451,-0.377398362181443,0.8750190444656805,-0.08676546960770654,0.4762493308196053],[0.04034068842918321,0.7493173989993516,0.4993759912039567,0.5086373332072454,-0.1005825050968922,-0.8550855062134168,0.7643614719361176,-0.40436431876590717,0.5022360380624195,-0.3962822270648452,-0.9090504152011688,-0.12879339709437296],[0.3980010704925603,0.2133961459312558,0.05146388019021086,0.9512003298204026,-0.15053697876255634,-0.26936323166794485,0.1589281638671352,-0.5092452125259561,0.8458198107452998,-0.26449909504453556,-0.847353486771864,-0.4604696484853116],[0.40878767135508604,0.28047358673285516,0.24216605323565898,0.9037930392031706,0.3898357568827389,-0.17659622007154688,-0.4070597143284008,0.910441738226093,-0.07347264978705302,0.1321383035261806,0.13828927634315002,0.9815373374404618],[-0.20197274123018802,0.40625072962338105,0.8516358742668201,-0.32297234218741394,0.14491851248894905,0.9352472886461453,-0.3461369383234093,-0.9378295721596117,0.02578591691245319,0.8808394212965677,-0.3153954950865815,0.3530546637123178],[0.4041397068434558,0.1086368510760924,0.5851529790492166,-0.1658970731217139,-0.9016527655371871,-0.3993750762488703,-0.3681910222173645,0.43233341045105944,-0.8231179705037959,0.9148297832319305,0.010493455431744458,-0.40370330083481],[0.05821734646252721,0.5085242084395861,0.7304569788863436,0.13550850265368844,-0.5756075863074275,-0.8064200842575122,0.9901538965405452,0.1075194089298366,0.08963725714764967,0.03510992557853992,-0.810626599174192,0.584509888570889],[0.09808011197780155,0.509314879357933,0.20667053743162497,-0.37654629191547684,-0.9214489103393556,-0.09562842505823878,0.3834709588583879,-0.24900129706256005,0.8893527858917357,-0.8433047573427612,0.29821176988103165,0.44710941227790035],[-0.256841825572313,0.5465058696610156,0.6132967760990063,-0.9634488841491236,0.23417121092762733,-0.13011568546680802,-0.06551922830626139,-0.6769244602046077,-0.7331306199435762,-0.2597565751890809,-0.6978087984178684,0.667524832868898],[0.18615747271345232,0.31566413363297835,0.3045025941656194,-0.7341756704924943,0.1133001907222388,-0.6694394308966275,0.6789590810275348,0.12368602186151748,-0.7236824816770943,0.0008069368884841277,-0.9858320520141232,-0.16773346140374465],[0.37546033724216793,0.19206191518040658,0.20119751292014182,-0.6962140818654087,0.6120671835987038,0.37504628377555815,-0.6694796022280831,-0.36513077261416693,-0.6468976589003157,-0.2590038887986695,-0.7014650965313473,0.663975680228903],[0.3110278283676576,0.6289648579967597,0.6116093997427372,0.9402547952476111,0.33969842165204445,-0.022932560715780745,0.2915107220019685,-0.7684158924284494,0.5697003731974211,0.1759045734816583,-0.54234859508484,-0.8215324597590387],[-0.35493569934128977,0.3896556376426833,0.683166608753431,-0.7614269058656136,-0.5945085007811534,0.25843511665960517,-0.5847566829938963,0.45783161096699077,-0.6696639737161811,0.27980115925044724,-0.6610218290633947,-0.6962481258745264],[-0.2839529977843351,0.05146373488743908,0.022258342389044002,-0.8864654138121859,-0.3741112031793813,0.2724332537897418,-0.3911440438163678,0.920285658851608,-0.008980150279421256,-0.247356841631214,-0.1145212371909132,-0.9621327762479008],[-0.21778656159097337,0.616688010797247,0.12439841496286982,0.1265500953377032,0.5199330661252264,-0.8447808473915819,0.2737172909796635,-0.8368672799565027,-0.4740590684260334,-0.9534484348871586,-0.1712389047145108,-0.24822030441404688],[0.35090769751033624,0.26563786889864655,0.8731988069482591,0.7552499081463417,0.6414469180432266,0.1346975410977817,0.04037683200879727,-0.25065023154620747,0.9672353244494154,0.6541920877554817,-0.7250657299574148,-0.21520315880452454],[0.2962698523310677,-0.009290657508921182,0.10224269806549635,0.22469099145408716,-0.24895458600777365,-0.9420910637858091,0.631007954791226,-0.699545953771911,0.3353571522295916,-0.7425246927541017,-0.66981868641226,-8.93497680440336e-05],[0.2790796154358618,0.27171426334357834,0.8978533214922908,0.7518737634326136,-0.39377823833667513,-0.5287953695657169,0.5772129600677595,0.7807356722436443,0.23932615572912175,0.318607976346314,-0.48517059793306677,0.8143085706965003],[-0.07486210260465061,0.47859726865389296,0.008893281053339322,0.4233120999257972,0.8914671768458283,-0.1615337075131403,0.10894604664954777,-0.22709094938308802,-0.9677605383707905,-0.8994095980007366,0.3920662868888583,-0.19325217129268413],[-0.05920277031456765,0.4649567834285648,0.25940720919724447,-0.3628791303237908,-0.4804920627859891,0.7984022259332162,-0.5825842604647009,0.7856903671019667,0.20805342223086443,-0.7272649560058884,-0.38963832542864923,-0.5650377501750758],[0.32098973527728525,0.5180809192521467,0.3834804555978535,0.33371563123782516,-0.8498404814238173,0.40792773085544715,0.8404689537069819,0.46420314859963113,0.27951274512064717,-0.426902582941625,0.2495728209631868,0.8691763869976256],[0.30134259893522286,0.18640595983684327,0.9110091005386799,0.3238752313515344,0.8501397122370045,-0.41517141543532804,-0.27767244902248667,0.5049125990903133,0.8172889809202858,0.9044350974895017,-0.14941799408855766,0.3995890607508009],[-0.0319479031272621,0.625817121960802,0.3798435499128618,-0.8941751475409397,-0.4476200860361951,-0.009330814385016983,0.44201185941980153,-0.8859050747963759,0.14070435168208872,-0.07124830982127325,0.12169000380876151,0.9900076875058272],[-0.009659373408992372,0.3542753284709889,0.7426438532684941,-0.2802875626129875,0.22540167689422458,-0.9330771491670677,-0.9453371180399086,-0.23358406389060493,0.2275438822564786,-0.16666317979827133,0.9458501832476547,0.2785512795696829],[0.24599076956202376,0.5546111691783939,0.1318483048686904,0.4556855535697905,-0.7387828448966431,-0.49653860308561737,0.8715886846269467,0.257026433027752,0.41745727632333746,-0.18078672821328023,-0.6230066779830321,0.7610380004247386],[0.12512208229405802,0.6873248612300173,0.2952473367896339,0.14238043647218399,0.22799618461629834,-0.9631954895608686,0.4384694643541453,0.8578938985008481,0.2678854003935379,0.8873963827619421,-0.4604734506086406,0.022177942771877047],[-0.1565046881559569,0.7224132950632812,0.4438656993548574,-0.15721640992575037,-0.9295666951796331,0.33344978581920426,0.9854170606138356,-0.1699147128035609,-0.009066808924414742,0.0650862281995723,0.327161656655694,0.942724261548972],[0.020539533196284585,0.6068429462760101,0.47544997722337873,0.5974215811232979,0.79322267531136,0.11783565580938313,0.35314055660977006,-0.39215376133012775,0.8494157843788726,0.7199855565811585,-0.4658467718537095,-0.5144002172122378],[0.24936725178593278,0.5675387347619667,0.7511217654179473,0.8414545244293731,-0.22208153603771685,0.4925790034791033,0.06820322656387251,-0.8606831760580512,-0.5045520690046795,0.5360061596694715,0.4581530986490803,-0.7090790752762566],[-0.39591739825483424,0.5410577403857624,0.31888420313848026,-0.5768376651709279,-0.3471372935953477,0.7394281624576182,0.07446276571142385,0.8790944100821942,0.47079540639734874,-0.8134578074877382,0.32663238902095504,-0.48124596401393915],[-0.022047928341420008,0.7525294380517347,0.5120118373623448,-0.6900506535907688,-0.32258858011307434,0.6478940526502646,0.6011572130221456,-0.7539570180900654,0.26487510283940063,0.40303858464221454,0.5722634208591069,0.7141949848859438],[-0.3421084504801145,0.5761644020590224,0.20336107500367173,0.1635314643527469,-0.9021357337740524,0.3992600380887101,0.7814906566079334,0.3654610405015537,0.5056783379877201,-0.602104487444785,0.22932367022065933,0.7647750260536617],[-0.08617726779566902,0.4404418433850555,0.7333258671973192,-0.7043675149096568,0.40743980000954033,0.5812565813031112,0.7007882517047541,0.5294696128225762,0.4780771437436696,-0.11297004121707956,0.7440797930771249,-0.6584702205276364],[0.2219750565032754,0.6271408365535188,0.5453571056477287,-0.010466911526967104,-0.1816467596691309,-0.983308140139596,0.7320367255604815,0.6684959493833526,-0.13128365507076026,0.6811847592201213,-0.7211918055273197,0.12597501119853108],[-0.35822172253214724,0.11619795589261872,0.844282543157328,-0.8389768037422325,0.5134424458502608,-0.18026307880919373,-0.07980928263344222,0.211582498659525,0.9740961578132457,0.5382828263914825,0.8316307480248035,-0.1365353351762941],[0.37042423376248756,0.2798416971369489,0.6015133820728075,0.20700180917113148,0.8091849767630004,-0.5498817367224894,-0.9697956423501358,0.2438386801608966,-0.006253809725230486,0.1290219480498026,0.5345674620087296,0.8352191122591602],[0.24607430853384196,0.46950463813857,0.7102130951723988,-0.3617679165068822,0.7904900964413467,0.49421592650826185,0.8742152529932399,0.4717959378349935,-0.11470084775782816,-0.32383895074594543,0.3905560145107862,-0.8617391330960986],[0.33625172769661893,0.446139974294362,0.8181273110220563,0.9395215241204977,-0.09891206192399576,-0.32789588243867984,0.2837518005098805,-0.31132999250506166,0.9069501372590443,-0.19179213076061807,-0.9451402222806635,-0.26443475339969996],[-0.07869097626185048,0.48562069670345126,0.8887458622592149,-0.13208176631980723,0.6602459688860822,-0.7393440792861732,0.9393731931800474,0.32148374394073204,0.11927366145369889,0.3164370568384201,-0.6787661327375789,-0.6626795048195695],[-0.47325779820924263,0.4066629106253352,0.21247959355776308,-0.2362434790888952,-0.8780514668733931,-0.4161906295310567,0.7387100645503033,-0.44054783518671525,0.5101225788419738,-0.6312657595023363,-0.18693107401869102,0.7527020090621236],[-0.014341837004870397,0.732009303186777,0.4069323979458802,-0.2926700731116753,0.7781580573938615,0.5557105955601516,0.8550570413340877,-0.04717572321670635,0.5163834885082763,0.42804402153260257,0.626294251013046,-0.6515626038825466],[-0.21714357431733255,0.3179693885049495,0.01748410882134903,-0.625448646338958,0.49972637084297516,-0.5992391384721749,-0.7795111014477176,-0.43394548796689825,0.4517230968114616,-0.03429917655763004,0.749643160281797,0.6609528717920731],[0.07096796158994781,0.687370463966947,0.5382678069556021,0.4213871329626654,-0.9064132234392013,-0.029119624792590698,0.8504425892205691,0.4061096141643388,-0.3343985402526548,0.31492901837290177,0.11614667303822436,0.9419817746266816]],"right_wrist_yaw_link":[[0.07398728519878191,-0.5518038526359502,0.6747037550608797,-0.020363941854195544,-0.42948123642683994,-0.9028461537988808,0.5332007356474168,0.7592278897202436,-0.3731889427300861,0.8457436286353048,-0.48899783131238816,0.21353883860920636],[-0.2683379533358696,-0.5180073447977199,0.21307314148483725,-0.8862447781742169,-0.249328962285716,-0.3903911650184483,0.44235270121428993,-0.20544947259309776,-0.8729917536492766,0.13745646906331638,-0.9463749694369279,0.29236986222232203],[0.2588947983792669,-0.6044761683129268,0.7336762987475871,-0.040158421300363956,-0.9440548541552751,-0.32733428409582244,-0.2872352039312911,0.32467796312504443,-0.9011549022690393,0.9570178884541354,0.05783297162375839,-0.2842043429857354],[-0.22805641089092696,-0.502963251165942,0.04774508279411952,-0.04030286094606339,-0.9319048423535734,-0.3604567161220939,-0.4432700048355387,-0.30663191677333207,0.8423114450302892,-0.8954816481719007,0.19372721134077636,-0.40072732047298987],[-0.32401552388654575,-0.49248427168627634,0.30996785601956955,-0.8129163568395035,0.3786389176709925,0.4424924483058759,-0.16802254613523243,0.5750062989703612,-0.8007097977011655,-0.5576158361928243,-0.7252587994428933,-0.4038121507057834],[0.2156906350647249,0.007953162328952917,0.17206843299780633,0.6825439908401001,-0.054297168173405574,-0.7288247512923929,0.17103578641424427,0.9814108640091448,0.08706018476022612,0.7105494073839835,-0.1840775204449816,0.6791428466332373],[0.3718498833379576,-0.4548531681816187,0.6164546289944409,0.7741384119753318,0.4996883361982478,0.38861457225860463,-0.2594055105722151,-0.30957594755782947,0.9148068177371879,0.5774240211396472,-0.8089958586798196,-0.1100327244496896],[0.22945303964264505,-0.4703109302486676,0.3646000932433103,-0.807782091520473,-0.24350434023106982,-0.536836780509161,-0.5200357181696051,-0.13446565746891187,0.8434938285424951,-0.2775805188528778,0.9605335096966086,-0.018012004421567085],[-0.11404989630564175,-0.31164286818110276,0.5994087333213195,-0.4425000421953641,-0.18318323635962677,-0.8778596781797842,-0.13395338780634541,-0.9544301102215184,0.2666826852229878,-0.8867075067643804,0.23559937737471537,0.3977973489458452],[-0.16777660513517947,-0.17015411138315675,0.27721626528462134,-0.6824107394860102,0.14626558065401052,0.7161857039554073,0.716069854780297,-0.06304426412693459,0.6951757934764543,0.14683169177354538,0.9872344203261141,-0.061714290195060376],[-0.13334476700662137,-0.5782055610007368,0.6977615131038183,-0.14662424162415477,0.39846723851290733,-0.9053867635436468,-0.9262113064040878,0.26606880442121106,0.26709550202708476,0.34732398082679905,0.877742132488674,0.33005272487416343],[0.3739876165467003,-0.5290029052316667,0.6800141746974675,0.061741258582894476,0.8597029821981638,0.5070491094442274,-0.046478179103550926,-0.5049907357811766,0.8618724590346327,0.9970094261619689,-0.07677980968177717,0.008778665582631524],[-0.17512692658441195,-0.18407503582893012,0.25366126596961397,0.16384959648278244,0.001520427711132824,-0.9864841600512421,0.9690066956632327,-0.18765714783331888,0.16065745742693402,-0.1848765358076913,-0.9822334158267265,-0.0322208525819588],[-0.019548063173408015,-0.04083817426243956,0.8004246762997052,-0.8181906413139743,0.5006326392279725,-0.28272077215136177,0.4149009945477472,0.17370256048240043,-0.893131896869185,-0.39802165668405254,-0.8480532890222718,-0.3498347892777237],[0.40858005685763266,-0.1705672738117351,0.057834824636337503,0.6970192267037036,-0.3506530828327319,0.625465117416849,-0.36731435488083847,0.5745305480547166,0.7314333968656481,-0.6158281921963663,-0.7395654567729949,0.2716589274150716],[0.36036108339046546,-0.10609824121138015,0.6683100563961043,0.3336196663063854,0.9037602856891184,-0.26817021509619404,0.8722895187265535,-0.18806625032281107,0.45137798019987163,0.3575037255378104,-0.3845106389953869,-0.8510832243241229],[0.2989038165048169,-0.4696757055452898,0.16741767114180664,0.5607143944193924,0.6381163749345793,0.5276427389164428,0.12992921587599637,0.5615406490693473,-0.8171844946550473,-0.8177516534698923,0.5267633163794985,0.23195396475190866],[-0.044294300875834455,-0.6678314869500035,0.7363529166740207,0.6429581123795389,-0.7466488196586771,-0.17064702115080757,-0.753480906176156,-0.5766687092971039,-0.3157842993968686,0.13737317697898266,0.3316153491922781,-0.9333594540297883],[0.1544257176951411,-0.3604337724522823,0.9291928975306804,0.1989803801813257,0.3946813744355622,-0.897013612481188,-0.28445709219045484,0.8991704901407365,0.3325305886119633,0.9378117993528586,0.18899482091772576,0.29118720208970483],[0.21426697931800615,0.02329039322818861,0.8814628459732454,-0.21545000222732252,0.9222221676750613,0.3210725307293633,0.7739458346317268,-0.03923405953866538,0.6320352313188846,0.5954738798607764,0.3846647397680579,-0.7052970270621635],[0.006444002401085914,-0.5211777206122231,0.9012102572245821,-0.4444331244185787,-0.8879862022786476,0.11815118485349677,-0.7502632774818584,0.2969047701262122,-0.5907220767319272,0.489473403140273,-0.3511809534115512,-0.798177753122219],[0.4083289220818836,-0.2745372600773065,0.7986957056196587,0.22084504553047382,-0.5493684809652029,-0.8058670721568344,0.10244779329811415,0.8347643164639273,-0.5409925929316028,0.9699133546420473,0.03691623058354909,0.24063515205911296],[0.40262390736496917,-0.4490866907053697,0.5381049490373464,0.9380689906869439,-0.3011423954440825,0.17128872227259423,-0.3012892423099477,-0.9531844955585769,-0.025770329742537047,0.17103029316432744,-0.027433102138497464,-0.9848837818378283],[-0.3080479501634174,-0.5855486731647569,0.16725482832943217,0.3545217684476305,0.6943706017799475,0.6262298165055049,-0.6870332263142348,0.6477373223854339,-0.3292745163677294,-0.6342709686222587,-0.31350570740052247,0.706692655820251],[-0.30385918567542924,-0.40990249055974504,0.32334412327943224,-0.31635993159048503,-0.8055656843429206,-0.5009793627418055,-0.8689929514772945,0.45790484496408745,-0.18754840239574394,0.3804834345498427,0.3760147353063934,-0.8448936470737713],[0.05900578467727447,-0.6686960458507809,0.5985093125031504,-0.1602174502099621,-0.8134126185632505,0.5591871606271863,-0.5165636685456703,-0.41364469293938166,-0.7497066388539746,0.8411256416177416,-0.40897185717988094,-0.35390630829074204],[-0.2622485505865141,-0.45509980723024157,0.5153795041345841,-0.8970805958406484,-0.14778768391771788,-0.4164195060854188,0.33459237527109437,-0.8427363525034304,-0.42171481190451576,-0.28860760034454136,-0.5176429663884662,0.8054510614381949],[0.2619084816095466,-0.6386118113843872,0.6328261782116669,-0.11037689578228049,0.955660141172336,-0.27300299531678895,-0.7770976020405107,0.08826462949310587,0.6231602298633417,0.6196259014763308,0.2809324647578415,0.7328988282598959],[0.23158471128931565,-0.41873009694842395,0.5976478040203742,0.9506559839090125,-0.09855730339749522,0.2941762366422578,0.23962271165961815,-0.36902945801601006,-0.8979967790439461,0.1970638381714896,0.9241773190591858,-0.3272034942076521],[0.020240756222784352,-0.3124800478692536,-0.03427633342552568,-0.3130250997181079,-0.41133689578851473,-0.856047454940167,0.509684294165464,-0.8333120592005421,0.21403955772708264,-0.8013970347009785,-0.36931418894218104,0.4704995458217337],[0.10993859764310111,-0.6439103290289452,0.40383017806780486,0.3647163520718538,0.7544359503000566,-0.5457182234690813,-0.8830392661461257,0.46614957387633393,0.05427917850358327,0.2953364809425195,0.4620941155974212,0.8362089400112426],[0.2987718072889938,-0.18451623064180997,-0.017558317848556916,0.9276578427668405,-0.3695744890837491,-0.05353152129033917,0.06057966083765707,0.007482917810162321,0.9981353167951931,-0.36848477776696076,-0.9291709761715184,0.02933028459632067],[-0.056023517816479726,-0.32997655719418995,0.8775533739328388,-0.3028634526137716,0.7049573187610323,0.6413336945741444,-0.010367210780671987,-0.6753361528108989,0.7374371848823489,0.9529775705710469,0.21669393030685113,0.21184308003018346],[0.12767187920424578,-0.5757232860172402,0.670443886717051,0.7761522195323227,-0.1370814664156882,0.6154643805131,-0.5644717168037442,0.28392770560760827,0.7750849881897037,-0.28099717622012355,-0.9489961694094049,0.14299250820442694],[-0.14176350816208047,-0.4976716221376709,0.08490175057726132,0.21547679232996259,0.4467035703361265,-0.8683465162112115,-0.774428307807889,-0.46350279748182066,-0.43061113872301937,-0.5948365725415675,0.765258830053913,0.24606579402980172],[0.16160401920056158,-0.0706859690083127,0.4888866924761994,-0.7214757742718585,-0.6863018225838907,-0.0919919314769066,0.06469665516962038,-0.19908281330080424,0.9778447608174317,-0.6894106540725096,0.6995397356592878,0.18803485922840962],[-0.005205765402171349,-0.4234473136784863,0.42236680611940336,0.9957794065674893,-0.046991097067969945,-0.0788366047749886,-0.0020911084350221446,-0.8703769754400855,0.49238150644523826,-0.09175511278186135,-0.4901385084036123,-0.8668017315731842],[-0.19276853827604665,-0.36742419942631294,0.6341301959192713,-0.9865253203730926,0.1512642186936745,0.06234523562999189,0.16360856041497812,0.9116789171984216,0.3769267712631352,0.0001766966482162069,0.38204801802771543,-0.9241424569293354],[0.21634515878281874,-0.284387113246906,0.4362956752790795,0.30014226494786866,0.7482754458538893,-0.591606691919663,0.51350447514821,-0.6494126067033349,-0.5608711262469672,-0.8038829360017073,-0.13545155365274847,-0.5791589607511325],[-0.30425796908540087,-0.36109904804157705,0.6737094599019361,-0.08409628253771331,0.14103775520605463,0.9864259561010011,0.9395954275113432,-0.3183991547827029,0.125628065468815,0.3317954910228791,0.9374061712193372,-0.1057422446176978],[0.014526935400796328,-0.23645202960707853,0.9053039877136685,-0.5987164081342246,0.8009438917596394,-0.005248321996168703,-0.6223395384842664,-0.46106236597545014,0.6325464358608863,0.5042144003001531,0.3819821683456481,0.7745046556321752],[-0.3881120740686898,-0.23393168309366,0.034014793590291165,-0.3147616852936868,0.9426807838965002,0.11080623242109543,0.08963507558877547,0.14573965752236817,-0.9852540309227147,-0.9449289045718055,-0.30018809419685966,-0.13037052353651418],[-0.03460573529911895,-0.1544363051964226,0.6653495472763556,-0.579362750154947,0.7259347302014303,0.37061782366782253,0.7354255729119932,0.6616251412203341,-0.14629217071408795,-0.35140863740090905,0.18780559095125351,-0.9171919262441678],[-0.0938086600840634,-0.443186142741229,0.3144410447614912,-0.972654606117049,-0.21333201678908603,0.09182846950695409,-0.19449397525741893,0.5320427166074819,-0.8240768418621514,0.12694530628524633,-0.8194022201078246,-0.5589855909542868],[0.24037452068875512,-0.6674189574819108,0.6235431531724027,0.7723773771258653,-0.6269052510715803,0.10209306285467294,-0.490275213248086,-0.6906254201954984,-0.5316641272968634,0.40381109764184664,0.36059164599123844,-0.8407795562825114],[0.5263826403603563,-0.24165445437879077,0.3958487949388051,0.2238270284406308,0.9368521910739296,-0.26871813005343337,0.010009692052292157,-0.27790840411691364,-0.9605554252546846,-0.9745774814782328,0.21230848075658004,-0.07158101420333207],[-0.23476320770514925,-0.048682904024874914,0.7619210642353137,0.6128112351514283,0.6913898765821821,0.38267796988049346,0.7140393052279301,-0.6919306989888487,0.10667604410746706,0.33854137215678043,0.2078748333889371,-0.9177024534029133],[-0.22360419769443046,-0.40227129802357525,0.5584882926025011,-0.3449188780365988,-0.5793581278982441,0.738495177514529,0.9155325372159122,-0.3811318155323936,0.12860292565869613,0.20695685751617054,0.7204739404220137,0.6618807749888254],[-0.11708703036411497,-0.7259974086057155,0.42130709860567256,-0.6905422103364421,-0.7024172646401661,0.1725150488481741,-0.6354793265923411,0.7031027739151392,0.31908073396333714,-0.34542362573716073,0.1107089682377345,-0.9318937939134109],[-0.038607634919667545,-0.6136600189586152,0.6593357830382731,-0.8821417996256129,-0.298271471057817,-0.36449962264217894,0.1378301290463861,0.5765439493063141,-0.8053570202371955,0.45036507511167145,-0.7606781211992913,-0.4674827216575759]],"left_grasp_frame":[[0.1297635809259946,-0.1520062696980556,0.6257630171922229,-0.12338279667719249,0.9412877379170881,0.31425161882008756,-0.9615256041133797,-0.03507151447659304,-0.27246743201108997,-0.24544897253505538,-0.3357787713945735,0.9093994823850716],[-0.1473963061441932,0.3571385949113416,1.0935486266687424,-0.38597724404061984,-0.8948459988687075,-0.22421464134055244,0.13533207646817202,0.1854943567368889,-0.9732815998967518,0.912527696106601,-0.406007982590777,0.04950476654774566],[0.06777090276796607,0.6744053179711996,0.02170662354779837,0.5256820639340574,0.25537393273623854,-0.8114447129268,0.02789542704567044,-0.9585383544178843,-0.28359490168159684,-0.8502236251763235,0.12644515644611343,-0.511010185420358],[0.17693369392061287,-0.1012819544796196,-0.21077705002057937,0.48171104932679554,0.8287403315192994,-0.28485773268380754,-0.6429533637986944,0.11336077429255495,-0.7574696738693478,-0.5954539754980408,0.5480317488659525,0.5874485214029433],[0.42181511791748755,0.34486120118805375,0.4146260299859148,0.6192329660633968,0.7826428704704387,0.06340876155643171,-0.661726892303923,0.5636204908126956,-0.494418307041566,-0.4226914403464071,0.26420083201581407,0.8669082227202783],[0.20889577481349433,0.8173059900620446,0.3299185283036925,0.8360228681802114,-0.06999862101215204,-0.5442113164351969,0.5166135046548431,0.43458883363074263,0.7377282917793726,0.1848681981520166,-0.8979046378874918,0.39948843609316026],[0.0338027194775983,0.7762499937759633,0.19876041568976915,0.5447914423944159,0.3815027739094552,-0.7467649682418178,0.44386353817369345,-0.8867322062070604,-0.12919424892420464,-0.7114685121449451,-0.2610778197674984,-0.6524192886876565],[0.38048739102069257,0.5408079922528947,1.0238578626277206,0.385703176998901,-0.4695487281809118,-0.7942021475144994,0.29253158987590056,0.878632884171451,-0.377398362181443,0.8750190444656805,-0.08676546960770654,0.4762493308196053],[0.1578055435558819,0.9269562787590724,0.40736158519177224,0.5086373332072454,-0.1005825050968922,-0.8550855062134168,0.7643614719361176,-0.40436431876590717,0.5022360380624195,-0.3962822270648452,-0.9090504152011688,-0.12879339709437296],[0.6185026341881328,0.2507946869766722,-0.010185322898569306,0.9512003298204026,-0.15053697876255634,-0.26936323166794485,0.1589281638671352,-0.5092452125259561,0.8458198107452998,-0.26449909504453556,-0.847353486771864,-0.4604696484853116],[0.6183491496753241,0.18599347613997821,0.2734344112449014,0.9037930392031706,0.3898357568827389,-0.17659622007154688,-0.4070597143284008,0.910441738226093,-0.07347264978705302,0.1321383035261806,0.13828927634315002,0.9815373374404618],[-0.27631488831118456,0.3259661913628653,1.0562033516176523,-0.32297234218741394,0.14491851248894905,0.9352472886461453,-0.3461369383234093,-0.9378295721596117,0.02578591691245319,0.8808394212965677,-0.3153954950865815,0.3530546637123178],[0.36540346953022074,0.022705398909298916,0.7971329407279537,-0.1658970731217139,-0.9016527655371871,-0.3993750762488703,-0.3681910222173645,0.43233341045105944,-0.8231179705037959,0.9148297832319305,0.010493455431744458,-0.40370330083481],[0.08915008694899809,0.738287024337641,0.7389674843115915,0.13550850265368844,-0.5756075863074275,-0.8064200842575122,0.9901538965405452,0.1075194089298366,0.08963725714764967,0.03510992557853992,-0.810626599174192,0.584509888570889],[0.010664993404376766,0.5988325360656316,0.011310866853594143,-0.37654629191547684,-0.9214489103393556,-0.09562842505823878,0.3834709588583879,-0.24900129706256005,0.8893527858917357,-0.8433047573427612,0.29821176988103165,0.44710941227790035],[-0.48043461795836906,0.5308477917295529,0.5534527914848594,-0.9634488841491236,0.23417121092762733,-0.13011568546680802,-0.06551922830626139,-0.6769244602046077,-0.7331306199435762,-0.2597565751890809,-0.6978087984178684,0.667524832868898],[0.01541692509591766,0.47272422824858895,0.3045849628479384,-0.7341756704924943,0.1133001907222388,-0.6694394308966275,0.6789590810275348,0.12368602186151748,-0.7236824816770943,0.0008069368884841277,-0.9858320520141232,-0.16773346140374465],[0.2141793401034896,0.03634436174309863,0.14152592655399276,-0.6962140818654087,0.6120671835987038,0.37504628377555815,-0.6694796022280831,-0.36513077261416693,-0.6468976589003157,-0.2590038887986695,-0.7014650965313473,0.663975680228903],[0.5291441457214988,0.6969487846379667,0.6519042198619712,0.9402547952476111,0.33969842165204445,-0.022932560715780745,0.2915107220019685,-0.7684158924284494,0.5697003731974211,0.1759045734816583,-0.54234859508484,-0.8215324597590387],[-0.5314183667120471,0.2535788100146737,0.74764280441043,-0.7614269058656136,-0.5945085007811534,0.25843511665960517,-0.5847566829938963,0.45783161096699077,-0.6696639737161811,0.27980115925044724,-0.6610218290633947,-0.6962481258745264],[-0.4894347248164193,-0.03928377557548854,-0.0357275516429779,-0.8864654138121859,-0.3741112031793813,0.2724332537897418,-0.3911440438163678,0.920285658851608,-0.008980150279421256,-0.247356841631214,-0.1145212371909132,-0.9621327762479008],[-0.188956066453104,0.6798916719311439,-0.09694817858529575,0.1265500953377032,0.5199330661252264,-0.8447808473915819,0.2737172909796635,-0.8368672799565027,-0.4740590684260334,-0.9534484348871586,-0.1712389047145108,-0.24822030441404688],[0.5262030649143004,0.2756094526109803,1.0248309816044883,0.7552499081463417,0.6414469180432266,0.1346975410977817,0.04037683200879727,-0.25065023154620747,0.9672353244494154,0.6541920877554817,-0.7250657299574148,-0.21520315880452454],[0.3478073332146267,0.1373071071511936,-0.07001640377482549,0.22469099145408716,-0.24895458600777365,-0.9420910637858091,0.631007954791226,-0.699545953771911,0.3353571522295916,-0.7425246927541017,-0.66981868641226,-8.93497680440336e-05],[0.4531770645823787,0.4057720540099886,0.9722764473895339,0.7518737634326136,-0.39377823833667513,-0.5287953695657169,0.5772129600677595,0.7807356722436443,0.23932615572912175,0.318607976346314,-0.48517059793306677,0.8143085706965003],[0.023241536202039295,0.5032669206256865,-0.1998824336035075,0.4233120999257972,0.8914671768458283,-0.1615337075131403,0.10894604664954777,-0.22709094938308802,-0.9677605383707905,-0.8994095980007366,0.3920662868888583,-0.19325217129268413],[-0.14288846124630594,0.32993251164799264,0.090335136194623,-0.3628791303237908,-0.4804920627859891,0.7984022259332162,-0.5825842604647009,0.7856903671019667,0.20805342223086443,-0.7272649560058884,-0.38963832542864923,-0.5650377501750758],[0.3986637131155642,0.7132368477572836,0.2849861337205165,0.33371563123782516,-0.8498404814238173,0.40792773085544715,0.8404689537069819,0.46420314859963113,0.27951274512064717,-0.426902582941625,0.2495728209631868,0.8691763869976256],[0.3762192555970496,0.12249925632874274,1.1210796464033361,0.3238752313515344,0.8501397122370045,-0.41517141543532804,-0.27767244902248667,0.5049125990903133,0.8172889809202858,0.9044350974895017,-0.14941799408855766,0.3995890607508009],[-0.23939432153942292,0.7284478354592625,0.3639333380738059,-0.8941751475409397,-0.4476200860361951,-0.009330814385016983,0.44201185941980153,-0.8859050747963759,0.14070435168208872,-0.07124830982127325,0.12169000380876151,0.9900076875058272],[-0.07526673856537136,0.1351078400462028,0.7041535900736444,-0.2802875626129875,0.22540167689422458,-0.9330771491670677,-0.9453371180399086,-0.23358406389060493,0.2275438822564786,-0.16666317979827133,0.9458501832476547,0.2785512795696829],[0.3513953801933045,0.757072810511386,0.09038305975402877,0.4556855535697905,-0.7387828448966431,-0.49653860308561737,0.8715886846269467,0.257026433027752,0.41745727632333746,-0.18078672821328023,-0.6230066779830321,0.7610380004247386],[0.15755106495070093,0.7892132591102459,0.501129172237192,0.14238043647218399,0.22799618461629834,-0.9631954895608686,0.4384694643541453,0.8578938985008481,0.2678854003935379,0.8873963827619421,-0.4604734506086406,0.022177942771877047],[-0.19276907419490463,0.9510155176165678,0.4595543211845725,-0.15721640992575037,-0.9295666951796331,0.33344978581920426,0.9854170606138356,-0.1699147128035609,-0.009066808924414742,0.0650862281995723,0.327161656655694,0.942724261548972],[0.15920961050754046,0.689299262009704,0.6421586463444406,0.5974215811232979,0.79322267531136,0.11783565580938313,0.35314055660977006,-0.39215376133012775,0.8494157843788726,0.7199855565811585,-0.4658467718537095,-0.5144002172122378],[0.44488499024000194,0.5830459244526182,0.87502719598378,0.8414545244293731,-0.22208153603771685,0.4925790034791033,0.06820322656387251,-0.8606831760580512,-0.5045520690046795,0.5360061596694715,0.4581530986490803,-0.7090790752762566],[-0.529276402433967,0.5586266789949197,0.12986853419408367,-0.5768376651709279,-0.3471372935953477,0.7394281624576182,0.07446276571142385,0.8790944100821942,0.47079540639734874,-0.8134578074877382,0.32663238902095504,-0.48124596401393915],[-0.18172853573568964,0.8921580479972299,0.6059595335176305,-0.6900506535907688,-0.32258858011307434,0.6478940526502646,0.6011572130221456,-0.7539570180900654,0.26487510283940063,0.40303858464221454,0.5722634208591069,0.7141949848859438],[-0.303921085009651,0.7577792499373959,0.06415623724815217,0.1635314643527469,-0.9021357337740524,0.3992600380887101,0.7814906566079334,0.3654610405015537,0.5056783379877201,-0.602104487444785,0.22932367022065933,0.7647750260536617],[-0.24922090658376073,0.6033172089011329,0.7067062904774979,-0.7043675149096568,0.40743980000954033,0.5812565813031112,0.7007882517047541,0.5294696128225762,0.4780771437436696,-0.11297004121707956,0.7440797930771249,-0.6584702205276364],[0.21893225964363552,0.7968847162686012,0.7034645735059629,-0.010466911526967104,-0.1816467596691309,-0.983308140139596,0.7320367255604815,0.6684959493833526,-0.13128365507076026,0.6811847592201213,-0.7211918055273197,0.12597501119853108],[-0.5529694546333672,0.09829173070383711,0.9690739797502292,-0.8389768037422325,0.5134424458502608,-0.18026307880919373,-0.07980928263344222,0.211582498659525,0.9740961578132457,0.5382828263914825,0.8316307480248035,-0.1365353351762941],[0.418103114388456,0.054853927641420275,0.6319673247679912,0.20700180917113148,0.8091849767630004,-0.5498817367224894,-0.9697956423501358,0.2438386801608966,-0.006253809725230486,0.1290219480498026,0.5345674620087296,0.8352191122591602],[0.16245629276956153,0.6722430208658761,0.6345467861917111,-0.3617679165068822,0.7904900964413467,0.49421592650826185,0.8742152529932399,0.4717959378349935,-0.11470084775782816,-0.32383895074594543,0.3905560145107862,-0.8617391330960986],[0.5540073306723331,0.5125346820822365,0.7734679910938949,0.9395215241204977,-0.09891206192399576,-0.32789588243867984,0.2837518005098805,-0.31132999250506166,0.9069501372590443,-0.19179213076061807,-0.9451402222806635,-0.26443475339969996],[-0.10979484736170274,0.7036213692008922,0.9617422368217046,-0.13208176631980723,0.6602459688860822,-0.7393440792861732,0.9393731931800474,0.32148374394073204,0.11927366145369889,0.3164370568384201,-0.6787661327375789,-0.6626795048195695],[-0.5283242783100114,0.5783558238222009,0.06650205750072041,-0.2362434790888952,-0.8780514668733931,-0.4161906295310567,0.7387100645503033,-0.44054783518671525,0.5101225788419738,-0.6312657595023363,-0.18693107401869102,0.7527020090621236],[-0.08189134081389597,0.9306975809432311,0.5058275319178236,-0.2926700731116753,0.7781580573938615,0.5557105955601516,0.8550570413340877,-0.04717572321670635,0.5163834885082763,0.42804402153260257,0.626294251013046,-0.6515626038825466],[-0.3626165556916989,0.1374121555044992,0.009940104097437924,-0.625448646338958,0.49972637084297516,-0.5992391384721749,-0.7795111014477176,-0.43394548796689825,0.4517230968114616,-0.03429917655763004,0.749643160281797,0.6609528717920731],[0.16870778418759413,0.8844564915951582,0.6119172434660916,0.4213871329626654,-0.9064132234392013,-0.029119624792590698,0.8504425892205691,0.4061096141643388,-0.3343985402526548,0.31492901837290177,0.11614667303822436,0.9419817746266816]],"right_grasp_frame":[[0.06869875511796093,-0.4283393238615767,0.8710421269857435,-0.020363941854195544,-0.42948123642683994,-0.9028461537988808,0.5332007356474168,0.7592278897202436,-0.3731889427300861,0.8457436286353048,-0.48899783131238816,0.21353883860920636],[-0.4741827601474209,-0.4159311191363464,0.24514453636319403,-0.8862447781742169,-0.249328962285716,-0.3903911650184483,0.44235270121428993,-0.20544947259309776,-0.8729917536492766,0.13745646906331638,-0.9463749694369279,0.29236986222232203],[0.24937382213581424,-0.6716753723220691,0.9555182079935843,-0.040158421300363956,-0.9440548541552751,-0.32733428409582244,-0.2872352039312911,0.32467796312504443,-0.9011549022690393,0.9570178884541354,0.05783297162375839,-0.2842043429857354],[-0.23763159735224146,-0.6052714582045995,-0.1602490548222235,-0.04030286094606339,-0.9319048423535734,-0.3604567161220939,-0.4432700048355387,-0.30663191677333207,0.8423114450302892,-0.8954816481719007,0.19372721134077636,-0.40072732047298987],[-0.5123282446459079,-0.5319644338102982,0.18035361797116894,-0.8129163568395035,0.3786389176709925,0.4424924483058759,-0.16802254613523243,0.5750062989703612,-0.8007097977011655,-0.5576158361928243,-0.7252587994428933,-0.4038121507057834],[0.3735791825741528,0.047686338070455,0.3373339648453698,0.6825439908401001,-0.054297168173405574,-0.7288247512923929,0.17103578641424427,0.9814108640091448,0.08706018476022612,0.7105494073839835,-0.1840775204449816,0.6791428466332373],[0.5516859117781885,-0.5144611577236917,0.7503430346298677,0.7741384119753318,0.4996883361982478,0.38861457225860463,-0.2594055105722151,-0.30957594755782947,0.9148068177371879,0.5774240211396472,-0.8089958586798196,-0.1100327244496896],[0.04171934146090078,-0.5904273528997134,0.30019265359134883,-0.807782091520473,-0.24350434023106982,-0.536836780509161,-0.5200357181696051,-0.13446565746891187,0.8434938285424951,-0.2775805188528778,0.9605335096966086,-0.018012004421567085],[-0.21725458589344881,-0.3425521718934203,0.3939491954626353,-0.4425000421953641,-0.18318323635962677,-0.8778596781797842,-0.13395338780634541,-0.9544301102215184,0.2666826852229878,-0.8867075067643804,0.23559937737471537,0.3977973489458452],[-0.3256421389343063,-0.0035978648318980566,0.31124132485948597,-0.6824107394860102,0.14626558065401052,0.7161857039554073,0.716069854780297,-0.06304426412693459,0.6951757934764543,0.14683169177354538,0.9872344203261141,-0.061714290195060376],[-0.16792613817246543,-0.7929113134959607,0.7785438336928546,-0.14662424162415477,0.39846723851290733,-0.9053867635436468,-0.9262113064040878,0.26606880442121106,0.26709550202708476,0.34732398082679905,0.877742132488674,0.33005272487416343],[0.3886279385600072,-0.5392467541931819,0.9113168751481979,0.061741258582894476,0.8597029821981638,0.5070491094442274,-0.046478179103550926,-0.5049907357811766,0.8618724590346327,0.9970094261619689,-0.07677980968177717,0.008778665582631524],[-0.13773184744680678,0.04082620741557075,0.21075143551818815,0.16384959648278244,0.001520427711132824,-0.9864841600512421,0.9690066956632327,-0.18765714783331888,0.16065745742693402,-0.1848765358076913,-0.9822334158267265,-0.0322208525819588],[-0.20953762872507284,0.05485691492814361,0.7078685874006165,-0.8181906413139743,0.5006326392279725,-0.28272077215136177,0.4149009945477472,0.17370256048240043,-0.893131896869185,-0.39802165668405254,-0.8480532890222718,-0.3498347892777237],[0.5706731599782371,-0.2553237524418547,-0.0848619866698553,0.6970192267037036,-0.3506530828327319,0.625465117416849,-0.36731435488083847,0.5745305480547166,0.7314333968656481,-0.6158281921963663,-0.7395654567729949,0.2716589274150716],[0.43759023701211497,0.09654718776513666,0.7507157761721439,0.3336196663063854,0.9037602856891184,-0.26817021509619404,0.8722895187265535,-0.18806625032281107,0.45137798019987163,0.3575037255378104,-0.3845106389953869,-0.8510832243241229],[0.42931428629238894,-0.44004403713416096,-0.02214838147035722,0.5607143944193924,0.6381163749345793,0.5276427389164428,0.12992921587599637,0.5615406490693473,-0.8171844946550473,-0.8177516534698923,0.5267633163794985,0.23195396475190866],[0.10475954018498791,-0.8428296410418392,0.7676389077157832,0.6429581123795389,-0.7466488196586771,-0.17064702115080757,-0.753480906176156,-0.5766687092971039,-0.3157842993968686,0.13737317697898266,0.3316153491922781,-0.9333594540297883],[0.2000267415659863,-0.4262174261087556,1.1469387866756555,0.1989803801813257,0.3946813744355622,-0.897013612481188,-0.28445709219045484,0.8991704901407365,0.3325305886119633,0.9378117993528586,0.18899482091772576,0.29118720208970483],[0.1644851881829932,0.20323388336981185,1.0191666161941129,-0.21545000222732252,0.9222221676750613,0.3210725307293633,0.7739458346317268,-0.03923405953866538,0.6320352313188846,0.5954738798607764,0.3846647397680579,-0.7052970270621635],[-0.09658663807537113,-0.6956012499164743,1.0142648203967959,-0.4444331244185787,-0.8879862022786476,0.11815118485349677,-0.7502632774818584,0.2969047701262122,-0.5907220767319272,0.489473403140273,-0.3511809534115512,-0.798177753122219],[0.4590593181194457,-0.25110841443286597,1.023857271646459,0.22084504553047382,-0.5493684809652029,-0.8058670721568344,0.10244779329811415,0.8347643164639273,-0.5409925929316028,0.9699133546420473,0.03691623058354909,0.24063515205911296],[0.6203545260348443,-0.5189991897741859,0.5771668854151832,0.9380689906869439,-0.3011423954440825,0.17128872227259423,-0.3012892423099477,-0.9531844955585769,-0.025770329742537047,0.17103029316432744,-0.027433102138497464,-0.9848837818378283],[-0.2254106969441672,-0.7451399949433524,0.020551354957673407,0.3545217684476305,0.6943706017799475,0.6262298165055049,-0.6870332263142348,0.6477373223854339,-0.3292745163677294,-0.6342709686222587,-0.31350570740052247,0.706692655820251],[-0.3775649546667511,-0.6116182521174114,0.4110847972146637,-0.31635993159048503,-0.8055656843429206,-0.5009793627418055,-0.8689929514772945,0.45790484496408745,-0.18754840239574394,0.3804834345498427,0.3760147353063934,-0.8448936470737713],[0.022186270161007146,-0.7890027345296432,0.7934216997850102,-0.1602174502099621,-0.8134126185632505,0.5591871606271863,-0.5165636685456703,-0.41364469293938166,-0.7497066388539746,0.8411256416177416,-0.40897185717988094,-0.35390630829074204],[-0.47062343728748535,-0.37774095925616546,0.4489285452364525,-0.8970805958406484,-0.14778768391771788,-0.4164195060854188,0.33459237527109437,-0.8427363525034304,-0.42171481190451576,-0.28860760034454136,-0.5176429663884662,0.8054510614381949],[0.23613140830804658,-0.8185019860357027,0.7770318724887247,-0.11037689578228049,0.955660141172336,-0.27300299531678895,-0.7770976020405107,0.08826462949310587,0.6231602298633417,0.6196259014763308,0.2809324647578415,0.7328988282598959],[0.45231220380025283,-0.3637010324346999,0.6431603387177365,0.9506559839090125,-0.09855730339749522,0.2941762366422578,0.23962271165961815,-0.36902945801601006,-0.8979967790439461,0.1970638381714896,0.9241773190591858,-0.3272034942076521],[-0.052913279345256824,-0.19410410405793402,-0.21989917068670178,-0.3130250997181079,-0.41133689578851473,-0.856047454940167,0.509684294165464,-0.8333120592005421,0.21403955772708264,-0.8013970347009785,-0.36931418894218104,0.4704995458217337],[0.19420843498693438,-0.8487335669348863,0.47286821420564795,0.3647163520718538,0.7544359503000566,-0.5457182234690813,-0.8830392661461257,0.46614957387633393,0.05427917850358327,0.2953364809425195,0.4620941155974212,0.8362089400112426],[0.5139466206895094,-0.16983845997142408,-0.1030251384996192,0.9276578427668405,-0.3695744890837491,-0.05353152129033917,0.06057966083765707,0.007482917810162321,0.9981353167951931,-0.36848477776696076,-0.9291709761715184,0.02933028459632067],[-0.12588427949269237,-0.33192075854985736,1.0987679954322054,-0.3028634526137716,0.7049573187610323,0.6413336945741444,-0.010367210780671987,-0.6753361528108989,0.7374371848823489,0.9529775705710469,0.21669393030685113,0.21184308003018346],[0.30811687400358956,-0.7061912159526391,0.6053444411261961,0.7761522195323227,-0.1370814664156882,0.6154643805131,-0.5644717168037442,0.28392770560760827,0.7750849881897037,-0.28099717622012355,-0.9489961694094049,0.14299250820442694],[-0.09231754820529213,-0.6776011516560327,-0.05294118960196084,0.21547679232996259,0.4467035703361265,-0.8683465162112115,-0.774428307807889,-0.46350279748182066,-0.43061113872301937,-0.5948365725415675,0.765258830053913,0.24606579402980172],[-0.005829362105714192,-0.0550657743033464,0.32906714721428165,-0.7214757742718585,-0.6863018225838907,-0.0919919314769066,0.06469665516962038,-0.19908281330080424,0.9778447608174317,-0.6894106540725096,0.6995397356592878,0.18803485922840962],[0.2257568220288427,-0.42362469357390725,0.40053869466779335,0.9957794065674893,-0.046991097067969945,-0.0788366047749886,-0.0020911084350221446,-0.8703769754400855,0.49238150644523826,-0.09175511278186135,-0.4901385084036123,-0.8668017315731842],[-0.421594568102452,-0.3292329066550423,0.6335935989158068,-0.9865253203730926,0.1512642186936745,0.06234523562999189,0.16360856041497812,0.9116789171984216,0.3769267712631352,0.0001766966482162069,0.38204801802771543,-0.9241424569293354],[0.28560570878788993,-0.165609241006702,0.24944009472263795,0.30014226494786866,0.7482754458538893,-0.591606691919663,0.51350447514821,-0.6494126067033349,-0.5608711262469672,-0.8038829360017073,-0.13545155365274847,-0.5791589607511325],[-0.3231510335450444,-0.14304284767687503,0.7506169387569388,-0.08409628253771331,0.14103775520605463,0.9864259561010011,0.9395954275113432,-0.3183991547827029,0.125628065468815,0.3317954910228791,0.9374061712193372,-0.1057422446176978],[-0.12437316303991816,-0.38043385995716894,1.0227612560634713,-0.5987164081342246,0.8009438917596394,-0.005248321996168703,-0.6223395384842664,-0.46106236597545014,0.6325464358608863,0.5042144003001531,0.3819821683456481,0.7745046556321752],[-0.4610646983063943,-0.21375293604207107,-0.1852816894874369,-0.3147616852936868,0.9426807838965002,0.11080623242109543,0.08963507558877547,0.14573965752236817,-0.9852540309227147,-0.9449289045718055,-0.30018809419685966,-0.13037052353651418],[-0.16878104293052287,0.0160843762823073,0.5832526611231788,-0.579362750154947,0.7259347302014303,0.37061782366782253,0.7354255729119932,0.6616251412203341,-0.14629217071408795,-0.35140863740090905,0.18780559095125351,-0.9171919262441678],[-0.3193983820183219,-0.48882204258133677,0.3435418473175654,-0.972654606117049,-0.21333201678908603,0.09182846950695409,-0.19449397525741893,0.5320427166074819,-0.8240768418621514,0.12694530628524633,-0.8194022201078246,-0.5589855909542868],[0.4196229289498459,-0.7814906845581081,0.7166982063027558,0.7723773771258653,-0.6269052510715803,0.10209306285467294,-0.490275213248086,-0.6906254201954984,-0.5316641272968634,0.40381109764184664,0.36059164599123844,-0.8407795562825114],[0.5781405476840433,-0.23993264305067163,0.1697108522993113,0.2238270284406308,0.9368521910739296,-0.26871813005343337,0.010009692052292157,-0.27790840411691364,-0.9605554252546846,-0.9745774814782328,0.21230848075658004,-0.07158101420333207],[-0.09235734271995893,0.11703446096182502,0.8398860516699606,0.6128112351514283,0.6913898765821821,0.38267796988049346,0.7140393052279301,-0.6919306989888487,0.10667604410746706,0.33854137215678043,0.2078748333889371,-0.9177024534029133],[-0.3031607136430725,-0.1897956123537819,0.606914096418903,-0.3449188780365988,-0.5793581278982441,0.738495177514529,0.9155325372159122,-0.3811318155323936,0.12860292565869613,0.20695685751617054,0.7204739404220137,0.6618807749888254],[-0.2771787863767464,-0.8732234676024722,0.340589492626087,-0.6905422103364421,-0.7024172646401661,0.1725150488481741,-0.6354793265923411,0.7031027739151392,0.31908073396333714,-0.34542362573716073,0.1107089682377345,-0.9318937939134109],[-0.24348440542076447,-0.5821880176286633,0.7635242504774689,-0.8821417996256129,-0.298271471057817,-0.36449962264217894,0.1378301290463861,0.5765439493063141,-0.8053570202371955,0.45036507511167145,-0.7606781211992913,-0.4674827216575759]],"torso_link":[[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0],[0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0]]}, -"full_q": [[0.0,0.0,0.0,0.14345071753370492,-0.3098374025788363,-0.5255347143607776,0.7792534501713814,0.4189000808417968,2.2871373785168285,-0.3090922181450222,0.903638890970159,-0.3527103564167966,-0.18750795902859696,-0.23424867825464224,-1.140222721280617,-2.139803602024264,0.3498724880490446,-0.19640966686612138,-0.13178048349257032,-1.6152526191406191,0.8004940171036221,2.9346488535298167,1.9704848873353376,-0.4109335951308114,-2.871893514353677,0.21478926377614505,0.19601538138491925,-1.6092712532372437,-2.0893129936300623,-2.2178873674398805,2.9702498282140013,-0.6809311575681383,-0.4187804924461013,1.0290064604176123],[0.0,0.0,0.0,-0.16810743836767195,-0.8084253013932006,0.16838942521617828,0.5383617953645458,0.19856725652810053,-3.0874695776558005,3.0319501577314565,0.7506480576569451,0.3080031738163792,-0.0992566119044068,-0.009696367483391588,-2.760246096800696,-1.1131055334026216,0.19391917461712532,-0.15901435656721574,0.1667568837571029,-1.7891346165394584,-2.8447710694291946,1.1023603086376996,2.535866627792416,2.7609350166676183,-2.587382915843323,0.45858439781050153,-0.12434543820709365,0.3238888066759462,-1.1423939111090062,-0.647532903270668,2.669490494753881,0.7713420061102285,0.3398490167647391,-0.7608118634358956],[0.0,0.0,0.0,-0.33110138810510337,0.2752623861550191,0.18499174814260982,-0.883391500232066,0.19559687995688418,-0.7190517269164123,0.9362979385740473,0.24724351894216062,-0.7907265895095392,-0.130053158489718,0.31104966153158214,-0.2491717893927503,-2.775423842510102,1.164123541729856,-0.23793440855043835,-0.08525938841649972,-1.6867882442840703,0.5972938951587476,1.3976649408335917,-0.6633889200182115,0.6651273352406872,1.4143351000639068,0.1489062998737879,0.6910745259432831,-2.2005646319139993,-1.2588947251413383,-0.5338885540738598,1.586254916941239,1.266896827151002,0.3673557695816718,0.744292930687291],[0.0,0.0,0.0,0.7679788256652906,-0.2639040629179103,0.5334091240535362,0.23672320393577775,-0.10504552995405037,0.08046298945782127,2.7826200602581017,-0.05778581493619775,-0.8887167791353145,-0.2552992482645734,-0.23936241109116388,1.383074828958494,-0.6281354135839643,1.6134840215050588,-0.1829310913947778,0.24424245945849787,-2.2518871333509725,0.04083956658288823,-0.18144350157217753,2.502397495564984,2.0370039780811693,-2.0195879745025733,-0.2747160106981532,0.24656869472151977,0.3513420213225946,-0.4385084794566332,1.0457114818222228,2.066977902385342,-0.8310338411588787,-0.22446550240369884,-0.6446000950409725],[0.0,0.0,0.0,-0.18999907233457666,-0.0059780698322212805,-0.5934719928236388,-0.7820841444032863,0.31420133777579823,-3.00035538645568,2.33186553578499,2.1115482255308087,-0.2847868949851696,0.2409089933235552,-0.18111993238127905,-0.3427633049235612,-2.320385771757055,1.9479025584362062,-0.07247511463981315,0.14765620370540755,-2.143040614906868,0.8807276001686168,2.109696986837284,-2.546694401909443,3.0044204199449336,1.6074308838020412,-0.19466507371159736,-0.811703657183605,0.4376711446765418,-0.8538896762529204,0.10974950265892147,2.829922285956508,0.4794513521672519,-0.3865250717766136,0.8095682196847158],[0.0,0.0,0.0,0.6577368182701151,-0.3729150137177456,0.6520489022202081,0.05611505635051299,0.2565950626916425,0.4815911472935528,1.4204937387412107,0.6214963154309354,-0.06040585694680356,0.04074186956459169,-0.3124138142035602,-2.8219875997063752,-2.0900352844201944,0.7382423910381969,-0.2641885128650857,-0.11189033864882522,0.16380161083722378,-2.6842973334013207,2.4808800638890025,0.4879792844322566,2.256574147644799,0.7769577983090223,0.35062279915408845,-0.6467470048179236,-0.3035620967787591,0.10946754612078191,-2.2969020257800477,3.0424053475486597,2.5763703591389095,-0.24094301249543534,-0.8393951286611969],[0.0,0.0,0.0,0.2718813753264725,0.8374728805971595,0.4280738774372469,-0.2036478516231453,-0.22461955193016353,1.6839245818956248,0.5992391749023864,0.479063485383296,-0.06878133257643293,0.239912003415634,0.4285494255973666,1.2346360475664304,-0.24317428499528537,0.7201015413001561,0.18646023855609994,0.11648889467610796,-0.9541071239811112,-1.9139919751672552,2.497367489206354,-2.37000725046287,0.43709632941182086,2.0682447220263835,-0.22584384105356906,0.524124540577894,-2.3224658488463827,-0.6985358782901847,-2.5254192426793933,0.40204756920930396,1.707788318363626,-0.42736763772352027,-0.984465237378298],[0.0,0.0,0.0,-0.5432500683671112,0.15325286978928515,0.5663407268631445,-0.6005423400698757,-0.16215816115457848,-0.1728273161878997,2.3875046775162647,1.1378137030187725,-0.5839958351502803,0.12886142926206767,0.11558586453020209,-2.4102403928859846,0.07062606157385343,1.17779592373068,-0.4671944666558577,0.12469884934925907,0.34332340331411526,0.8440207792605259,2.670968523953277,-0.6321344996316762,1.6885515425901698,2.56072925034034,-0.2353894127602544,-0.07807586078918605,-1.79909797005996,-0.7309834085407187,-2.5492904524393705,-0.27098885783468984,0.26698452438794495,-0.3229935630796149,0.667071137580683],[0.0,0.0,0.0,-0.08078863519494539,-0.5313609123570966,-0.46049641379247785,0.7064501611059615,0.28357474730366244,-2.1105229349605117,1.0392907669071567,0.02417470954603798,-0.3338565260165094,0.20150736252947532,-0.2252377218122311,0.5824822902474889,-2.5891990166310537,0.1482285294773421,0.23037563949285722,-0.2325114769319446,-1.3619246622930476,-0.001131851817910423,1.8042216065359984,2.0551957752569434,1.8648111679117052,-1.2276249188805355,0.39559137180097514,0.4347660125907389,-0.2572521431456045,-3.389498432138258,2.040089771485439,-0.8977649794312533,-2.2230086065871353,-0.41143263305794525,-0.6643765527786258],[0.0,0.0,0.0,0.43390036810587324,0.09890589393279145,-0.7700850687313722,-0.45707448147970486,-0.3545608273393621,-2.35676362096139,2.6290560558263634,-0.04318889918245933,-0.5355635152965809,0.1323400872901071,-0.2961873175340124,1.985691339973838,-0.6044139785846276,1.4446089990490574,0.38912709046404126,0.11964524057131071,-1.8775505389040013,-0.7779230001222484,-0.005076692081322609,-1.8720749265955514,1.8002148327373788,2.0894068679322997,0.20685586469551964,-0.394466269462601,0.2152918137258535,-0.3731352808604589,2.489113254012553,-0.8564623241421322,1.2280978758854153,0.1706439232647574,-0.5888514751931757],[0.0,0.0,0.0,0.12004332379963117,0.44793641406267215,-0.24346409748465941,-0.8518613752270912,-0.36134079024767995,2.0339591433427997,1.5112082011641568,2.1684497439406134,-0.6287253150711185,0.2206283693259402,-0.09106596516374199,-0.9271151331170797,-0.5960207391792682,0.43855662774739046,0.23769863782204992,-0.12219080049726683,0.9828060339369533,-0.8078409217185878,-0.1260954028917391,0.9441820397767899,0.2478906755417032,0.5760723712489058,0.026888084084436492,-1.0686148768075094,1.494442784338871,-1.2399677079517883,-2.422228985478398,0.34946773460743374,1.973996508144146,-0.06793718307563379,-0.860318968449531],[0.0,0.0,0.0,-0.5849811757639146,0.06931378649193001,0.3287557603408941,0.738181734430418,-0.395144737362453,-0.8928684982004667,2.7543144164892155,1.8051342677071198,0.5105639767682034,-0.20473575967594776,-0.030948037308885123,-0.6101048469555472,-0.5874321231999478,2.0081460173545804,-0.7333311291893425,-0.20863523890067084,2.0055883767324256,-0.9984201477700667,2.7208239075092036,-0.03507405022712984,0.5985033750182289,0.5174328565611694,0.45646948896686734,0.4823851027987829,1.2574562013182011,-2.6152800612490044,0.7451279992088091,1.7088332850606516,2.3954546356581936,-0.09871917450893825,-0.9834680983788542],[0.0,0.0,0.0,-0.6758683985233971,-0.28222392904325044,-0.6448473553089131,-0.21845697540685066,0.10667612721536784,-2.6335675504553877,0.6706672736083745,1.5104324780360572,0.0837166086699197,-0.24979705934321222,0.33475728403812094,0.5284217765995685,-1.8176521688028726,2.032566572068784,-0.17668871299716937,0.1627554148487972,-2.2617812576935314,-1.412361180949608,0.17945036323896907,-0.10640404397813397,0.14468922893892722,1.2946277340176628,-0.1794557938228124,0.42525955899295,0.9713160936095258,-0.7393446117283267,1.3028296044476342,-0.7816208115067418,-2.370735060776926,-0.2068595420883113,-0.049304150806216196],[0.0,0.0,0.0,0.974639800503508,-0.10387289244313058,-0.19453290593082154,0.038008288570917925,0.3939875348042126,-2.1429919741009718,2.5098205325937784,-0.01469544060173765,-0.6192489995747148,0.16223419277604312,0.14741778694143387,-1.6305672519475263,-1.7159819842098976,1.338317660972931,-0.1973668119595262,-0.03198876776138143,0.059018093616034495,0.029310571501142135,2.9216003862205433,-1.3312110118691451,0.06886203370276522,2.2054787316137707,-0.18461239127575502,-0.09795347790018494,-2.645063947150777,-0.39484628506497277,0.5264837139447063,-0.07160863976412646,0.9140021729207759,-0.4197391088891395,0.5789051598570296],[0.0,0.0,0.0,0.8552284952637129,0.4351297831172731,0.2702425904701322,-0.07883676191519538,-0.0026864990193083416,-1.9045938719498983,0.029743044629437776,0.13191618203962857,-0.31583818119985596,0.1317417494211745,-0.3348057473464753,1.3102829488356131,-1.9517362340988158,1.1855475560323798,-0.5021039340319591,0.06891105513287515,1.8581610789164533,-0.9545181275046661,1.3401915667974225,0.5461354892012094,3.1535428742408325,-0.5655461637447154,-0.44556179751716507,0.2017763802620265,-0.8715151397599872,-0.00464395098856496,0.34532715892864463,1.5781796646233393,-0.3027169043205533,0.34364279984766954,-0.30779491537968384],[0.0,0.0,0.0,-0.7734712391862362,-0.3303269766556728,-0.35736001654936406,-0.4061036188179925,0.006064735113411046,1.86943594383763,0.29340632457371213,1.1397578176784404,-0.31210380580543895,0.2270487446609259,0.08326576607018249,-0.9608065344363275,-2.029041423288475,1.6702416977662908,-0.5768885081469584,0.25491390284184345,-0.15315111148280902,-0.42262394094634903,2.0743981305469403,2.7140499214380274,0.2973803269236692,1.1987081404718287,0.07466059204005349,0.43112133119605467,-1.860081490623698,-0.5935090282424502,-2.2513037264533615,2.874376059467222,2.254405624722269,-0.18231638734408762,0.08490780886017335],[0.0,0.0,0.0,-0.1741924823254108,-0.6439021423330293,0.12220186608268585,-0.7349242914247398,-0.27146248018879265,-2.29891710727597,1.5569951035545258,0.6447242823012176,0.1929007651432838,0.24872599185860123,-0.4147210993177517,-0.0685397899169704,-1.0162899004124664,2.0493639160491894,-0.44720600891228046,0.06809182590560531,-0.46764975699318523,-1.3037528349946033,-0.10616584503777517,2.1810617689625618,-0.7614930147842234,-0.74504702323136,-0.2539249234354976,-0.1178316781814257,-1.1730544165886285,-0.4861547942577724,-2.6685728646979845,0.4325428720587632,-2.2017540887343054,0.4111835729322396,1.0234792075422061],[0.0,0.0,0.0,-0.8899671760617184,0.350385596880316,0.07793209330129287,0.28127379519826246,-0.19367681908679388,0.2566686557227973,3.1219962966740913,1.308447403632588,0.40074055662708175,0.24696512410431437,-0.2770335568727506,1.430698840609749,-1.6150979108781034,1.3598947248881155,0.32386979529498994,-0.13722408075107995,-0.03680965229942812,-1.3973136163994022,0.4354381921044161,0.7202942944124415,2.85089117523174,1.3235640023203947,-0.2741817400608452,-1.268388706444282,0.01719879606447705,-2.1717408903790094,-0.12867229507432354,1.7560332722738223,-1.9035825894209193,-0.21471566067606448,-0.9348130368743077],[0.0,0.0,0.0,0.2937242610598385,-0.5001812439088723,-0.005687721609242413,-0.814562724122287,-0.40948401183797956,0.3945097362367851,0.9425828369661324,0.8199525855591943,-0.15875940264854171,0.16012816318883327,0.24936486536876962,-2.5253174987918237,-2.5406176066864745,1.8137034726500367,-0.8161310096077018,-0.2515512525080076,1.89416159037162,1.3703404953597622,2.4808935459661083,2.067196155954636,1.634019290339127,2.444745044777142,0.1915605087806147,0.6199649129344842,0.19882720443116364,-2.7449716213810538,2.394919212246732,1.9794128558785447,-1.0960099093138544,-0.20154911229690425,-0.24308314014327292],[0.0,0.0,0.0,0.2381792990246721,-0.8547845361960938,-0.461098075601321,-0.0016071318872201978,-0.3097421967067537,1.3098693847189566,0.6813650796421367,0.577306193669155,0.0280772775994248,-0.17338717544375534,0.2144388990192933,0.055617105624874874,-0.1746705129353492,0.4231869465539243,0.28736514365269794,-0.10859883693209355,0.8852567477598035,-0.5437365429783134,2.67875279657489,2.8922921253551275,0.3992255229780244,0.24315229269403593,-0.2910672665268096,-0.7853782524559294,-2.748879356966032,-0.2541592999199058,-1.4549996003255508,2.039155135424801,2.729527714127122,0.07096104852694152,-0.6268763926626492],[0.0,0.0,0.0,-0.7830158161524301,0.0009597087084513567,0.5733461261526811,-0.24116287077419868,0.39497856807942805,-2.741308051803605,2.0872787988753636,-0.05058805830577373,0.3982862303454765,-0.035885252885613894,0.053618001357354506,1.4705199093007342,-0.8527503361091022,0.1478079277885923,0.030990676457526622,0.23655737918612396,1.588251454110917,0.3659159859862955,-0.12891498587875205,-2.632599929788563,0.8312955723780306,-2.894041614145575,0.23673682748471025,0.11764172729016353,-3.109309024032635,-1.096818529895783,2.1024725495014813,1.6036921906327153,-2.4589462473637838,-0.32928773403722433,0.5093045882044218],[0.0,0.0,0.0,-0.13884479643972272,0.6007471676250166,-0.4160174456310629,0.6683969240075374,0.15502885396834182,1.4967779517736202,0.3191620277779979,-0.09242685846160607,-0.3140066899858285,0.10065826855718657,-0.35335098900358036,-2.348329696195475,0.4205996400831218,0.4771863264192857,0.284496352139039,0.07573903331808668,0.029608532729266557,0.49840659595061565,0.9162827239800084,-1.5120035493211743,1.4026830839366735,0.8358203842432648,0.19817653204644714,0.730142140156504,0.8908371530427677,-3.146165954474042,-2.0039165848890947,1.6022907685141565,2.037008343535904,-0.3986700895308648,0.560296642602172],[0.0,0.0,0.0,-0.06805892069509362,-0.08366455786993632,-0.987714871929688,-0.1130820800974543,-0.12193980272728361,-1.2109615597041616,2.758448291329414,0.5032312013455464,-0.5252751106354168,0.1473637162830032,-0.058667603326634066,-0.7242477155631684,-0.5616235581715086,1.0391232771680348,0.4916116702516825,-0.21826434572073955,-0.30897642442242423,-2.4317719944208567,0.5922180366780914,1.344480478445524,1.6054303709912736,-0.638496669764375,0.011539403470648135,0.18080711762399004,-1.5168711693331924,-1.2935908910077814,1.3133645881917646,0.3885575880654357,-1.471807360355262,0.021989103786424546,0.48488890913167637],[0.0,0.0,0.0,-0.9419644160550219,-0.17070355953729882,-0.1609367309611754,-0.2401305942085431,0.043913329992909256,-0.971624089193301,0.0751223643112609,0.06822101806256872,-0.4460839435717325,0.08469273802370086,0.1139345636305456,2.4427853311787913,-1.0435366368170764,1.5793760406086776,0.01132237389898294,-0.061142583046055715,1.2452786963079583,-1.163299705173275,-0.15046872230716854,0.767527666914269,2.4440481002492893,-2.83378604413649,-0.30789902430987803,-1.2568835942252756,0.7251173022832393,-0.7534387073439004,-1.321794567054091,1.4075713891857238,0.1507725002476854,-0.3900775891888231,0.8401902299743562],[0.0,0.0,0.0,-0.4201293733650396,0.43886392816528325,-0.5629527495378395,-0.5603337969273521,-0.3250719862712815,1.328681733028279,1.8518369656548221,1.0802418306302366,-0.4180717827449538,0.2145565700996665,-0.0898960782289367,-0.6728060300089838,-1.8792152600428742,0.223204106182613,0.3531369845462303,-0.018228588606277074,-0.1706439002681739,0.5082441877996016,2.9542944082016147,-2.042376758511538,1.8433713362293338,0.7234419514669344,0.36263652031644256,0.7403735658633077,0.4790819765668779,-0.39093433365426034,-2.555914917252824,-0.1959440666246871,2.972122867638631,-0.24654396107180107,-0.6720424250119743],[0.0,0.0,0.0,0.8484136421792878,0.45456057674802797,0.028640048677867777,-0.26971970903570447,-0.08139899165376718,-2.670723227762906,2.1852281139120024,-0.07864827318798909,0.29439916759907436,0.11222205976601174,0.3820562618320829,-1.158293491922788,-2.736947952422575,0.9990469103604277,-0.8671942137980524,0.26068488889439967,1.0035461711943778,-0.17259551611754498,0.6588543024516189,0.23713549470244688,2.3458123669457915,2.185337497712456,-0.08756030089521705,1.049511725657037,0.11576784852160715,-1.5313346705291715,-1.2659110253722898,0.5438037453283784,-2.356949879518992,0.33795737084651434,0.3771294136250509],[0.0,0.0,0.0,-0.44082502016834413,-0.8637198131169175,0.16445855606803267,0.18059559617796336,0.04466973390924911,1.2940719558796183,-0.1725172952832918,0.4597911630080983,-0.16652351971987656,-0.15563927260714389,0.18128435603178278,-2.333181053193693,0.25820733130121765,1.575909694494455,-0.22584462295864272,-0.2186598078110915,-1.7229064702129364,0.7132839241742217,1.6721453628813232,-0.9726200135805934,-0.4673673341704629,-1.8649997319342486,0.43868444520806826,-0.14613477805809305,-1.5963004025260323,-1.9690563430938817,-1.4871245179439696,-0.18799229677933904,-2.407547263861679,0.23905065381128376,0.12204483511603059],[0.0,0.0,0.0,0.6713447896952839,-0.4416167150190741,-0.595141917663985,-0.008772012539419838,0.37651865504248666,1.5884905007691477,1.9487662868009028,0.9486424106284063,-0.6313548203517005,-0.19539753280056565,0.03496880005497238,1.4000359506501021,-1.8710384221681708,0.16102579516894744,-0.07985492870247624,-0.09961836433094326,-1.061953802734136,-1.751798600679261,0.8124089877934184,-0.44511911385847425,2.8273411298307787,-0.4560024054878853,-0.31042955269072225,0.01206151998224203,1.408313782841884,-2.1843785206116957,-1.838398741071168,1.086940524128303,1.1092709070833058,-0.26450100898538936,-0.8538694381664571],[0.0,0.0,0.0,-0.03649224496867651,0.6258521339597931,0.2088597666442519,-0.7505697971224448,-0.41938749712112305,0.9603069821758754,-0.07816524550315895,1.331433323140896,-0.7659933546297171,-0.049919328708884964,-0.41499982236275973,-0.6565943797071925,-1.2769906461115934,-0.04159321965834985,-0.5101186727800698,-0.20719752669755762,-0.7010013568337103,-2.553746898516334,0.492406028550403,1.7838797124133423,1.661612782835119,0.6699433679503688,0.05048113084783146,-0.2915876486846084,-1.0276246204879387,-1.6469976420206835,0.41125209523664763,-0.3537765803777815,-2.5850426121543637,-0.44514872472447153,-0.16444163850708726],[0.0,0.0,0.0,0.8256068845345693,0.49689371238078717,0.0770498045756366,0.2559946062330573,0.36919823244768996,-1.8161465053232235,-0.0088261501031629,1.4286322854072728,0.3503392093359007,-0.08951776787778781,-0.1919143966588155,1.8544596856714297,-2.234180207534127,2.147277776909548,-0.17506809347735064,-0.030729306285313923,0.5039631458565994,-0.613385944272518,1.0451575467998415,2.088385613066677,0.05023241641381393,1.252178220951322,0.44911321074455823,0.15784868805951868,-0.3774755200379407,0.08787407257508262,-2.4364942600968975,0.43249690673354513,0.7249541159337731,-0.08462047869265726,-1.195517788081012],[0.0,0.0,0.0,-0.6522094562616996,-0.6587929405973307,-0.2554423449659884,0.2745248531193797,-0.014853663946441553,2.3684028386178677,0.051069354145372614,0.25503801526697684,0.3835567565690966,-0.10218784746795243,0.3715053939605814,-0.5819060422867657,-2.546840326663928,0.5056381595137407,-0.6651956715369656,0.16246596936014773,-0.455210921626948,0.34442399601892193,2.003994567636644,2.0250545632967585,-0.35355361901651006,-1.926513917747858,0.15942937439180505,-0.9532072181406179,1.4322013129467472,-2.432685528206835,0.7311869867117378,0.24608662300192008,-1.5559848210445002,-0.38936718430260053,1.1521555836645105],[0.0,0.0,0.0,-0.2124690992483238,-0.7127428966197249,0.6277011105051669,0.22988205900702596,0.3170255119386823,-1.9180195889082468,3.0931937295941156,0.7277352149689604,0.24953589534381393,-0.005868625847204489,-0.3153061903246797,-2.63498150831663,-2.6369488951579347,1.703555869418289,0.12081608405693667,0.15359001782277737,-0.6916402092124956,-0.8788363461512896,0.788131051774108,0.02580099802959257,2.223563368350212,-1.655711372693832,-0.4257511179183749,-0.6255146109137089,-0.7275956865392139,-0.12075371002590707,-0.8116300533698015,1.9845980025221055,-2.2253624997400676,0.142028439289555,-0.8009101837403749],[0.0,0.0,0.0,-0.753644705580222,0.1398631141336708,-0.5615512550777944,-0.3116378587018692,0.18595925452786105,1.855737697151676,0.9080648262522097,0.587029026367775,0.21963918029400709,-0.24954391634616624,0.12962711873547844,-1.0370694119088313,-0.8840700469809448,-0.01527146888700387,0.08437652583299993,0.16753307208249285,-0.5016405531019761,-0.20483792519092914,0.9765238096856785,-2.0370935241958223,2.3122832365039283,1.4087011520062882,-0.06507698006096524,-0.9398899940884617,-2.991842445666708,-1.2281142972575605,-2.1258628693390405,2.767109001761619,1.96763021113615,0.2357196072623029,0.3407910830570744],[0.0,0.0,0.0,-0.27709252405363793,-0.39766804592425764,-0.7687615920171089,0.4172355126348748,0.022852615334948312,-1.1283344778250397,1.7224102811642543,0.23522898650085938,-0.45799687770279457,-0.21116117015906244,-0.25429886123561085,-0.7713743087299698,-0.36958873781622703,1.8554188598173056,-0.6811450012352143,0.11524532254477943,-1.0612366903456008,-2.8452545878145807,2.2380610574444644,-0.30718767468940955,1.0118564949723101,1.2676409943424067,-0.009855932554584779,-0.43563855230544446,0.08718927826089695,-2.611392384721352,1.0585810321946063,0.2684581088127933,1.4373413190328108,-0.4553132670865099,0.09367159551319348],[0.0,0.0,0.0,-0.2834464080996073,-0.06769932060615687,0.9548206026222358,-0.05824562243642328,0.21576547506593285,0.046330153773920024,2.5112699955015825,0.033970460600811325,0.35002051160743364,0.13349827758322408,-0.025777168946689355,-0.3077114846712501,-0.5948145673103684,0.12922564390102592,0.3512531161382467,-0.19894752382363967,-1.3928010513989277,-2.5465973517436744,2.531464273200042,1.3074272992045946,3.0413233676454174,-0.5097512613889275,0.25560262379551835,0.022174024880984078,0.5654804035391758,-0.3096303818960333,2.1086384822111306,2.6178562375059213,-1.2213114344413578,0.17990622525846067,-0.16503751995494742],[0.0,0.0,0.0,0.4496918462392373,-0.7563688812337992,0.19999813498933638,0.43091078534641564,0.06132803499599099,0.9895184501504279,-0.3882133383958233,1.6951998344321115,0.08437566181066036,0.049421661383512006,-0.41403526584969236,1.9297375796878886,-0.3816855480959904,0.273681297295772,-0.10512327744858163,0.21454877313013476,0.025622791883937346,-2.431008814311854,1.4266632910442332,-1.9618780474604443,0.9588459854199982,0.693513644039399,-0.2870053273482065,0.2388150330576555,0.552152723422227,-3.3638971178644104,-0.9436256937142145,-0.8874042684233887,2.2350554038856973,0.12904096845730395,0.7409566828465075],[0.0,0.0,0.0,0.3817741006450792,0.6607342095955069,-0.6279184162606284,0.15296176930065283,0.3820054387539273,-1.7661518166255536,2.7597382904338033,2.0111431718486985,-0.22103053217168156,-0.09782314739089318,-0.15564357889458652,2.296635595878635,-2.734543958122301,1.3730338100487023,0.23673764657913476,0.0016751591579155511,-1.5272105318901352,1.2440563745245918,1.1125258056616032,-1.1512259531809057,0.8396339889552942,-1.6832745704192937,-0.2201291384404004,-0.3992649847634251,1.4966259853760424,-1.089538078310194,-1.0506265994642026,-0.9299310089921488,-1.1759185401837637,0.25765895585488985,0.057910374554025035],[0.0,0.0,0.0,-0.221501461368856,0.7701016374016258,0.3658007498270513,-0.47336073148156593,0.1833304370929904,-0.8481695393442648,1.090698438874884,0.7373462504680851,0.3076282221586618,-0.11863939632840564,-0.19033170170675745,-1.2030859875180442,-2.642237259288126,1.6855452760980896,-0.8609912662460829,-0.09764621282990144,-1.2432046689975367,-2.210751618372415,1.9766858772762679,0.44186779987572455,1.7570728890190963,1.831880388174179,0.36726213228042015,0.8731886933068362,-0.5509602106212559,-1.988792344076842,-1.9840030445497858,-0.4468486627143332,0.6159791667291299,-0.06512159703293963,-0.6443252418560615],[0.0,0.0,0.0,0.12010747204921363,-0.7251432841857401,0.056034802312305565,0.6757229561647929,-0.27779627745627106,-2.7296749416790185,0.486081026264443,1.125318921773609,0.1396441246433543,-0.21252520677417575,-0.3977776153440632,-3.0593666800713435,-0.8878043161361897,1.0608105822803884,-0.7613218873606484,0.019744010091857334,2.0832113919356536,0.6821924342965424,0.8912937332981112,-2.5635303916366023,1.2261680644066992,1.9915669552679578,-0.3367521324406133,-1.0047841604329244,-1.8631579502130202,-1.3221467265896552,2.097921739091948,-0.8746969730981659,-1.1402385519292468,-0.2936368062675605,0.3480703806216714],[0.0,0.0,0.0,-0.800403124718403,-0.2383611356900091,-0.24955340725250974,0.49016518018138383,-0.378073064313788,-2.0147800219794023,2.195142770285732,0.4336785326158502,-0.052151712151849994,0.07001355564918521,0.2816934546133239,-0.7777575445584937,-2.333343033889948,1.890402388004273,-0.5048399604481634,0.10073230615797191,-1.5698419280709048,0.23698200265082292,2.931213525676738,0.9065299508481659,3.13497648642781,1.9864096683041241,-0.041042719709911524,-0.10322761385152535,1.4757737803232724,-0.7476617798872947,0.23989309217844745,3.0030642055696193,2.7718383726351066,-0.2107285895823232,-1.2331765245629962],[0.0,0.0,0.0,-0.19648482529798358,0.9243668615999602,0.2832671708490609,-0.16339929168337788,0.37499131460045626,-2.1980142261420643,1.1166526942197548,0.05586942173450957,-0.5783272505363966,-0.11664884002464648,-0.1096516931355006,-2.3867761452785903,-0.29349506825493776,1.06991877386873,-0.7909746289978192,-0.18819543414758405,0.41090419875619233,0.9988385102061339,2.581966849711,-0.8643285018277131,0.512661833575772,1.0493080608318426,-0.34610573696456787,-1.0683092058633947,-0.34474155873725953,-3.3757358711026746,0.6810178904233641,0.4001100926497594,2.555717438994627,0.30012528494399704,-1.2539472807177148],[0.0,0.0,0.0,0.7067380081984942,0.04791724232198186,0.6579950382939215,-0.25547574295118985,-0.25516763362141615,-1.4114211404244437,3.123340807945114,1.8457889633923164,-0.7296247114771139,0.11769638833181367,0.017589009767823793,0.10742256528482841,-1.9149438183120389,0.5797996662091569,-0.881980369346081,-0.19631156273317776,0.9558099565898028,-0.7374839947460035,3.3512890859496998,-2.329206532093402,1.646523064252097,-0.35714539592030414,-0.1868889063219012,-0.26066928475550566,0.7789790946889559,-0.11429817342837678,-0.7145881102970559,1.523995697821826,0.5035438165736625,0.09727232688027121,0.4398331391014194],[0.0,0.0,0.0,-0.6836548949918703,-0.11716227871048819,0.677466992677501,0.24480085545913413,-0.11494795193878143,-2.2908869484121572,2.73155208764111,1.1482116391858366,-0.3533317689294877,-0.1189219205828215,0.3987291052569146,-2.049985954839352,-2.499070702324954,0.7510983401595787,-0.2505894623204987,0.13059192924341317,0.30252432945921326,-1.683026668262797,1.1227016049721308,-1.1576196618377592,-0.03411377718672115,-1.4288331718303573,-0.09019519181879804,-0.5098846644505663,-2.6111405352600836,-0.7091874832408158,0.4438337013588258,-0.8525254944169527,-0.17956602034478308,-0.07290038559528644,0.30244965666206935],[0.0,0.0,0.0,0.5127564356483932,0.250750464911791,0.21318540263131774,-0.7929420068028928,0.3170653087882918,2.152813628415434,0.6294856788780976,0.5361899053336603,-0.8032259232672145,-0.21882371118261362,-0.32165580262848603,-2.0377088847094735,0.2167598786624847,1.4592049414030814,-0.7641297013241779,0.023436087394391902,0.8441225998033275,0.7662885520176062,3.115531411812629,-1.392404761679002,0.25599522316597545,-1.2742872019565916,-0.16634982239513363,-0.6484182653880263,-0.6368167536305434,-1.1013470631706004,-2.7828708635263193,-0.6632305426026455,2.946658189613138,-0.3146920468555787,-0.39498806622570504],[0.0,0.0,0.0,-0.5027405435547679,-0.04573804190164922,0.7210011469905971,0.4746760194358917,0.37430068854975734,0.9523165774209699,1.7066261600914714,-0.11065289954396028,0.07400431627044068,0.14963467129556296,-0.2180671443060321,0.7553911045241359,-1.8645978754391108,0.30849495300596796,-0.24197472579048163,0.03374697118183745,0.17603540331509304,-2.2724195076609206,0.914000581418007,2.5322405079639454,1.6309009928728757,0.7970431539943799,-0.0022991530656205628,1.0020810113191323,1.353989087465981,-2.3671667450687286,-1.525714749869507,1.6960523658674187,-1.7472279990613824,0.22641732774155676,-0.5033435428071519],[0.0,0.0,0.0,-0.360437719782087,0.7021740888567161,-0.5486277228194527,0.27576044105431835,-0.1255088989348459,2.17328441497123,2.2588614805028966,0.08785979553574458,0.14049372389736636,0.23807136036630067,0.04513911030378487,1.6303000063705295,-2.7518712182921017,1.1063968645438451,0.19172086487909434,0.1293795058126721,0.20882587377555284,-3.1194705186016383,1.2514959314461458,2.3156100621736693,2.108195005517267,1.8879551451722527,-0.32327929432220015,0.8868926327735358,1.4780182536087119,-3.210562917228347,-0.7273216053203893,0.9510903141709581,-1.8598258173356141,-0.3422238204386266,-0.8918640514180206],[0.0,0.0,0.0,0.022898675419910336,0.5837155482818568,0.6463693164769432,0.4908752551057691,0.42362952821853955,1.8499305366540741,0.12773670529869458,1.7336542790508624,0.3146003511520251,0.05174699965709739,-0.3859383757112144,-0.3207714424794976,-0.3308607515567399,0.1392679862708202,-0.0018536917509610618,-0.23354713693025045,1.0123505623743432,1.052575289463419,0.5309809392664695,-1.7686414917408513,1.4945013185868883,2.116743541552884,0.21071616878287902,-0.7901975423194074,-0.5059696838434111,-3.015199664527651,-1.7195437862414957,0.15393605332133942,0.08796710978718458,-0.18017189810629508,1.0704406194726173],[0.0,0.0,0.0,0.3825061747076842,-0.05235939385645391,0.8793157451559339,0.27883927360819183,-0.22867126060404153,2.3455900223786377,0.6364208029264173,0.6578680485549352,-0.6552434182182261,-0.06643045165968056,0.4295623007038966,-0.5189956938201039,-1.122199060965662,0.7220389781060987,-0.33395856155033543,0.1644327539533652,0.8604875101109806,-0.0374667489934386,1.2597089054282893,-1.4380630387264248,2.1786034728290193,-0.49577315119234866,0.2684120174401766,-0.07646718834717636,-2.4492639517636157,-2.1300719474956606,-0.15503817730466363,-0.44792346025597607,-1.2148664214959353,-0.15195129926613193,-0.5115046152552571],[0.0,0.0,0.0,0.6256454637506372,0.5229012935654459,-0.4945483222214325,-0.3009384453070093,0.2525424966046255,1.6592162916091264,1.0098141984432958,0.5319666036587657,0.10668337181415855,-0.06222783807678109,-0.32829505867880887,-2.608437076560054,-1.9557860964054077,0.7790887505880966,-0.45827357773944055,-0.13365518029822848,0.44261983830736185,0.2768337502663685,0.6596757143540064,-1.9875987664047892,0.440117946426976,1.6519834635804305,-0.3475929508960942,0.7330417476361024,0.0837038962938994,-1.6595769671999998,-0.5420994012628584,2.119977061668913,-2.01927183575052,-0.2154215445282695,0.29825283733081354],[0.0,0.0,0.0,-0.7929172894521279,-0.10499279125491906,-0.236555948795086,0.5516339084592679,-0.09628184426601066,-1.9178345507223702,0.8705801265834978,1.5937237516983425,-0.36638462448604303,0.07909280597610552,-0.1586970217802513,0.23257988526168516,-2.8671229096705324,0.9679472730743129,0.0806361955005116,-0.07086008450379647,-1.9629564392390881,1.4197739936969458,2.334875293760015,1.8270247960315436,2.461979406673078,-1.3191407187476512,-0.31582342478551345,-0.7923987546765237,-1.2048822936132355,-1.553795166471886,-1.1765249396902269,0.3271994262179425,2.1994267445597435,0.16859365201143994,0.6544267181392434]], -"gravity": [[-5.898843138251666,29.196533757855274,13.05069337026934,5.946317202318365,0.3188238508561437,0.054247440199193975,-3.9428614764942087,15.823468000951875,-16.574285944614537,-5.948943529122866,-0.31157271249144153,-0.06062863117850658,-4.702762822300581,-8.69378521929531,-5.169517091832044,4.226154891666852,1.0682438203129878,-0.11165417684270418,-0.35600166297707814,-0.5006525978181143,-1.733248326550422,-6.441487287713314,-3.968209805014072,-0.14484321627460683,0.18257403482898296,-0.3024463114663834,0.06302147976805825],[-11.955876357949762,-23.266033732504848,22.57536406944069,3.6623039834044664,0.2007770229549424,0.08298188501112394,30.880192714319293,15.292439384520405,-8.691692979137443,8.125803689289102,0.33032746119960593,-0.03421264805407323,-4.335751837683479,2.9953796751838246,12.019539146259827,1.9041808383169854,-2.531213281055531,0.10954636268660131,0.791792636437227,0.0470552938112419,-3.072114575285718,9.671538456162107,-2.743208050627453,3.2216211986499927,0.43762667400432276,0.6542996939691356,0.059160894582731674],[-10.777749548593423,-17.323423907557668,18.0633261038943,-8.26860930932472,0.03392368594339939,0.0668500145208553,2.5762952614022607,24.935521224659226,-21.751429599880865,-5.121988472610537,-0.25651685538127506,-0.08286269816609962,3.582679248203874,7.31672019251712,3.8355723274986766,-1.2575000666983713,3.5627260503064893,-0.0034866375300417994,-0.1218623920828796,-0.23538635933560656,-8.965375468241355,-1.4746894259254058,-0.1989857281276563,-3.8671766784624815,0.19200525064782745,0.1335421542395088,0.5089620807230915],[-19.84245204817435,33.54916403303191,-9.621935467625356,-9.355886617894532,-0.040120455889944034,-0.00880053142982607,14.556145726184631,-2.7262512666664462,-6.215296776718512,8.449190235220561,0.07956757416326352,-0.010186312076157243,-10.60859754820639,9.54769810473448,11.601531259911376,0.3117135634558453,-4.173069791848112,0.07615118252239358,0.4281121567250773,-0.4030580348687771,8.513712209186236,12.525115606416298,-1.5060979203056242,-1.9980521030141003,-0.41678073329616067,-0.6708036031506852,0.2147861907839606],[-6.1873083746929955,-10.9012976642951,-19.740389325686223,3.52338154326182,0.1823458212303655,-0.10455303541809238,-0.6370526077044583,15.1997735587872,-20.436362397546034,-1.3272569026707752,-0.1588197112483366,-0.08706117380804157,0.33079753088025576,-5.626965213032428,1.4774211246988975,2.9081797507018243,-1.3473328861206382,-0.3622971068189485,-0.4878564330765113,-0.023224200318673636,10.395846549671127,-7.210180495972191,2.0023332109799914,2.8563418924206587,0.010543920678934268,0.348459351710521,-0.5042643567578056],[-28.924736472936974,-3.048563321687723,4.461571863107121,-2.4666558491161776,-0.283254087801143,0.01985155704669857,38.60263661567616,-17.948753682883353,7.335874195978009,7.255313307846747,0.3358764901430137,0.030148151689422007,-18.27829737700198,-6.169807112495771,-0.259389950955871,-1.2550193193773564,4.092301614718812,0.2709615235663065,0.6444658550071091,-0.10619296451437826,-5.267275077108643,-8.181796570355553,-3.987360382819416,-1.3040108923554081,0.4566631053002843,0.4696679954555285,-0.35200876862197505],[-33.768105823150044,-13.153391153756424,31.893511113795537,-4.4882405975700275,-0.16664273733169696,0.10390774419412226,1.9523932410967801,-23.21802197712812,6.349304618282059,-1.979388129305074,-0.21302204119392945,0.03334931986899886,6.764250948258342,-2.409632201339987,1.0078712546362227,-1.5316548218415422,2.035879417869046,0.20409163367546568,-0.2959323050785312,-0.4040663227491187,2.532465110354366,8.304325753077574,-2.5036436828323354,-1.7717219576968326,-0.4377787650041314,-0.4109820618245498,0.06927973880613124],[7.810010215425301,-7.375240059285084,-7.187268785759133,-5.205956811305379,0.2030974932987447,-0.041668035826475755,22.397133420910055,-16.02752424208929,27.566632085713472,-2.969594031190905,-0.17242171357067426,0.10389156720329126,16.153881567555608,-1.9862073911676865,-12.162756039358822,0.5347777881566036,-1.9878931111463523,-0.1489660780528508,0.05235015184365016,0.5263487906215447,-5.022185802421286,6.353317607140966,-0.9068817079943963,-2.5921535149943375,0.25380688006298435,-0.5820298445234556,0.16014419464622828],[-34.439250910653904,1.9646337203070339,-29.392950356725777,1.0145718913530533,0.1959253508093556,-0.10296371934644323,29.661650576367155,12.814047199884152,-32.67645153141318,-4.12688505230383,-0.08745834640995402,-0.09303505169361452,0.8207691238744488,0.2538929603678448,4.520507411802219,-0.08162815446627647,-2.894108860407242,-0.08715545233875291,-0.444357765279109,-0.452714869109091,0.8876537625023695,-7.731024431130927,1.3767281371622533,-0.3831661763990343,-0.007608061357936352,-0.3932143773404042,-0.4447551367381399],[26.444647399300074,22.163305583229622,19.97435517754556,-6.998056600295992,-0.2422412428460867,0.06301021699601236,2.943388097131729,3.5832176779622444,-7.055469768812125,-8.305732360900137,-0.23080123511162168,-0.02863172611079347,-2.2528694760290637,-15.45589229837205,-3.3203319311127313,-1.154289053775693,0.31009960377297213,0.11391388162737263,0.6203382651746292,-0.26377456106029523,-3.5355460608561633,-3.8042101499933194,0.4050797728975004,-4.309821014989047,0.053784569348347924,-0.1400198471105299,0.4529145277439677],[-25.401629612549986,5.405136560989402,10.330318507214082,-3.110983239466986,0.24191220531955232,0.030289589528680504,13.463643372481688,-28.791588208441947,-5.89203618207275,-9.89087036498336,-0.3149541259140657,-0.021500589041027193,-4.510172741047748,-9.327408461642149,-9.565166490133088,-4.006252738690015,-0.38566023042834374,-0.37042383808487467,-0.3077625367956173,-0.40955593024512726,2.0892119546350902,-5.818990442684438,0.7957143828551114,1.4869890314235377,0.01962690071267328,0.028103186063723556,0.18660355506355442],[25.557140521053153,6.529649769417793,25.16818173817515,1.1435807926369022,0.032041354011812816,0.11147415003443727,-13.410548565715931,8.742327720440503,-23.55023043036379,-1.067505663992048,0.051657294479397,-0.10713364151723974,0.9741083459463322,-4.183810920974562,-5.075605973800475,-1.1442881083355958,3.6958792647598386,-0.4162385202713078,0.2769696844544279,-0.5715858593337307,-7.309498108864433,-3.8680340174912327,-0.07260370773635597,3.49782852937492,-0.1576839078492691,-0.21788341546481027,-0.5529753099248871],[-9.719195125649833,13.664865490488692,7.282476280344746,-5.554387785631286,-0.013542168612876448,0.048922082087502185,27.18622286890994,8.110109381341092,-11.310768861664016,5.892770271479785,-0.023150153384237343,-0.04196884039479062,-3.000797458058429,-1.2318968971418278,0.1564124244375729,0.11329612797797904,-4.118042414560374,-0.05712046618138461,-0.22388296874418073,0.598741555894357,5.8702159372021265,2.154508998547561,2.3639400483533293,3.3903292779392857,-0.05184422018184695,-0.835023664452194,0.02553474142776331],[2.911195790451288,-18.822682365895638,9.43421273578891,6.36073627529462,0.3088851345531455,0.02344460403787635,-14.100877227412294,0.30164313157285877,-12.879026050670875,3.3235896759057346,0.29679562007736787,-0.0468172026199455,4.687680202284506,-1.6671703086571725,-8.149302897678492,-1.9186246118455352,-4.141658195693409,-0.119132045474881,0.1918219595575046,0.608783703101551,-2.77806353009044,4.837466015985385,1.1737494114036486,-4.208011847533186,-0.13731593097260153,-0.22699217002143754,0.390605199523837],[-1.2073605116821744,36.296200486490285,3.672908095212554,9.303728552683143,0.2821294212795544,0.010294541258309252,20.239406623410055,12.819066262432193,-10.096474254909538,-2.2759097829306287,-0.3253198365877421,-0.03148725524250435,-8.017584985659841,3.053487926966106,-8.35397110004329,1.3605151525120245,2.113480683724806,-0.07298670197310216,0.6912112617009426,-0.04214783400256758,12.081941739205607,-6.75786686918739,-0.23839105717984313,3.643860101614492,0.21020341020626654,0.5175072105441779,0.2085688175854074],[-28.34559588564473,-6.10257400235702,30.311465797047433,-0.195691996039847,-0.09968090072608736,0.1157264765944647,-0.9517488605932167,-6.775322932168165,3.2615097890133544,9.555464795452508,0.18196021911422688,0.021544007742762706,3.939871525515562,-3.147259172357958,-11.247510321443567,3.5600251509692558,2.8104520984098205,-0.2565686991872042,0.7429216530186693,-0.1775532584321153,2.9347363823784898,12.522033758333317,3.121144001778113,1.568218595034856,0.005860227415943662,-0.6093962116652406,0.07884469844934895],[42.6958747710255,3.2311953323016156,23.493057785921195,-4.886585030854116,-0.24097805961300123,0.09350674048804017,-24.273432025069276,13.518499473794112,-7.40773672161435,-5.486794695955441,0.14659010771113898,-0.043682077271277334,-11.638105144896318,2.4076517907886625,-3.130088479472517,1.4952983839768719,-4.074379516893336,-0.02283196732841271,-0.7228532761609455,-0.16170777978042059,7.7166513360073905,-8.563945916352486,1.5845517960383853,-3.986661466828887,-0.354235935703534,0.08740671241667186,-0.4606503058328921],[3.1057531982808704,-9.132605268081576,14.495636709089824,8.509059387830801,0.2650197106957117,0.05091990454415349,-7.512841230007995,-2.3431408658089787,9.95730978021922,-6.711083513077534,-0.29615507963514287,0.025615067988169662,-9.244072164432556,9.647104357682677,-8.099314766026694,-0.2627754074950224,1.652539329042666,0.14802783671926714,0.13667791783037944,0.34715058628175605,-1.6323667777981368,14.910591172428065,-0.248446297009128,1.856675507630701,-0.5290467396841106,-0.5693811729845684,0.14255019660996734],[31.51345473820595,14.016044467740468,-13.389911427175534,9.199722936427456,0.22243331301444785,-0.041147865856609664,-20.143415295259803,22.13536890667218,1.5444995328955882,-1.5529050473752437,-0.34559845562465025,-0.009761910147299695,13.280880627558245,-1.0058586568994852,-11.568929889225414,-0.06701819980833881,2.45911096949854,-0.057967024730448936,-0.19986113228955765,-0.5974672602178995,-4.355439182756261,-14.919326286884441,-1.4526479435686392,1.9547315315608402,-0.2198835412494926,-0.47109958116292333,0.5141927490149311],[-5.707763285151316,-15.593210255277468,6.253128474247948,-8.369302098449428,-0.1321991574912932,0.028262147623046855,-5.6508254904537605,-1.6654175040442,33.34797081683621,-1.5364934320600232,3.5252603825783584e-05,0.10950138108079172,-7.5461099683743456,-3.062126142372832,-12.150107832157127,3.674291239391706,-0.7698718723757936,-0.14794452382880063,-0.04163109432712888,0.2560929196365885,9.012478751364512,12.515326473280602,1.3026212842553,3.659053371449506,-0.36320921571161224,-0.7052882457830809,0.11594874348136613],[23.046325653972236,9.633817130555434,2.974256597611986,-4.90244071061859,-0.08694956730637857,0.010642685931337022,-42.7826848229332,-4.600954100079058,35.896235159045645,-1.6952126832515284,0.002086507731352008,0.10199295323765233,-1.844804681117175,-7.9195261095293406,12.489424439161883,-1.6059750637629584,2.8246055124738763,0.1562996825307232,-0.2527060666362589,0.43770581437142675,4.23614923851973,9.796319939749601,0.08250867271072237,-2.7999635917366197,-0.050609355030443615,0.23914139533888817,-0.004066743679423817],[36.01512334357693,11.57517889325314,-29.260360425898774,3.577987982942134,0.2174299623291678,-0.09564434185145736,-20.935606624782135,-14.730625449768333,-27.574902002723302,-1.982686980333145,-0.14360636702639132,-0.08221117157404743,3.8288085406820223,10.925996941355576,-7.418377437325647,-0.9197240006039756,-1.9915194160406084,0.34174587985587473,-0.5700238565674539,0.09249078423294144,-7.461435763460223,12.92066596963181,-0.21274700835531085,2.4139364543408495,0.33584362569853976,-0.7327470127785625,-0.09867932936655074],[-8.511880953593725,33.17210335279292,-2.9249080382346815,-9.679518744141173,-0.18487821876085514,-0.015334064735462066,4.971336265814393,-14.70647805818545,-6.157220395799335,1.9518375757468658,0.008557423211200088,-0.04124607729514856,4.771357906629104,-8.442524172211897,3.361054633445491,0.09852166468484974,-1.8231224945590399,-0.014543302914372506,-0.010594518519047667,-0.45448377266253337,-10.815798122034419,-0.5369972979054625,-4.0403061719524365,-0.321664150051731,-0.27076334002851554,0.7281108908597991,-0.1510317244941731],[12.700637760320438,20.472431354298674,17.08075398438402,5.173048268003087,0.30969367758396693,0.047461833552991654,8.215504587385666,3.4857833294046032,-12.666227381089865,8.178398029056511,0.1613425600257944,-0.05644487879153072,8.25025945390564,4.252711850794238,2.654299190291099,-1.0749511040171016,-2.0329073002537124,0.2502057928796324,0.205722069530212,0.2843713751128405,-13.028620542518695,4.279119927067739,0.9488250417818294,0.19527103908580543,-0.30733007080085545,0.14221595115734276,0.3846195388730332],[-35.08528425615875,7.912513336899499,27.7718759332935,-4.45438925486127,0.013519652978920566,0.08502020023785241,42.276616977885865,9.275228197384656,13.540361300595197,-8.602136967608292,-0.32472057851585734,0.04045190203742045,2.664250970790251,9.494774395054408,1.039616215071597,-0.46288700380076825,0.8365040893801411,-0.1543531503205439,0.1592310951844217,-0.24648234672787953,-5.332089292054008,-2.225645042601994,2.103655710699663,-3.3205854350641872,0.3912391165787303,0.5179325505687417,-0.38009041914491853],[-8.698589562621171,-15.143093486545583,27.772486126109442,6.794618764952616,0.21616179566726346,0.0831749436388298,18.36099266857186,-26.041292231420304,21.600697305659697,5.740152204368979,0.18126907289278654,0.07609262360544741,-1.5427593750040263,3.0007338225351847,-12.579214321857723,1.924780539740998,0.6745030338338643,0.3684194312397155,-0.3558051856841029,-0.25663713541136307,0.9384736807923307,13.838582384276098,0.31644309224425937,4.019269905094748,0.2973957290379467,-0.42243250706307583,0.4481665680424666],[15.973320906448242,-32.58858307583155,-15.657220764415184,-8.727722009380257,-0.2649753561445574,-0.050946596891072414,-12.245262427283155,24.24788467495836,-8.48418952232592,6.582453173669809,0.32926355073725955,-0.02191280858299663,-2.074518477021588,-4.057984105927179,-3.6827444526509323,2.419168742140954,1.9386836849510338,0.0326563881841734,-0.7326875641117587,0.20509774929696498,-4.48147961420459,1.157264098026183,-3.7049460158811383,-2.525506290503819,-0.016502371272075128,0.8154273936094886,0.12485609966064044],[31.95520999562715,-0.6817536530009006,-26.943392285061574,-5.792111665426399,-0.03611372048153527,-0.08319484074607758,-29.539384408796057,0.5147182234658573,24.04311458568031,-0.47938690722429556,-0.19204606011248795,0.06430300542166177,-11.280705562578735,3.1533017811491155,2.032830978112828,-0.019662544323486984,4.335518589459888,-0.09675933732946215,0.6136964524545903,0.2707819824408851,0.9829266838529238,15.07255854765426,-0.4074409362116735,4.1706986889823305,0.3136944625131924,0.17392919028207807,-0.5808236317296632],[-24.188731401617144,-0.47287111545704275,18.229575686593392,6.492734279990737,-0.10883780938062218,0.06657718879834891,36.818683629751916,-7.699871461683234,17.13572786496856,-7.267228169134791,-0.2642855421100467,0.048886279741753115,16.80725127218333,6.863039376512758,12.681126523575449,-0.08978247654216506,-4.197752810942141,-0.06571286857145253,-0.46038000771349935,0.5128935581055799,-1.798125131507858,5.021026613634763,0.14790240707681823,-2.290303243468545,0.13250684228358425,0.5478702370438914,-0.10081716265572059],[15.342113019689641,26.353294769546668,14.934582965660761,2.815011777377018,0.17923508639214092,0.0684757622618258,1.3602535969969956,12.306291462798075,-6.87326313070552,7.54559372747892,0.036339370327363216,-0.03863414079541472,0.33970161947399946,0.5146696058658812,-8.062237569337773,2.8322130062944684,3.5918281326096118,-0.2797684789950991,0.6419244244738465,-0.37875232032226547,-0.23339866893438385,7.621969774068697,-1.6154493312543596,3.545961869051226,0.2550235856043007,0.17606961424955533,-0.09540459690434956],[-6.637246208750642,2.280169317805548,1.07379446706738,2.382716698796585,-0.012217482502138333,-0.004970998754480931,24.256739643135653,-0.05063833661923811,28.041248175801407,1.6038024517635732,-0.19628945510834733,0.09914202139397478,4.586138377848247,3.227278729006337,-5.018687831163236,-2.1350799902928417,-1.4574305722967824,0.31194846224194944,0.42644963024773325,-0.37856133285957744,1.712943785586064,-9.159877274898545,-4.042180239902981,-0.37955954883510595,-0.5378858975661649,0.43793782939827114,0.175070166356655],[32.50982669557367,1.2826842842082091,34.614032655187884,-1.3697693273606852,-0.01922965826972807,0.11410957158872932,10.312989375359896,-9.451441894826207,22.564874587904768,-4.12549639725387,-0.0664664973841554,0.09358844468020384,-20.733582459900425,-7.666314489840612,-12.977460782140666,1.9114714467392582,-1.7954375944709664,0.4407003370579049,0.5272367395074047,-0.39029446128554107,-5.405724736292819,-12.420642938627715,0.28802984924315034,-4.030068574378924,0.31478199354318903,0.48816250490860635,-0.09895746129395212],[-38.593606105485854,7.756042199713621,30.963125121591716,3.598232232812263,0.05931982560135149,0.08970191137772063,40.17026702546014,-7.986463256894579,31.303324442615097,-3.2471220104546763,-0.011585235259811444,0.08798580770665786,-3.3582833490168245,-4.573210362380138,0.13737354068420216,-1.4644406298711838,-1.6451131229492237,0.2004397964390847,0.24991379452615958,0.5349326863815587,7.225054071782306,8.619471208240393,3.3208307920460047,2.440363842289524,0.15420465736740255,-0.7348856874751664,-0.03463605972301117],[-36.966918318153695,3.2904917591641545,-19.192107364346064,-8.357543327293536,-0.09718026807490697,-0.047671423370447066,16.500788592768522,-20.368463904211843,13.913109658668972,3.3343400117001907,-0.22865308931677922,0.0742826627579908,-0.48229217467951324,0.8725085323799129,-9.484848178765109,-1.2849470558025289,0.4943227071463763,-0.14543479077495627,-0.40469885897181473,-0.0036165779233893114,-2.0764262420177695,-12.878653587251812,-2.442784736035038,1.316330917067444,0.132949109751029,-0.6210931406409943,-0.16796423442075595],[17.35102328834139,-17.87040034790906,17.48463057026437,5.813968660779255,0.26424540124263995,0.03968314254965274,-16.95050604653853,9.64300250044808,-23.446984184369303,3.541184284586879,0.04356991477944305,-0.08410737482645989,3.7349731311511207,1.2242395239981345,-7.631323389387289,2.659607051698341,3.423232909331715,-0.034805169823716044,0.7471331279947344,-0.10666518102376259,3.632404030175969,-12.17518419873412,-2.8024386615801005,3.1080655800123878,0.09325724195410173,-0.37096957227788896,0.5595949995062096],[0.8559997071590388,-18.681242649151255,-3.3439504750317894,4.731536115573065,-0.023077853184590963,-0.009167129511849266,2.8612756656691953,-6.8742262374560195,2.5727567694437763,-0.13021889695950725,-0.2404479250538153,0.0313305635777689,-11.675433350478738,8.521753647442072,-2.3413903304781747,-2.4061607215142726,-1.9680674592667733,0.2006420447203125,-0.6362794114189703,-0.3080632667861536,7.484479485717067,-1.6349442035832003,2.103678020932902,2.557050125987692,-0.1592653103466646,0.07141045476259539,-0.6072801698138465],[24.01604060458632,8.598480205761307,17.458010339073493,-6.2683230688498135,-0.22061543141351722,0.08181676648267501,-33.500485941355386,10.536650936521442,25.98580661351278,-1.3889233008733792,-0.11191076214788571,0.10160724261535423,8.476783814538349,-3.4441097459660135,-2.6230356577592153,0.5687578594197298,-3.171462309200731,0.08854138809592363,-0.18583001922401918,-0.5746529143804249,-2.4041207395748496,-7.272639652426655,-0.12836564587261048,3.4884307915979456,0.15085958200025265,0.2084273696776917,0.4190869508750034],[-22.332136968001905,-8.899606106524613,-11.214024878819416,-7.918178844506216,-0.19734133143361224,-0.03477269651336577,-13.163730447463326,-7.48673208327962,-13.980933428852328,8.054686244592254,0.08605403289645831,-0.06706648035269666,-0.11074810474852548,0.10137703914075141,9.743772402907965,-0.22680990628030562,-1.3448557928322138,-0.26046782590526535,0.20063593187743606,-0.36227830666558325,-2.059869996763819,-6.566833458675445,-1.2128286289057781,-2.5089359384590715,-0.22747178366027487,-0.38593615994795516,0.32892247217538045],[-14.701385728246839,22.865184752955976,5.676136265404458,0.13478172229969396,0.17266000151644215,0.04349938648522078,34.453316971015525,18.807358837499326,9.506158913519759,7.507200377140274,0.2440808065559938,0.033444693346954116,4.123378124576995,5.5823974557558875,-12.911978583891736,0.29920111028865354,-3.670850948631449,0.3131243205395247,0.3799738300081956,0.016544358993688325,2.1052549679401014,-8.326919320235028,2.0954333378065653,-0.472060114860439,0.09819359578520456,-0.6306436711362177,0.18012636035844407],[-25.540216530133755,-17.554922133785958,-9.45907617234641,9.123881492488525,0.17490244091100393,-0.02623094992635871,26.60765859194748,-5.0527314041346365,24.541632189782113,-1.7264685319219022,-0.05001743862301411,0.11416708875364534,-7.986616549576291,5.199019212942082,-7.275109733045227,4.330865016329317,0.45828686208487923,-0.08242571065189466,-0.7919804828274752,0.1959332160477532,-0.3922718944787939,3.3742082599870193,-1.1823682753201692,3.7506686166826744,-0.38163585559538277,-0.319746252329914,-0.29939723242453437],[-9.945399678432928,14.696153680797769,12.893390565231273,8.427020540194349,0.23053546389359914,0.03773338842001147,18.985532273216517,23.959183174589377,14.654944908338962,8.501525963362187,0.11709296226249559,0.04072755862454145,-2.5407616130150696,7.0399674354736375,-3.42662778955021,2.30569195114854,-1.880034205763187,0.09217833473448532,0.2784167439642217,0.5027548430056389,4.20665403726265,-6.351938163145974,-2.127731607110194,-2.186476995113519,0.21076934670743314,0.1360211350990939,-0.11639853047002605],[-8.034108337801133,-5.417958340720159,0.3583327202035634,9.721508792901814,0.10005344979069858,0.011297486991300676,41.64044796797331,11.668260720954336,0.36826634557093546,-9.496940716174453,-0.08966374923767731,0.015172795324184878,21.101246620160033,6.078378236826159,14.644676762436921,-0.00035415572627832403,4.250470001577224,0.17650660045099326,0.803405422250493,-0.02914778143987778,-6.3799783125549,-14.38156543515554,-0.28368461081473045,-4.006177111914159,0.2542710785549978,-0.751594922895019,0.09530266082247368],[27.033424169959584,21.6225713989368,25.004053001052874,-5.072721584043334,-0.20697202012060134,0.08716011666167878,-27.151848369718312,19.42321135502084,-15.2657269355247,-8.638924177978444,-0.12407489721400924,-0.055710079568276824,2.663807719157969,-4.125316041279555,3.428756783212056,2.016610891165731,-3.571124049823788,-0.131267904717171,-0.42510849056725775,-0.515169364175797,-8.5985109704774,-2.8102802787945995,-2.5459747476624797,0.7100993095633105,-0.023264292920761848,0.28558673728616935,-0.52794188661322],[8.395228197666793,15.135891409608378,-23.228316534698465,7.001479017595667,-0.0675785395336385,-0.09008321829984559,-12.765605461722483,-6.98726049305186,-19.445998956752216,-7.209598637973533,0.011246834893674493,-0.0780951321553419,-7.823655879378359,-5.277993916453126,11.190319992751496,-1.4398651312438138,1.3087638411922053,0.1367381283942318,0.19339922703823936,-0.24829961393603217,0.7429479578251074,-5.99027832410063,2.829294781984729,3.1754091842394896,0.052857912033445226,-0.44218432063939705,-0.47326761325846173],[37.06934865468625,-3.6591162794537677,-13.565862212288117,8.334189034753017,0.1642241120077822,-0.03528107645387485,-27.784978692595093,-7.715393572405558,7.738482994429275,5.239607891645742,0.33904926813772795,0.022322689428817613,-11.514930216703913,-12.21116334483912,-1.5244106047034964,0.31110608306672644,3.5613147639408047,-0.14233687363497416,0.14403110649350065,-0.16182451278233986,-3.97255545386629,13.38691517096303,-0.02295415840607802,3.8008919769672134,0.2723469447728018,0.2000194846837856,0.5918401644717226],[-24.078292520120343,3.242446902381067,30.045103888940158,-0.8818228810620086,-0.12806389221989414,0.10756876747339676,-30.16865155384281,7.216428027033049,29.22239458513505,-4.2394462168850175,-0.1413670709397617,0.10710308675687595,-11.14493188988733,-1.872681278528879,-9.216631525417439,-0.5675112696428362,3.7029494694006457,0.18933391547078296,-0.11037179115392765,-0.16653429580981252,7.284094486712157,12.987598769966159,2.828012722351407,0.63847431839704,-0.4655468440595284,-0.5943221088701023,0.01949866933214298],[-18.915169468729857,4.178542509491551,23.656236605157595,-4.083685067101461,-0.11104383435556803,0.10386906405882466,-10.953280551768085,24.139842623280902,11.896220291122557,7.288026840030591,0.07317964196636792,0.018993499564886385,-20.27988133430694,-1.9572303057689004,15.630881286515923,0.40980182459323183,4.3780870940165935,-0.3856027786786093,-0.34011300317715426,-0.48253869883177836,7.351445868573332,-10.019817854060308,2.7024781113043463,-0.6533378030747284,-0.017301851224632767,0.017786512471836484,-0.17622536726289365],[-23.930554038609177,27.78105309998745,7.588357738025014,6.594211363743758,0.30328896974150216,0.025934567780921076,27.276161616380172,-13.23244819568285,-11.38695294185893,-7.505028371822894,-0.3117956253197324,-0.03098463807430067,-1.562367290250629,-3.2882556326971195,9.333574604552156,-1.8030766710187978,3.3608854974271036,0.23226279964868904,0.3248238936068765,0.5571567567120046,3.1152047979561512,9.012054954777247,3.812330304355884,1.0322530938691692,-0.3024742924012005,-0.6106139751782844,0.225629675850645],[35.52892180637309,-9.808289671799017,-30.60040739020999,-1.250643000602821,-0.13492613729027528,-0.10122008435420213,0.7056401486049658,-0.2464667599891748,2.8221024058757505,-5.350110750095458,0.17208871009646903,0.025972624685196462,-1.0352638102584493,7.732210455191278,-10.580479761292484,-0.7145684121931606,-1.67651971083557,-0.01900227537872591,0.07382874233790294,-0.1768112568569064,1.4969530149596448,5.754362914260456,-0.13587802464824872,0.908708702431732,-0.007883484569088304,0.0030446897413848473,-0.03177080794712269],[-32.72903610661499,4.007155827740289,-23.44737026227066,3.088718845298323,0.1922645901522197,-0.10039473605702233,11.395916849244859,20.369891406585396,25.064049736682726,-4.467424605275533,-0.22475364921057184,0.08251277899263647,2.6484904886266794,1.7944339002124425,4.071050305816442,1.6986611513714256,-2.588069273695447,0.3200297280647064,0.34289735115500136,-0.15130613680321464,0.9397784409037092,7.129257497242868,1.774822171301644,-1.8050805262366407,-0.19985364799394853,0.504658275068969,0.3037121187614436]], -"twist": {"left_wrist_yaw_link":[[0.2758728247565696,-0.30613284666721874,-0.1680741212552253,0.016163452985387072,-0.5911781307253973,0.9524814802188076],[0.18256644312743853,0.4068428563668897,-0.1964418846670635,0.22058418200847646,0.07374196148745671,-0.3701891760183178],[0.020046954650957304,0.2875994621817797,-0.18778430637017246,-0.12310083116709114,-0.9927187269778657,0.939216792513438],[0.21563492140466428,0.13641453659404706,-0.08932633036728807,0.2946899217805242,-0.49300471423234693,-0.2294438970268271],[0.07093416165558611,0.1573408547182069,0.05881288678887962,0.27927803286252223,0.040465321419557526,-0.488978342225961],[-0.11767552007759693,-0.07280417929792735,0.08791211987084446,0.40218397609280854,0.09717246259265261,-1.4929121743060865],[0.1492651999635905,-0.3314082026247353,-0.16158122339581363,-0.2743792906634549,1.4920170002925723,-1.0546234828737238],[0.5195885587660197,-0.36574710125153037,0.08897123209727892,1.5474382677431366,0.9124815976275769,-0.4099474576667377],[-0.0015067895962256337,0.1242752126992446,-0.10992667163514237,-0.019644568871396987,0.6260372406129449,1.0640544187281016],[0.5235166057787682,-0.040618167594496754,-0.10690133890285403,0.33243276762143825,-0.8774452336496116,1.4137975252928259],[-0.06552698879937727,0.2820883994866631,0.37152886016021913,1.0645557329162019,0.7130651685185522,-0.37920398754533763],[-0.637853739914608,0.09645183506578314,-0.2098921550678557,0.2702910859442942,-0.520869641009051,-1.708967559173976],[0.5242339424199508,-0.2542725419475489,0.1418481840251503,-0.09409307400365008,-0.5749845334266046,0.8571717678651627],[0.007229073333828162,-0.05960882615195558,-0.0464332341547927,0.45998328337205263,-1.3440305985325587,-0.39716128296195197],[-0.18688866754477568,-0.17310719684394915,0.06231685911208321,0.6076246667052401,1.485183114190864,-0.5915050721782276],[-0.08733923266555527,0.14249666162519842,-0.06366701045514626,0.16430461220344253,-0.6322199575825778,0.23039438049206484],[0.003143563443851985,-0.4227605532286419,0.12697472415396693,0.2981759997271072,0.038285160402783514,-0.36328168676108313],[-0.4584965252849171,0.28218747510114944,-0.22322289110682444,-0.12323426639776874,1.7653250848846986,0.7102303698386707],[-0.018441064427613334,-0.4001559178593002,0.09379044697581637,0.15098448748290677,0.4608151145385543,0.8286476396872956],[-0.14677474088580642,0.7572263031980906,0.0837119839416744,-1.4136401943972743,-0.39725517767702906,-1.5781547868682997],[0.11922049559395545,-0.08339350638077182,0.3040355098507215,-0.6505941704713238,-1.478541637453088,-0.10928178917156306],[0.0481989697800273,-0.4461662091162825,-0.6418865281764908,-2.334925425083134,-0.4302424463027462,-0.5999019604603761],[-0.09768904124039254,0.6018720266089757,0.04586090793879666,-1.280325148565724,0.3331292404819752,0.5042164492958744],[-0.1465725681366149,0.512989267149696,0.1945138739800389,2.0424595356432835,0.45428319294684366,0.8305679066360281],[0.06377719077747547,-0.5012040741922922,0.09496539672475203,0.6771485143437579,-0.8734138188993967,-0.4090941930996526],[0.014877165994918201,-0.09566427073567088,0.047239570345378804,-0.3819489031194869,0.6803069291765862,1.9211531706262548],[0.03655989564564043,-0.1950257424857687,0.07320607796950071,-1.0767995213340413,0.3491978581238596,0.19754073494047938],[0.2810207233068712,0.6690398067487705,0.3497742972056992,-0.8576445161082722,-0.9285459282971292,-0.10300075423305133],[-0.02639048052001837,0.43758584918207805,0.09299266731288722,-0.9450536782781556,0.01611321998313202,0.11181766877953736],[-0.3554207942758964,-0.2156589594015763,0.2975590978362797,-0.39635610062057713,0.9686574215534705,1.303057578470058],[-0.41315267508855674,0.18943106385451855,-0.018448001982796104,-1.1271012366269135,-0.2206543928329171,1.2070583002040411],[0.43331296625459614,-0.7188276972772303,0.14406183097483483,-0.20945105261621488,-1.2817283970485986,-0.524313718184197],[0.6643383631146055,-0.6905785658585939,-0.46305712504922397,-1.4850350605376752,1.2402429088965239,-1.4322857950145114],[-0.006642339831204991,-0.04622050126188677,-0.20170570890863268,-0.5882035611110638,-0.4626720275217612,0.4094587332902596],[0.019085531282416517,0.0933178480080654,-0.014183992885737625,-0.5761729112565187,0.17179020621320307,0.20339851322621894],[0.0680740007729437,0.019894641984834198,-0.0990897685162357,0.5252041893480799,1.4006172622799626,1.9695826293790883],[0.11090031596029741,-0.1697381225653779,-0.2961654264736964,-0.916166758332379,-1.642958205146685,-0.0003306428462535038],[0.2953674629573447,-0.8597285036847772,0.13311818172212483,-0.9972200547000126,-1.1416620709670922,-2.037308487742088],[0.3047996065304618,0.014142256059344605,-0.14337874251716246,-0.8697344919886083,-1.364361325945187,0.022661139381868922],[0.08827333183345205,-0.6945125279775053,0.16907240863899886,0.40655415475762136,0.14760315608773544,-1.0728347876891702],[0.5378106195606424,0.01646095294207181,0.23116997579256007,1.2305309367148454,1.1460717097587712,-0.7766529368009616],[0.3132039876098176,0.09513458558670515,0.047103873582849023,0.6988646763767485,1.8156676520768014,-0.31091757804493125],[-0.03212772724158269,0.06851134240055769,-0.5317082945806142,0.31417133690583715,2.115334106283088,0.0018105039311383242],[-0.22847355709402464,0.19651999051944263,-0.11425743685591627,0.2147963501075863,0.37124805498724084,0.029876089132377104],[0.3570209024011811,-0.050838782731801944,-0.17874570798503278,-0.7066668852332375,0.16480694262643475,-0.6995605881596874],[0.05179922307191451,-0.19804832871689731,0.0741309170286645,1.4287616533247562,-1.3176212350890855,-0.3306704296126659],[-0.39883878100909603,0.48920413140420466,0.1698168386789058,0.4796877484585484,0.18132970060430767,-2.0917588852219686],[-0.28593323071291904,-0.34870545893113364,0.2375971116842519,-0.9214053227984909,0.4612091223508921,0.6901910215979739],[0.0923587560776088,-0.23635960063373188,-0.11970650910863471,0.35282876728989704,-0.06933403183101798,1.693543402742747],[-0.05071003942791433,-0.030797926019346264,-0.03190405503531649,1.770336403699141,0.30885338877955104,1.1006876248193695]],"right_wrist_yaw_link":[[-0.25700427687713556,-0.5160342794484012,-0.16963254799370528,1.5160812975686566,-1.5649037996183153,0.22427634046284384],[-0.2918924292126497,-0.03213569247650339,0.1842494254589684,1.298120982080502,-1.1291211222511015,0.9573778488283127],[-0.20262605039704012,0.45655440158489197,0.02459028972951055,-0.02981482765493515,0.046633592252321776,0.12721702981312522],[-0.1323332471936136,-0.06181412160086558,0.06163116045995802,0.7971940667354621,-0.9270098710820716,1.5352416359687358],[-0.41962450310382976,-0.03543526668430157,0.5032147239029977,0.2954026657546124,-0.4908110092657455,1.019805665555221],[-0.08794847782848411,0.23733603039695828,0.1402053938035747,-0.21731146316975097,0.037226035122463336,1.5599004721792178],[-0.11212082500322124,0.018652252493730356,-0.06388445894147537,0.301698926355324,2.8371211194498893,-1.1534684273812519],[-0.14691308237649175,-0.07386809450363593,0.14979360631962438,-0.6638541550599345,0.05261275790109032,-0.8064860246911009],[-0.10340794989970961,-0.0933706079048352,0.050660411651363056,-0.30244781223920136,0.05673112118896015,0.9538171538284512],[0.1318254688317316,-0.2118651571197766,-0.022823873823135783,-0.6240605519463873,-1.1154256083074632,-1.331720614719698],[0.2670424904532676,0.04011225788350124,-0.061007457696666996,-0.7118017192749264,1.1328949024311545,0.36362454669069816],[0.8712969127091754,-0.34333184210233614,-0.07930257211536672,-1.0870884700289083,0.1264210163534703,-2.2401034964684636],[-0.2985543747284336,0.25372808553602616,-0.14170674280244755,0.8742014020155513,-0.3188604004065176,1.6545690159343056],[0.007111228212740003,-0.1854871187728409,0.14137145764908376,1.3571878390939225,0.028544893595863596,0.3333963552581853],[-0.34293028623516747,0.3377347934857917,0.40417198998452974,1.847228436097778,0.4534404950867171,1.3300704051602257],[0.3216068630655584,-0.005534120781752985,-0.20777489193902943,0.5159651911260776,0.6067275076891603,0.3827097631357437],[-0.6395094974765314,-0.3192059583384156,0.345976086155122,-1.326447403670762,0.8530304179909098,-1.4870388351464687],[0.5038665770006066,-0.15471204045332052,-0.17782614621563786,1.3884628230903484,0.21585987496058961,0.1355867472463635],[0.43206329733814725,0.2145815197065184,-0.10364408784098085,0.13907669005850032,1.7610032525909385,0.15038807316758152],[0.1542272801317035,-0.19742123987020854,0.14697593502955356,0.4643536837489876,1.3394562397273564,-0.8144704768525817],[0.10244359146030835,-0.1852853283545618,-0.08183739316150228,-1.0197204006123914,0.9595430365397735,-1.2136034064530472],[0.2995976420271057,-0.47927616388420285,-0.33866311157094264,0.6342704454563541,0.6730706740343243,-0.826621484395198],[0.583812124291699,0.2787361191703592,-0.11183516239080021,-0.41874843293209746,1.156388355518378,0.4934883960892618],[0.47919902076486215,0.43170759459403485,-0.07279674146042076,1.4551623814651315,-0.30384299055916353,-0.6310095715222134],[-0.140566643653053,-0.11509795592614536,-0.23594123837978473,-0.46970911514531527,-1.3918772060459328,-1.3907247593846708],[-0.1630251933573918,0.008682728509932652,0.45739862587369623,0.8235997308295457,-2.341015847626285,1.2125483129401335],[0.10863709985236873,0.03688373652237617,0.2647677220987443,1.0716837076795507,-0.6944664308723258,0.6104065342574215],[0.48231388018850047,-0.36820994962003406,0.12953774422329797,0.3623064586581618,0.08454242236295584,0.8210454385869219],[0.1594962055606631,-0.26438958710337696,0.10964661032286441,0.43593860914602306,-1.1205298786366116,-0.5871726809622106],[-0.509896599104815,0.025910783800728893,-0.04718610406724713,0.01923555429828902,1.4207794979757133,-0.9381897665910084],[0.5968839955497902,-0.061838925978757064,-0.14782661741228084,-0.12660503642781112,-0.7784511293574388,0.050806435869522826],[-0.529028602348669,0.01025295247388199,-0.1246665198266421,0.5630606805622466,0.8972235785389916,-0.6581563683068057],[0.05392463080156625,0.24603691640861947,-0.0201256123672041,-0.6155733576194773,1.5272101954722803,-1.317656709093439],[0.01962232692159531,0.09236794375539553,0.37696480621334116,-0.44760682344336544,1.26425516981858,-1.0869234354569854],[0.06596848420026319,-0.5244846228365851,-0.15472497920305175,-0.48771922144519797,-0.6988646475007856,1.3278862735656656],[0.13908345164542688,-0.0960521008597966,0.05848969831156492,0.10433215187929934,0.6753313445619562,-1.5615379441967188],[-0.20693577160707213,0.23867173214765688,-0.04721756896439344,-0.08185321328595418,0.4084908286691856,-1.4270692957190896],[-0.13470694707581188,0.0606594582793258,0.14823944044574158,1.5299108649474278,-1.1210074019590017,-0.9971044743868326],[0.3534179521575341,-0.1043887151208816,-0.047720951334716016,0.09230897961231582,-0.051695837208889595,-1.657057283791255],[0.24997341331327624,0.29236013369308717,0.22092389340860813,-1.075433291357341,0.028843242201023245,0.1871493104419944],[-0.8538258135866765,-0.16459245593296323,0.11832759300597136,0.5569686101389709,-1.2423478906962606,-0.5362339596272147],[0.21584602455478102,-0.3192316817381942,0.006684225555771268,0.24353310442874,-1.7146630409799553,0.7002674351960436],[0.23720796196228167,0.05509732723597373,0.09785286688285141,-0.9746900766796947,0.3017397553082085,0.5536046635024002],[0.13438494800031306,0.07025912332349636,-0.20883000239322685,1.2743216389169945,0.10135565732707025,1.0360955585375538],[-0.09086408407105256,-0.05167950484022579,0.004774828873079279,-0.4547529938587679,0.400949792686292,-0.49160935833785724],[0.029512519272319782,-0.25959559447134056,0.4102870739382247,0.029944629702947367,-1.3112254229159293,-0.6027514917286538],[0.24507531296561394,-0.006632243205191055,0.0588075998798976,0.19842912142644792,0.7782596685328105,-0.013623204320642679],[-0.013855528323509489,0.012463289778384207,-0.07495886993740405,0.5441900661702167,-0.12890555156584782,-1.1608976615259399],[0.43914189586766755,0.11453519931499517,-0.10798486193503137,-0.4742688164551868,0.3306699196404028,0.4399940059091154],[0.15569884285965277,0.644581859886386,-0.034139811218491495,-1.6090493221602742,-0.0338786236616605,0.25545314208852177]]}, -"collision_free_reduced": [false,true,true,false,true,true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,true,true,true,true,true,true,false,true,true,true,true,true,true,true] -} diff --git a/core_ws/src/golem_upper_ik/test/fixtures/old_controller/g1.json b/core_ws/src/golem_upper_ik/test/fixtures/old_controller/g1.json deleted file mode 100644 index dba45986..00000000 --- a/core_ws/src/golem_upper_ik/test/fixtures/old_controller/g1.json +++ /dev/null @@ -1,916 +0,0 @@ -{ - "robot": "g1", - "body_joints": [ - "left_hip_pitch_joint", - "left_hip_roll_joint", - "left_hip_yaw_joint", - "left_knee_joint", - "left_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_hip_pitch_joint", - "right_hip_roll_joint", - "right_hip_yaw_joint", - "right_knee_joint", - "right_ankle_pitch_joint", - "right_ankle_roll_joint", - "waist_yaw_joint", - "waist_roll_joint", - "waist_pitch_joint", - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint" - ], - "upper_body_joints": [ - "waist_yaw_joint", - "waist_roll_joint", - "waist_pitch_joint", - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint" - ], - "upper_body_index": [ - 12, - 13, - 14, - 15, - 16, - 17, - 18, - 19, - 20, - 21, - 22, - 23, - 24, - 25, - 26, - 27, - 28 - ], - "lower_body_index": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8, - 9, - 10, - 11 - ], - "enabled_joints": [ - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint" - ], - "waist_joints": [ - "waist_yaw_joint", - "waist_roll_joint", - "waist_pitch_joint" - ], - "ee_frames": [ - "left_hand_palm_link", - "right_hand_palm_link" - ], - "joint_limits": { - "position": [ - [ - -2.5307, - 2.8798 - ], - [ - -0.5236, - 2.9671 - ], - [ - -2.7576, - 2.7576 - ], - [ - -0.087267, - 2.8798 - ], - [ - -0.87267, - 0.5236 - ], - [ - -0.2618, - 0.2618 - ], - [ - -2.5307, - 2.8798 - ], - [ - -2.9671, - 0.5236 - ], - [ - -2.7576, - 2.7576 - ], - [ - -0.087267, - 2.8798 - ], - [ - -0.87267, - 0.5236 - ], - [ - -0.2618, - 0.2618 - ], - [ - -2.618, - 2.618 - ], - [ - -0.52, - 0.52 - ], - [ - -0.52, - 0.52 - ], - [ - -3.0892, - 2.6704 - ], - [ - -1.5882, - 2.2515 - ], - [ - -2.618, - 2.618 - ], - [ - -1.0472, - 2.0944 - ], - [ - -1.972222054, - 1.972222054 - ], - [ - -1.614429558, - 1.614429558 - ], - [ - -1.614429558, - 1.614429558 - ], - [ - -3.0892, - 2.6704 - ], - [ - -2.2515, - 1.5882 - ], - [ - -2.618, - 2.618 - ], - [ - -1.0472, - 2.0944 - ], - [ - -1.972222054, - 1.972222054 - ], - [ - -1.614429558, - 1.614429558 - ], - [ - -1.614429558, - 1.614429558 - ] - ], - "velocity": [ - 32.0, - 20.0, - 32.0, - 20.0, - 30.0, - 30.0, - 32.0, - 20.0, - 32.0, - 20.0, - 30.0, - 30.0, - 32.0, - 30.0, - 30.0, - 37.0, - 37.0, - 37.0, - 37.0, - 37.0, - 22.0, - 22.0, - 37.0, - 37.0, - 37.0, - 37.0, - 37.0, - 22.0, - 22.0 - ], - "effort": [ - 88.0, - 139.0, - 88.0, - 139.0, - 35.0, - 35.0, - 88.0, - 139.0, - 88.0, - 139.0, - 35.0, - 35.0, - 88.0, - 35.0, - 35.0, - 25.0, - 25.0, - 25.0, - 25.0, - 25.0, - 5.0, - 5.0, - 25.0, - 25.0, - 25.0, - 25.0, - 25.0, - 5.0, - 5.0 - ] - }, - "named_configs": { - "home": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "arms_down": [ - 0.0, - 0.0, - 0.0, - 1.5707963267948966, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.5707963267948966, - 0.0, - 0.0, - 0.0 - ], - "t_pose": [ - 0.0, - 1.5707963267948966, - 0.0, - 1.5707963267948966, - 0.0, - 0.0, - 0.0, - 0.0, - -1.5707963267948966, - 0.0, - 1.5707963267948966, - 0.0, - 0.0, - 0.0 - ], - "arms_front_45": [ - -0.7853981633974483, - 0.0, - 0.0, - 1.5707963267948966, - 0.0, - 0.0, - 0.0, - -0.7853981633974483, - 0.0, - 0.0, - 1.5707963267948966, - 0.0, - 0.0, - 0.0 - ] - }, - "startup_sequence": [ - "home" - ], - "planner_frame_names": [ - "left_hand_palm_link", - "right_hand_palm_link" - ], - "configs": { - "g1_split": { - "mode": null, - "topics": { - "low_cmd": "rt/safety/lowcmd_upper_in", - "low_state": "rt/lowstate", - "safety_out": "rt/lowcmd" - }, - "network": { - "domain_id": 0, - "interface": null - }, - "controller": { - "v_lim": 0.15, - "w_lim": 0.5, - "dq_lim": 0.3, - "d_min": 0.02, - "timeout": 10.0, - "waist_target": [ - 0.0, - 0.0, - 0.0 - ], - "state_timeout": 5.0, - "startup_timeout": 30.0, - "startup_move_timeout": 30.0, - "follow_tolerance": 0.05, - "imu_frame": "imu_in_pelvis", - "threshold_ik": 0.0001, - "threshold_joint": 0.001, - "threshold_linear": 0.001, - "threshold_angular": 0.002 - }, - "frequency": { - "ctrl_hz": 30.0, - "pub_hz": 500.0, - "check_hz": 1000.0 - }, - "planner": { - "enabled": false, - "planner": "RRTConnect", - "timeout": 1.0, - "frame_names": [ - "left_hand_palm_link", - "right_hand_palm_link" - ] - }, - "gains": { - "kp": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 200.0, - 200.0, - 200.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0 - ], - "kd": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 8.0, - 10.0, - 10.0, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5 - ], - "ki": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - }, - "clip": { - "q_low": [ - -2.5207, - -0.5136, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.5207, - -2.9571, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.608, - -0.51, - -0.51, - -3.0792, - -1.5782, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558, - -3.0792, - -2.2415000000000003, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558 - ], - "q_high": [ - 2.8698, - 2.9571, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.8698, - 0.5136, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.608, - 0.51, - 0.51, - 2.6604, - 2.2415000000000003, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558, - 2.6604, - 1.5782, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558 - ], - "dq": [ - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 3.0, - 3.0, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2 - ], - "tau": [ - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 21.0, - 21.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0 - ] - }, - "logging": { - "enabled": false, - "base_dir": "data/control_record", - "filename": "record", - "record_interval": 1.0 - } - }, - "sim_split": { - "mode": null, - "topics": { - "low_cmd": "rt/safety/lowcmd_upper_in", - "low_state": "rt/lowstate", - "safety_out": "rt/lowcmd" - }, - "network": { - "domain_id": 1, - "interface": null - }, - "controller": { - "v_lim": 0.5, - "w_lim": 1.0, - "dq_lim": 1.0, - "d_min": 0.02, - "timeout": 5.0, - "waist_target": [ - 0.0, - 0.0, - 0.0 - ], - "state_timeout": 5.0, - "startup_timeout": 30.0, - "startup_move_timeout": 30.0, - "follow_tolerance": 0.05, - "imu_frame": "imu_in_pelvis", - "threshold_ik": 0.0001, - "threshold_joint": 0.001, - "threshold_linear": 0.001, - "threshold_angular": 0.002 - }, - "frequency": { - "ctrl_hz": 30.0, - "pub_hz": 500.0, - "check_hz": 1000.0 - }, - "planner": { - "enabled": false, - "planner": "RRTConnect", - "timeout": 1.0, - "frame_names": [ - "left_hand_palm_link", - "right_hand_palm_link" - ] - }, - "gains": { - "kp": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 200.0, - 200.0, - 200.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0, - 60.0 - ], - "kd": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 8.0, - 10.0, - 10.0, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5, - 1.5 - ], - "ki": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - }, - "clip": { - "q_low": [ - -2.5207, - -0.5136, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.5207, - -2.9571, - -2.7476000000000003, - -0.077267, - -0.8626699999999999, - -0.25179999999999997, - -2.608, - -0.51, - -0.51, - -3.0792, - -1.5782, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558, - -3.0792, - -2.2415000000000003, - -2.608, - -1.0372, - -1.962222054, - -1.604429558, - -1.604429558 - ], - "q_high": [ - 2.8698, - 2.9571, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.8698, - 0.5136, - 2.7476000000000003, - 2.8698, - 0.5136, - 0.25179999999999997, - 2.608, - 0.51, - 0.51, - 2.6604, - 2.2415000000000003, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558, - 2.6604, - 1.5782, - 2.608, - 2.0844, - 1.962222054, - 1.604429558, - 1.604429558 - ], - "dq": [ - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 2.0, - 3.2, - 2.0, - 3.0, - 3.0, - 3.2, - 3.0, - 3.0, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2, - 3.7, - 3.7, - 3.7, - 3.7, - 3.7, - 2.2, - 2.2 - ], - "tau": [ - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 83.39999999999999, - 52.8, - 83.39999999999999, - 21.0, - 21.0, - 52.8, - 21.0, - 21.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0, - 3.0, - 3.0 - ] - }, - "logging": { - "enabled": false, - "base_dir": "data/control_record", - "filename": "record", - "record_interval": 1.0 - } - } - } -} diff --git a/core_ws/src/golem_upper_ik/test/fixtures/old_controller/g1_pairing.json b/core_ws/src/golem_upper_ik/test/fixtures/old_controller/g1_pairing.json deleted file mode 100644 index c270ee09..00000000 --- a/core_ws/src/golem_upper_ik/test/fixtures/old_controller/g1_pairing.json +++ /dev/null @@ -1,1042 +0,0 @@ -{ - "cases": [ - { - "controller": "g1_split", - "safety": "split", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "g1_split", - "safety": "split", - "sim": false, - "verdict": "accepted", - "message": null - }, - { - "controller": "g1_split", - "safety": "split", - "sim": true, - "verdict": "refused", - "message": "safety config \"split\" is for the real robot, not the sim" - }, - { - "controller": "g1_split", - "safety": "full", - "sim": null, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "full", - "sim": false, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "full", - "sim": true, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "sim_split", - "sim": null, - "verdict": "refused", - "message": "controller config \"g1_split\" and safety config \"sim_split\" mix sim and real: use sim_split.yaml with sim_*, g1_split.yaml with the others" - }, - { - "controller": "g1_split", - "safety": "sim_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "g1_split", - "safety": "sim_split", - "sim": true, - "verdict": "refused", - "message": "controller config \"g1_split\" is for the real robot, not the sim" - }, - { - "controller": "g1_split", - "safety": "/opt/configs/sim_split.yaml", - "sim": null, - "verdict": "refused", - "message": "controller config \"g1_split\" and safety config \"sim_split\" mix sim and real: use sim_split.yaml with sim_*, g1_split.yaml with the others" - }, - { - "controller": "g1_split", - "safety": "/opt/configs/sim_split.yaml", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "g1_split", - "safety": "/opt/configs/sim_split.yaml", - "sim": true, - "verdict": "refused", - "message": "controller config \"g1_split\" is for the real robot, not the sim" - }, - { - "controller": "g1_split", - "safety": "custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "sim_custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "sim_custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "sim_custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "relax_safety_split", - "sim": null, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "relax_safety_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "g1_split", - "safety": "relax_safety_split", - "sim": true, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "split", - "sim": null, - "verdict": "refused", - "message": "controller config \"sim_split\" and safety config \"split\" mix sim and real: use sim_split.yaml with sim_*, g1_split.yaml with the others" - }, - { - "controller": "sim_split", - "safety": "split", - "sim": false, - "verdict": "refused", - "message": "controller config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "sim_split", - "safety": "split", - "sim": true, - "verdict": "refused", - "message": "safety config \"split\" is for the real robot, not the sim" - }, - { - "controller": "sim_split", - "safety": "full", - "sim": null, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "full", - "sim": false, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "full", - "sim": true, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "sim_split", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "sim_split", - "safety": "sim_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "sim_split", - "safety": "sim_split", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "sim_split", - "safety": "/opt/configs/sim_split.yaml", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "sim_split", - "safety": "/opt/configs/sim_split.yaml", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "sim_split", - "safety": "/opt/configs/sim_split.yaml", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "sim_split", - "safety": "custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "sim_custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "sim_custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "sim_custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "relax_safety_split", - "sim": null, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "relax_safety_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_split", - "safety": "relax_safety_split", - "sim": true, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "split", - "sim": null, - "verdict": "refused", - "message": "controller config \"sim_split\" and safety config \"split\" mix sim and real: use sim_split.yaml with sim_*, g1_split.yaml with the others" - }, - { - "controller": "config/sim_split.yaml", - "safety": "split", - "sim": false, - "verdict": "refused", - "message": "controller config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "config/sim_split.yaml", - "safety": "split", - "sim": true, - "verdict": "refused", - "message": "safety config \"split\" is for the real robot, not the sim" - }, - { - "controller": "config/sim_split.yaml", - "safety": "full", - "sim": null, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "full", - "sim": false, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "full", - "sim": true, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "sim_split", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "config/sim_split.yaml", - "safety": "sim_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "config/sim_split.yaml", - "safety": "sim_split", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "config/sim_split.yaml", - "safety": "/opt/configs/sim_split.yaml", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "config/sim_split.yaml", - "safety": "/opt/configs/sim_split.yaml", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "config/sim_split.yaml", - "safety": "/opt/configs/sim_split.yaml", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "config/sim_split.yaml", - "safety": "custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "sim_custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "sim_custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "sim_custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "relax_safety_split", - "sim": null, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "relax_safety_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "config/sim_split.yaml", - "safety": "relax_safety_split", - "sim": true, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "split", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "split", - "sim": false, - "verdict": "accepted", - "message": null - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "split", - "sim": true, - "verdict": "refused", - "message": "safety config \"split\" is for the real robot, not the sim" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "full", - "sim": null, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "full", - "sim": false, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "full", - "sim": true, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "sim_split", - "sim": null, - "verdict": "refused", - "message": "controller config \"g1_split\" and safety config \"sim_split\" mix sim and real: use sim_split.yaml with sim_*, g1_split.yaml with the others" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "sim_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "sim_split", - "sim": true, - "verdict": "refused", - "message": "controller config \"g1_split\" is for the real robot, not the sim" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "/opt/configs/sim_split.yaml", - "sim": null, - "verdict": "refused", - "message": "controller config \"g1_split\" and safety config \"sim_split\" mix sim and real: use sim_split.yaml with sim_*, g1_split.yaml with the others" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "/opt/configs/sim_split.yaml", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "/opt/configs/sim_split.yaml", - "sim": true, - "verdict": "refused", - "message": "controller config \"g1_split\" is for the real robot, not the sim" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "sim_custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "sim_custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "sim_custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "relax_safety_split", - "sim": null, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "relax_safety_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "/opt/configs/g1_split.yaml", - "safety": "relax_safety_split", - "sim": true, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "split", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "custom", - "safety": "split", - "sim": false, - "verdict": "accepted", - "message": null - }, - { - "controller": "custom", - "safety": "split", - "sim": true, - "verdict": "refused", - "message": "safety config \"split\" is for the real robot, not the sim" - }, - { - "controller": "custom", - "safety": "full", - "sim": null, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "full", - "sim": false, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "full", - "sim": true, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "sim_split", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "custom", - "safety": "sim_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "custom", - "safety": "sim_split", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "custom", - "safety": "/opt/configs/sim_split.yaml", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "custom", - "safety": "/opt/configs/sim_split.yaml", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "custom", - "safety": "/opt/configs/sim_split.yaml", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "custom", - "safety": "custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "sim_custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "sim_custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "sim_custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "relax_safety_split", - "sim": null, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "relax_safety_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "custom", - "safety": "relax_safety_split", - "sim": true, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "split", - "sim": null, - "verdict": "refused", - "message": "controller config \"sim_custom\" and safety config \"split\" mix sim and real: use sim_split.yaml with sim_*, g1_split.yaml with the others" - }, - { - "controller": "sim_custom", - "safety": "split", - "sim": false, - "verdict": "refused", - "message": "controller config \"sim_custom\" is a sim config; the real robot never runs one" - }, - { - "controller": "sim_custom", - "safety": "split", - "sim": true, - "verdict": "refused", - "message": "safety config \"split\" is for the real robot, not the sim" - }, - { - "controller": "sim_custom", - "safety": "full", - "sim": null, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "full", - "sim": false, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "full", - "sim": true, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "sim_split", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "sim_custom", - "safety": "sim_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "sim_custom", - "safety": "sim_split", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "sim_custom", - "safety": "/opt/configs/sim_split.yaml", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "sim_custom", - "safety": "/opt/configs/sim_split.yaml", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "sim_custom", - "safety": "/opt/configs/sim_split.yaml", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "sim_custom", - "safety": "custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "sim_custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "sim_custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "sim_custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "relax_safety_split", - "sim": null, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "relax_safety_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "sim_custom", - "safety": "relax_safety_split", - "sim": true, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "split", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "safety_split", - "safety": "split", - "sim": false, - "verdict": "accepted", - "message": null - }, - { - "controller": "safety_split", - "safety": "split", - "sim": true, - "verdict": "refused", - "message": "safety config \"split\" is for the real robot, not the sim" - }, - { - "controller": "safety_split", - "safety": "full", - "sim": null, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "full", - "sim": false, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "full", - "sim": true, - "verdict": "refused", - "message": "safety config \"full\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "sim_split", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "safety_split", - "safety": "sim_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "safety_split", - "safety": "sim_split", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "safety_split", - "safety": "/opt/configs/sim_split.yaml", - "sim": null, - "verdict": "accepted", - "message": null - }, - { - "controller": "safety_split", - "safety": "/opt/configs/sim_split.yaml", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_split\" is a sim config; the real robot never runs one" - }, - { - "controller": "safety_split", - "safety": "/opt/configs/sim_split.yaml", - "sim": true, - "verdict": "accepted", - "message": null - }, - { - "controller": "safety_split", - "safety": "custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "sim_custom", - "sim": null, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "sim_custom", - "sim": false, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "sim_custom", - "sim": true, - "verdict": "refused", - "message": "safety config \"sim_custom\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "relax_safety_split", - "sim": null, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "relax_safety_split", - "sim": false, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - }, - { - "controller": "safety_split", - "safety": "relax_safety_split", - "sim": true, - "verdict": "refused", - "message": "safety config \"relax_safety_split\" must relay the upper-body channel: one of split, sim_split" - } - ], - "default_controller_config": { - "split": "g1_split", - "full": null, - "sim_split": "sim_split", - "/opt/configs/sim_split.yaml": "sim_split", - "custom": null, - "sim_custom": null, - "relax_safety_split": null - } -} diff --git a/core_ws/src/golem_upper_ik/test/fixtures/old_controller/h12.json b/core_ws/src/golem_upper_ik/test/fixtures/old_controller/h12.json deleted file mode 100644 index 2828aec0..00000000 --- a/core_ws/src/golem_upper_ik/test/fixtures/old_controller/h12.json +++ /dev/null @@ -1,1540 +0,0 @@ -{ - "robot": "h12", - "body_joints": [ - "left_hip_yaw_joint", - "left_hip_pitch_joint", - "left_hip_roll_joint", - "left_knee_joint", - "left_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_hip_yaw_joint", - "right_hip_pitch_joint", - "right_hip_roll_joint", - "right_knee_joint", - "right_ankle_pitch_joint", - "right_ankle_roll_joint", - "torso_joint", - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint" - ], - "upper_body_joints": [ - "torso_joint", - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint" - ], - "upper_body_index": [ - 12, - 13, - 14, - 15, - 16, - 17, - 18, - 19, - 20, - 21, - 22, - 23, - 24, - 25, - 26 - ], - "lower_body_index": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8, - 9, - 10, - 11 - ], - "enabled_joints": [ - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint" - ], - "joint_limits": { - "position": [ - [ - -0.43, - 0.43 - ], - [ - -3.14, - 2.5 - ], - [ - -0.43, - 3.14 - ], - [ - -0.12, - 2.19 - ], - [ - -0.897334, - 0.523598 - ], - [ - -0.261799, - 0.261799 - ], - [ - -0.43, - 0.43 - ], - [ - -3.14, - 2.5 - ], - [ - -3.14, - 0.43 - ], - [ - -0.12, - 2.19 - ], - [ - -0.897334, - 0.523598 - ], - [ - -0.261799, - 0.261799 - ], - [ - -2.35, - 2.35 - ], - [ - -3.14, - 1.57 - ], - [ - -0.38, - 3.4 - ], - [ - -2.66, - 3.01 - ], - [ - -0.95, - 3.18 - ], - [ - -3.01, - 2.75 - ], - [ - -0.4625, - 0.4625 - ], - [ - -1.27, - 1.27 - ], - [ - -3.14, - 1.57 - ], - [ - -3.4, - 0.38 - ], - [ - -3.01, - 2.66 - ], - [ - -0.95, - 3.18 - ], - [ - -2.75, - 3.01 - ], - [ - -0.4625, - 0.4625 - ], - [ - -1.27, - 1.27 - ] - ], - "velocity": [ - 23.0, - 23.0, - 23.0, - 14.0, - 9.0, - 9.0, - 23.0, - 23.0, - 23.0, - 14.0, - 9.0, - 9.0, - 23.0, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4, - 9.0, - 9.0, - 20.0, - 20.0, - 31.4, - 31.4, - 31.4 - ], - "effort": [ - 200.0, - 200.0, - 200.0, - 300.0, - 60.0, - 40.0, - 200.0, - 200.0, - 200.0, - 300.0, - 60.0, - 40.0, - 200.0, - 40.0, - 40.0, - 18.0, - 18.0, - 19.0, - 19.0, - 19.0, - 40.0, - 40.0, - 18.0, - 18.0, - 19.0, - 19.0, - 19.0 - ] - }, - "named_configs": { - "home": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "t_pose": [ - 0.0, - 1.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -1.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "arms_front_45": [ - -0.75, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -0.75, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "arms_overhead": [ - 0.0, - 2.9, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -2.9, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "arms_asym": [ - -1.45, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -1.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "arms_front_yaw": [ - -1.45, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 0.0, - -1.45, - 0.0, - -1.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "init_1": [ - 0.0, - 0.35, - 0.0, - 1.3, - 0.0, - 0.0, - 0.0, - 0.0, - -0.35, - 0.0, - 1.3, - 0.0, - 0.0, - 0.0 - ], - "init_2": [ - 0.7, - 0.35, - 0.0, - 1.3, - 0.0, - 0.0, - 0.0, - 0.7, - -0.35, - 0.0, - 1.3, - 0.0, - 0.0, - 0.0 - ], - "init_3": [ - 0.7, - 0.35, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.7, - -0.35, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "prep": [ - 0.7, - 0.35, - 0.0, - -0.7, - 0.0, - 0.0, - 0.0, - 0.7, - -0.35, - 0.0, - -0.7, - 0.0, - 0.0, - 0.0 - ], - "staging_right": [ - 0.7, - 0.35, - 0.0, - -0.7, - 0.0, - 0.0, - 0.0, - -1.0597, - -0.8747, - 1.5802, - -0.5551, - 2.4575, - -0.0036, - -1.2632 - ], - "staging_left": [ - -1.0597, - 0.8747, - -1.5802, - -0.5551, - -2.4575, - -0.0036, - 1.2632, - 0.7, - -0.35, - 0.0, - -0.7, - 0.0, - 0.0, - 0.0 - ] - }, - "startup_sequence": [ - "init_1", - "init_2", - "init_3", - "home" - ], - "planner_frame_names": [ - "left_grasp_frame", - "right_grasp_frame" - ], - "configs": { - "safety_split": { - "mode": "debug", - "topics": { - "low_cmd": "rt/safety/lowcmd_upper_in", - "low_state": "rt/lowstate" - }, - "network": { - "domain_id": 0, - "interface": null - }, - "controller": { - "v_lim": 1.0, - "w_lim": 2.0, - "dq_lim": 1.0, - "d_min": 0.02, - "timeout": 5.0, - "torso_target": 0.0, - "threshold_ik": 0.0001, - "threshold_joint": 0.01, - "threshold_linear": 0.001, - "threshold_angular": 0.002, - "max_state_lag_ms": 50.0 - }, - "frequency": { - "ctrl_hz": 30.0, - "pub_hz": 500.0 - }, - "planner": { - "planner": "RRTConnect", - "timeout": 1.0, - "range": 0.0, - "try_direct": true, - "simplify": true, - "simplify_time": 1.0, - "shortcut": true, - "moving_speed": 0.3, - "min_interpolation_steps": 30, - "max_interpolation_steps": 300, - "validity_resolution": 0.0025, - "constraint_check_steps": 10, - "frame_names": [ - "left_grasp_frame", - "right_grasp_frame" - ], - "frame_x_max": 0.6, - "frame_y_min": -0.45, - "frame_y_max": 0.45, - "frame_z_min": -0.05, - "frame_z_max": 0.6, - "frame_limit_margin": 0.01, - "frame_z_corridor_margin": 0.05, - "start_limit_tolerance": 0.05 - }, - "gains": { - "kp": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 160.0, - 240.0, - 200.0, - 150.0, - 150.0, - 120.0, - 120.0, - 120.0, - 240.0, - 200.0, - 150.0, - 150.0, - 120.0, - 120.0, - 120.0 - ], - "kd": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 10.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0 - ], - "ki": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 60.0, - 600.0, - 600.0, - 400.0, - 400.0, - 100.0, - 100.0, - 100.0, - 600.0, - 600.0, - 400.0, - 400.0, - 100.0, - 100.0, - 100.0 - ] - }, - "clip": { - "q_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq": [ - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14 - ], - "tau": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 120.0, - 24.0, - 24.0, - 10.799999999999999, - 19.9998, - 11.4, - 11.4, - 11.4, - 24.0, - 24.0, - 10.799999999999999, - 19.9998, - 11.4, - 11.4, - 11.4 - ] - }, - "logging": { - "enabled": false, - "base_dir": "data/control_record", - "filename": "record", - "record_interval": 1.0 - } - }, - "sim_safety_split": { - "mode": "debug", - "topics": { - "low_cmd": "rt/safety/lowcmd_upper_in", - "low_state": "rt/lowstate" - }, - "network": { - "domain_id": 1, - "interface": null - }, - "controller": { - "v_lim": 1.0, - "w_lim": 2.0, - "dq_lim": 1.0, - "d_min": 0.02, - "timeout": 5.0, - "torso_target": 0.0, - "threshold_ik": 0.0001, - "threshold_joint": 0.01, - "threshold_linear": 0.001, - "threshold_angular": 0.002, - "max_state_lag_ms": 50.0 - }, - "frequency": { - "ctrl_hz": 50.0, - "pub_hz": 500.0 - }, - "planner": { - "planner": "RRTConnect", - "timeout": 1.0, - "range": 0.0, - "try_direct": true, - "simplify": true, - "simplify_time": 1.0, - "shortcut": true, - "moving_speed": 0.3, - "min_interpolation_steps": 30, - "max_interpolation_steps": 300, - "validity_resolution": 0.0025, - "constraint_check_steps": 10, - "frame_names": [ - "left_grasp_frame", - "right_grasp_frame" - ], - "frame_x_max": 0.6, - "frame_y_min": -0.45, - "frame_y_max": 0.45, - "frame_z_min": -0.05, - "frame_z_max": 0.6, - "frame_limit_margin": 0.01, - "frame_z_corridor_margin": 0.05, - "start_limit_tolerance": 0.05 - }, - "gains": { - "kp": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 160.0, - 240.0, - 200.0, - 150.0, - 150.0, - 120.0, - 120.0, - 120.0, - 240.0, - 200.0, - 150.0, - 150.0, - 120.0, - 120.0, - 120.0 - ], - "kd": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 10.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0 - ], - "ki": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 60.0, - 600.0, - 600.0, - 400.0, - 400.0, - 100.0, - 100.0, - 100.0, - 600.0, - 600.0, - 400.0, - 400.0, - 100.0, - 100.0, - 100.0 - ] - }, - "clip": { - "q_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq": [ - 69.0, - 69.0, - 69.0, - 42.0, - 27.0, - 27.0, - 69.0, - 69.0, - 69.0, - 42.0, - 27.0, - 27.0, - 69.0, - 27.0, - 27.0, - 60.0, - 60.0, - 94.19999999999999, - 94.19999999999999, - 94.19999999999999, - 27.0, - 27.0, - 60.0, - 60.0, - 94.19999999999999, - 94.19999999999999, - 94.19999999999999 - ], - "tau": [ - 600.0, - 600.0, - 600.0, - 900.0, - 180.0, - 120.0, - 600.0, - 600.0, - 600.0, - 900.0, - 180.0, - 120.0, - 600.0, - 120.0, - 120.0, - 54.0, - 54.0, - 57.0, - 57.0, - 57.0, - 120.0, - 120.0, - 54.0, - 54.0, - 57.0, - 57.0, - 57.0 - ] - }, - "logging": { - "enabled": false, - "base_dir": "data/control_record", - "filename": "record", - "record_interval": 1.0 - } - }, - "safety_full": { - "mode": "debug", - "topics": { - "low_cmd": "rt/safety/lowcmd_in", - "low_state": "rt/lowstate" - }, - "network": { - "domain_id": 0, - "interface": null - }, - "controller": { - "v_lim": 1.0, - "w_lim": 2.0, - "dq_lim": 1.0, - "d_min": 0.02, - "timeout": 5.0, - "torso_target": 0.0, - "threshold_ik": 0.0001, - "threshold_joint": 0.01, - "threshold_linear": 0.001, - "threshold_angular": 0.002, - "max_state_lag_ms": 50.0 - }, - "frequency": { - "ctrl_hz": 30.0, - "pub_hz": 500.0 - }, - "planner": { - "planner": "RRTConnect", - "timeout": 1.0, - "range": 0.0, - "try_direct": true, - "simplify": true, - "simplify_time": 1.0, - "shortcut": true, - "moving_speed": 0.3, - "min_interpolation_steps": 30, - "max_interpolation_steps": 300, - "validity_resolution": 0.0025, - "constraint_check_steps": 10, - "frame_names": [ - "left_grasp_frame", - "right_grasp_frame" - ], - "frame_x_max": 0.6, - "frame_y_min": -0.45, - "frame_y_max": 0.45, - "frame_z_min": -0.05, - "frame_z_max": 0.6, - "frame_limit_margin": 0.01, - "frame_z_corridor_margin": 0.05, - "start_limit_tolerance": 0.05 - }, - "gains": { - "kp": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 160.0, - 240.0, - 200.0, - 150.0, - 150.0, - 120.0, - 120.0, - 120.0, - 240.0, - 200.0, - 150.0, - 150.0, - 120.0, - 120.0, - 120.0 - ], - "kd": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 10.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0 - ], - "ki": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 60.0, - 600.0, - 600.0, - 400.0, - 400.0, - 100.0, - 100.0, - 100.0, - 600.0, - 600.0, - 400.0, - 400.0, - 100.0, - 100.0, - 100.0 - ] - }, - "clip": { - "q_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq": [ - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14 - ], - "tau": [ - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4 - ] - }, - "logging": { - "enabled": false, - "base_dir": "data/control_record", - "filename": "record", - "record_interval": 1.0 - } - }, - "debug": { - "mode": "debug", - "topics": { - "low_cmd": "rt/safety/lowcmd_in", - "low_state": "rt/lowstate" - }, - "network": { - "domain_id": 0, - "interface": null - }, - "controller": { - "v_lim": 1.0, - "w_lim": 2.0, - "dq_lim": 1.0, - "d_min": 0.02, - "timeout": 5.0, - "torso_target": 0.0, - "threshold_ik": 0.0001, - "threshold_joint": 0.01, - "threshold_linear": 0.001, - "threshold_angular": 0.002, - "max_state_lag_ms": 50.0 - }, - "frequency": { - "ctrl_hz": 30.0, - "pub_hz": 500.0 - }, - "planner": { - "planner": "RRTConnect", - "timeout": 1.0, - "range": 0.0, - "try_direct": true, - "simplify": true, - "simplify_time": 1.0, - "shortcut": true, - "moving_speed": 0.3, - "min_interpolation_steps": 30, - "max_interpolation_steps": 300, - "validity_resolution": 0.0025, - "constraint_check_steps": 10, - "frame_names": [ - "left_grasp_frame", - "right_grasp_frame" - ], - "frame_x_max": 0.6, - "frame_y_min": -0.45, - "frame_y_max": 0.45, - "frame_z_min": -0.05, - "frame_z_max": 0.6, - "frame_limit_margin": 0.01, - "frame_z_corridor_margin": 0.05, - "start_limit_tolerance": 0.05 - }, - "gains": { - "kp": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 160.0, - 240.0, - 200.0, - 150.0, - 150.0, - 120.0, - 120.0, - 120.0, - 240.0, - 200.0, - 150.0, - 150.0, - 120.0, - 120.0, - 120.0 - ], - "kd": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 10.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0, - 12.0 - ], - "ki": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 60.0, - 600.0, - 600.0, - 400.0, - 400.0, - 100.0, - 100.0, - 100.0, - 600.0, - 600.0, - 400.0, - 400.0, - 100.0, - 100.0, - 100.0 - ] - }, - "clip": { - "q_low": [ - -0.429, - -3.1390000000000002, - -0.429, - -0.119, - -0.896334, - -0.260799, - -0.429, - -3.1390000000000002, - -3.1390000000000002, - -0.119, - -0.896334, - -0.260799, - -2.349, - -3.1390000000000002, - -0.379, - -2.6590000000000003, - -0.949, - -3.009, - -0.4615, - -1.2690000000000001, - -3.1390000000000002, - -3.399, - -3.009, - -0.949, - -2.749, - -0.4615, - -1.2690000000000001 - ], - "q_high": [ - 0.429, - 2.499, - 3.1390000000000002, - 2.189, - 0.522598, - 0.260799, - 0.429, - 2.499, - 0.429, - 2.189, - 0.522598, - 0.260799, - 2.349, - 1.5690000000000002, - 3.399, - 3.009, - 3.1790000000000003, - 2.749, - 0.4615, - 1.2690000000000001, - 1.5690000000000002, - 0.379, - 2.6590000000000003, - 3.1790000000000003, - 3.009, - 0.4615, - 1.2690000000000001 - ], - "dq": [ - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 2.3000000000000003, - 2.3000000000000003, - 1.4000000000000001, - 0.9, - 0.9, - 2.3000000000000003, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14, - 0.9, - 0.9, - 2.0, - 2.0, - 3.14, - 3.14, - 3.14 - ], - "tau": [ - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 120.0, - 120.0, - 180.0, - 36.0, - 24.0, - 120.0, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4, - 24.0, - 24.0, - 10.799999999999999, - 10.799999999999999, - 11.4, - 11.4, - 11.4 - ] - }, - "logging": { - "enabled": false, - "base_dir": "data/control_record", - "filename": "record", - "record_interval": 1.0 - } - } - } -} diff --git a/core_ws/src/golem_upper_ik/test/test_config.py b/core_ws/src/golem_upper_ik/test/test_config.py index fd43b92f..b65434f3 100644 --- a/core_ws/src/golem_upper_ik/test/test_config.py +++ b/core_ws/src/golem_upper_ik/test/test_config.py @@ -1,11 +1,6 @@ ''' -The controller-config loader, on the carried configs and on configs written by the tests. - -test/fixtures/old_controller/.json holds what the robot's previous arm controller's -load_controller_config returned for the same configs (test/fixtures/README.md); these tests -import no old package. +The controller-config loader, on the installed configs and on configs written by the tests. ''' -import json import math from pathlib import Path import re @@ -15,7 +10,6 @@ import pytest import yaml -FIXTURES = Path(__file__).parent / 'fixtures' / 'old_controller' CONFIGS = { 'h12': ['safety_split', 'sim_safety_split', 'safety_full', 'debug'], 'g1': ['g1_split', 'sim_split'], @@ -25,19 +19,6 @@ 'h12': ('torso_joint',), 'g1': ('waist_yaw_joint', 'waist_roll_joint', 'waist_pitch_joint'), } -# the keys both old loaders return under controller, with the same meaning -SHARED_CONTROLLER_KEYS = ( - 'v_lim', 'w_lim', 'dq_lim', 'd_min', 'timeout', - 'threshold_ik', 'threshold_joint', 'threshold_linear', 'threshold_angular', -) -# where the configs differ from the old loaders' fixtures: the G1 has a self-collision model, -# so its planner is on -FIXTURE_DIFFERENCES = { - 'g1': {('planner', 'enabled'): True}, -} -G1_CONTROLLER_KEYS = ( - 'state_timeout', 'startup_timeout', 'startup_move_timeout', 'follow_tolerance', 'imu_frame', -) def load(robot, name_or_path): @@ -45,20 +26,11 @@ def load(robot, name_or_path): return load_controller_config(robot, name_or_path) -def old_config(robot, name): - return json.loads((FIXTURES / f'{robot}.json').read_text())['configs'][name] - - def installed(robot, name): share = Path(get_package_share_directory('golem_upper_ik')) return share / 'config' / robot / f'{name}.yaml' -def ros_name(dds_topic): - assert dds_topic.startswith('rt/'), dds_topic - return '/' + dds_topic[len('rt/'):] - - def write_config(tmp_path, robot, raw, directory=None): '''raw written as YAML to tmp_path//test.yaml''' path = tmp_path / (directory or robot) / 'test.yaml' @@ -84,76 +56,20 @@ def minimal(robot): def test_configs_load(case): robot, name = case.split('/') config = load(robot, name) - old = old_config(robot, name)['controller'] assert config['robot'] == robot - # the file names every held joint, so the values below are carried, not the defaults + # the file names every held joint, so the values below are the file's, not the defaults raw = yaml.safe_load(installed(robot, name).read_text()) assert sorted(raw['controller']['hold_targets']) == sorted(HELD[robot]) hold = config['controller']['hold_targets'] - if robot == 'h12': - assert hold == {'torso_joint': old['torso_target']} - else: - assert hold == dict(zip(HELD['g1'], old['waist_target'])) + assert hold == raw['controller']['hold_targets'] assert list(hold) == list(HELD[robot]) -@pytest.mark.parametrize('case', CASES) -def test_config_values_match_old_loaders(case): - robot, name = case.split('/') - config = load(robot, name) - old = old_config(robot, name) - - for key in ('low_cmd', 'low_state'): - assert config['topics'][key] == ros_name(old['topics'][key]), key - changed = FIXTURE_DIFFERENCES.get(robot, {}) - for key in SHARED_CONTROLLER_KEYS: - expected = changed.get(('controller', key), old['controller'][key]) - assert config['controller'][key] == expected, key - for key in ('ctrl_hz', 'pub_hz'): - assert config['frequency'][key] == old['frequency'][key], key - for key in ('kp', 'kd', 'ki'): - gains = config['gains'][key] - assert gains.dtype == np.float32, key - np.testing.assert_array_equal(gains, old['gains'][key], err_msg=key) - clip = config['limits'] - np.testing.assert_array_equal(clip['q_clip_limits'][:, 0], old['clip']['q_low']) - np.testing.assert_array_equal(clip['q_clip_limits'][:, 1], old['clip']['q_high']) - np.testing.assert_array_equal(clip['dq_clip_limits'], old['clip']['dq']) - np.testing.assert_array_equal(clip['tau_clip_limits'], old['clip']['tau']) - # the old loaders' logging section configured a command recorder no node started - assert 'logging' not in config - - # the planner section is the file's, with enabled and frame_names filled in - planner = dict(config['planner']) - assert planner.pop('enabled') == changed.get(('planner', 'enabled'), - old['planner'].get('enabled', True)) - frame_names = planner.pop('frame_names') - assert frame_names == old['planner'].get('frame_names', frame_names) - assert planner == {key: value for key, value in old['planner'].items() - if key not in ('enabled', 'frame_names')} - - if robot == 'h12': - # keys the H1-2 loader had and the G1's did not - assert config['controller']['max_state_lag_ms'] == old['controller']['max_state_lag_ms'] - assert config['topics']['safety_out'] == '/lowcmd' - assert config['controller']['imu_frame'] == 'torso_link' - assert frame_names == ['left_grasp_frame', 'right_grasp_frame'] - else: - # keys the G1 loader had and the H1-2's did not - assert config['topics']['safety_out'] == ros_name(old['topics']['safety_out']) - for key in G1_CONTROLLER_KEYS: - assert config['controller'][key] == old['controller'][key], key - assert config['frequency']['check_hz'] == old['frequency']['check_hz'] - assert config['controller']['max_state_lag_ms'] == 0.0 - - -def test_every_installed_config_has_a_fixture(): +def test_every_installed_config_is_tested(): share = Path(get_package_share_directory('golem_upper_ik')) / 'config' for robot, names in CONFIGS.items(): assert sorted(path.stem for path in (share / robot).glob('*.yaml')) == sorted(names) - fixture = json.loads((FIXTURES / f'{robot}.json').read_text()) - assert sorted(fixture['configs']) == sorted(names) @pytest.mark.parametrize('robot', ['h12', 'g1']) @@ -166,7 +82,6 @@ def test_defaults(robot, tmp_path): assert config['topics'] == { 'low_cmd': '/safety/lowcmd_upper_in', 'low_state': '/lowstate', - 'safety_out': '/lowcmd', } controller = config['controller'] assert controller['hold_targets'] == {joint: 0.0 for joint in HELD[robot]} @@ -237,27 +152,6 @@ def test_topics_are_ros_names(tmp_path): load('h12', write_config(tmp_path, 'h12', raw)) -def test_safety_out_is_an_output_of_the_robot(tmp_path): - raw = edited('h12', 'safety_split', topics={'safety_out': '/safety/lowcmd_upper_in'}) - with pytest.raises(ValueError, match='topics.safety_out'): - load('h12', write_config(tmp_path, 'h12', raw)) - raw = edited('g1', 'g1_split', topics={'safety_out': '/lowcmd'}) - assert load('g1', write_config(tmp_path, 'g1', raw))['topics']['safety_out'] == '/lowcmd' - raw = edited('g1', 'g1_split', topics={'safety_out': '/safety/lowcmd_upper_in'}) - with pytest.raises(ValueError, match='topics.safety_out'): - load('g1', write_config(tmp_path, 'g1', raw)) - - -@pytest.mark.parametrize('robot, name', [('h12', 'safety_split'), ('g1', 'g1_split')]) -def test_arm_sdk_output_refused(robot, name, tmp_path): - '''golem_safety writes /lowcmd only: a follow gate on another topic is refused, naming it''' - raw = edited(robot, name, topics={'safety_out': '/arm_sdk'}) - with pytest.raises(ValueError, match=re.escape( - f"key 'topics.safety_out': '/arm_sdk' is not a golem_safety output of robot " - f'{robot} (/lowcmd)')): - load(robot, write_config(tmp_path, robot, raw)) - - @pytest.mark.parametrize('key, raw_edit, message', [ ('bogus', {'bogus': 1}, 'unknown'), ('controller.bogus', {'controller': {'bogus': 1}}, 'unknown'), @@ -265,16 +159,11 @@ def test_arm_sdk_output_refused(robot, name, tmp_path): ('limits.clip.bogus', {'limits': {'clip': {'bogus': 1}}}, 'unknown'), ('zmp', {'zmp': {}}, 'unknown'), ('logging', {'logging': {'enabled': False}}, 'unknown'), - ('mode', {'mode': 'debug'}, 'removed'), - ('network', {'network': {'domain_id': 1}}, 'ROS supplies the domain'), - ('controller.torso_target', {'controller': {'torso_target': 0.0}}, 'hold_targets'), - ('controller.waist_target', {'controller': {'waist_target': 0.0}}, 'hold_targets'), - ('limits.estop', {'limits': {'estop': {'velocity_ratio': 0.5}}}, 'no estop of its own'), ('frequency.bogus', {'frequency': {'bogus': 1.0}}, 'unknown'), - ('planner.frame_z_min_margin', {'planner': {'frame_z_min_margin': 0.01}}, - 'frame_limit_margin'), + ('topics.safety_out', {'topics': {'safety_out': '/lowcmd'}}, 'unknown'), + ('limits.enforce', {'limits': {'enforce': {'velocity_ratio': 0.2}}}, 'unknown'), ]) -def test_unknown_and_replaced_keys_raise(tmp_path, key, raw_edit, message): +def test_unknown_keys_raise(tmp_path, key, raw_edit, message): raw = minimal('h12') for section, value in raw_edit.items(): raw[section] = value @@ -388,37 +277,14 @@ def test_planner_values_load_with_their_types(tmp_path): assert type(planner['max_interpolation_steps']) is int -def test_mode_without_an_output_topic_raises(monkeypatch): - import dataclasses - from golem_upper_ik import config as config_module - from golem_upper_ik.profile import load_profile - profile = dataclasses.replace(load_profile('h12'), modes=('full_body_mode', 'wobble_mode')) - monkeypatch.setattr(config_module, 'load_profile', lambda robot: profile) - with pytest.raises(ValueError, match='h12.*wobble_mode'): - config_module.load_controller_config('h12', 'safety_split') - - def test_numbers_written_without_a_dot_load(tmp_path): - # PyYAML reads 1e-3 (no dot) as a string; the old loaders' float() took it as a number + # PyYAML reads 1e-3 (no dot) as a string; the loader parses it as a number path = tmp_path / 'h12' / 'test.yaml' path.parent.mkdir(parents=True) path.write_text('gains: {kp: 10.0, kd: 1.0, ki: 0.0}\ncontroller: {threshold_ik: 1e-3}\n') assert load('h12', path)['controller']['threshold_ik'] == 0.001 -def test_two_spellings_of_one_key_raise(tmp_path): - raw = minimal('h12') - raw['limits'] = {'clip': {'velocity_ratio': 0.1}, 'enforce': {'velocity_ratio': 0.2}} - with pytest.raises(ValueError, match='limits.enforce'): - load('h12', write_config(tmp_path, 'h12', raw)) - raw = minimal('h12') - raw['limits'] = {'enforce': {'velocity_ratio': 0.2}} - config = load('h12', write_config(tmp_path, 'h12', raw)) - from golem_upper_ik.profile import load_profile - np.testing.assert_array_equal(config['limits']['dq_clip_limits'], - load_profile('h12').dq_max * 0.2) - - def test_position_offset_too_large_raises(tmp_path): raw = minimal('h12') raw['limits'] = {'clip': {'position_offset': 0.5}} diff --git a/core_ws/src/golem_upper_ik/test/test_controller_loop.py b/core_ws/src/golem_upper_ik/test/test_controller_loop.py index 359dde25..eaa1f040 100644 --- a/core_ws/src/golem_upper_ik/test/test_controller_loop.py +++ b/core_ws/src/golem_upper_ik/test/test_controller_loop.py @@ -187,7 +187,7 @@ def test_startup_home_ee_target_and_zero_gain_shutdown(ros, models, robot): arms = G1_LIMP_ARMS if robot == 'g1' else 'home' q0 = body_pose(profile, arms, [0.05] + [0.0] * (len(held) - 1)) model = shared_model(ros, models, robot) - robot_ = FollowingRobot(ros, profile, q0, config['topics']['safety_out']) + robot_ = FollowingRobot(ros, profile, q0, '/lowcmd') controller = build(ros, model, config) # start-up: the arms through the start-up postures to the last, the held joints eased to @@ -213,14 +213,14 @@ def test_startup_home_ee_target_and_zero_gain_shutdown(ros, models, robot): assert np.linalg.norm(ee_position(controller, 1) - right_start) < 0.005 # shutdown: one command, then silence. Mode 0 and zero gains and torque on the upper - # slots, except the held joints of a profile that keeps them on /lowcmd (the G1's waist), + # slots, except the held joints of a profile that keeps them (the G1's waist), # which stay at their hold controller.shutdown() stopped = time.monotonic() time.sleep(0.3) received, last = robot_.last_command assert received < stopped + 0.05 - keeps_held = profile.shutdown_keeps_held_joints_on_lowcmd + keeps_held = profile.shutdown_keeps_held_joints if not keeps_held: assert robot_.zero_gain_commands >= 1 for slot in upper: @@ -287,17 +287,17 @@ def shutdown_and_note(self, _shutdown=cls.shutdown, _name=name): # nothing relays the commands: the gate stays shut, the commands stay near the swaying # robot, and the start-up gives up after its own 2 s wait, not after the moves' 10 s config = config_of(robot, CONFIGS[robot], startup_timeout=2.0, startup_move_timeout=10.0) - safety_out = config['topics']['safety_out'] + lowcmd = '/lowcmd' q0 = np.zeros(len(profile.body_joints)) q0[slots_of(profile, ('left_elbow_joint', 'right_elbow_joint'))] = 0.8 - robot_ = RelayedRobot(ros, profile, q0, safety_out) + robot_ = RelayedRobot(ros, profile, q0, lowcmd) robot_.start() recorder = CommandRecorder(ros, config['topics']['low_cmd'], profile, robot_) startup = watch_startup(monkeypatch) model = shared_model(ros, models, robot) with pytest.raises(RuntimeError, match=re.escape( 'golem_safety did not relay the commands within controller.startup_timeout ' - f'(2 s): no {safety_out} sample carried them')) as raised: + f'(2 s): no {lowcmd} sample carried them')) as raised: build(ros, model, config) stopped = time.monotonic() # golem_safety engages each input on its own: no cause waits for another input @@ -324,7 +324,7 @@ def test_startup_move_stops_with_an_error_after_its_own_timeout(ros, models, rob # moves through its start-up postures back to it (at least 4 s at 1 rad/s) q0 = body_pose(profile, G1_LIMP_ARMS if robot == 'g1' else 'home') config = config_of(robot, CONFIGS[robot], startup_move_timeout=2.0) - robot_ = FollowingRobot(ros, profile, q0, config['topics']['safety_out']) + robot_ = FollowingRobot(ros, profile, q0, '/lowcmd') recorder = CommandRecorder(ros, config['topics']['low_cmd'], profile) model = shared_model(ros, models, robot) with pytest.raises(RuntimeError, match=r'Moving \w+: not done within ' @@ -355,7 +355,7 @@ def test_every_command_holds_the_hold_targets_while_the_held_joints_sag(ros, mod sag = np.zeros(len(profile.body_joints)) sag[held[-1]] = 0.02 q0 = body_pose(profile, 'home', targets) - robot_ = FollowingRobot(ros, profile, q0, config['topics']['safety_out'], sag=sag) + robot_ = FollowingRobot(ros, profile, q0, '/lowcmd', sag=sag) model = shared_model(ros, models, robot) controller = build(ros, model, config, init=False) recorder = CommandRecorder(ros, config['topics']['low_cmd'], profile) @@ -383,12 +383,12 @@ def test_waits_for_state_and_tracks_the_robot_through_engagement( profile = profile_of(robot) controller_config, safety_config = SPLIT[robot] config = config_of(robot, controller_config, follow_tolerance=follow_tolerance) - safety_out = config['topics']['safety_out'] + lowcmd = '/lowcmd' arms, held = STANDING[robot] q0 = body_pose(profile, arms, held) - robot_ = RelayedRobot(ros, profile, q0, safety_out) + robot_ = RelayedRobot(ros, profile, q0, lowcmd) upper_in = CommandRecorder(ros, config['topics']['low_cmd'], profile, robot_) - output = CommandRecorder(ros, safety_out, profile, robot_) + output = CommandRecorder(ros, lowcmd, profile, robot_) layer = SafetyNode(robot, safety_config, tmp_path) ros.on_close(layer.stop) opened = watch_gate_opening(monkeypatch) @@ -500,7 +500,7 @@ def slow(self, *args, _call=getattr(cls, name), _kind=kind, **kwargs): # the elbows 0.5 rad from home and the waist yaw 0.1 rad from its target q0 = body_pose(profile, 'home', [0.1, 0.0, 0.0]) q0[slots_of(profile, ('left_elbow_joint', 'right_elbow_joint'))] = 0.5 - FollowingRobot(ros, profile, q0, config['topics']['safety_out']) + FollowingRobot(ros, profile, q0, '/lowcmd') model = shared_model(ros, models, 'g1') controller = build(ros, model, config) dt = controller.dt diff --git a/core_ws/src/golem_upper_ik/test/test_frame_task_server.py b/core_ws/src/golem_upper_ik/test/test_frame_task_server.py index 8a15c891..61799b2f 100644 --- a/core_ws/src/golem_upper_ik/test/test_frame_task_server.py +++ b/core_ws/src/golem_upper_ik/test/test_frame_task_server.py @@ -164,8 +164,7 @@ def test_relative_frame_goal(ros, models, robot): from custom_ros_messages.action import FrameTask profile = profile_of(robot) config = config_of(robot, CONFIGS[robot]) - FollowingRobot(ros, profile, body_pose(profile, START[robot]), - config['topics']['safety_out']) + FollowingRobot(ros, profile, body_pose(profile, START[robot]), '/lowcmd') model = shared_model(ros, models, robot) left = PoseRecorder(ros, '/left_ee_pose') server_of(ros, model, CONFIGS[robot]) @@ -232,7 +231,7 @@ def test_relative_frame_goal_is_a_pelvis_frame_move(ros, models, robot, imu): profile = profile_of(robot) config = config_of(robot, CONFIGS[robot]) FollowingRobot(ros, profile, body_pose(profile, START[robot]), - config['topics']['safety_out'], imu_quaternion=IMU_ORIENTATIONS[imu]) + '/lowcmd', imu_quaternion=IMU_ORIENTATIONS[imu]) model = shared_model(ros, models, robot) left = PoseRecorder(ros, '/left_ee_pose') target = PoseRecorder(ros, '/left_ee_target') @@ -285,8 +284,7 @@ def test_slow_mode_scales_speed(ros, models, robot, tmp_path): v_lim = 0.04 # m/s config_path = edited_config(tmp_path, robot, CONFIGS[robot], v_lim=v_lim) config = config_of(robot, str(config_path)) - FollowingRobot(ros, profile, body_pose(profile, START[robot]), - config['topics']['safety_out']) + FollowingRobot(ros, profile, body_pose(profile, START[robot]), '/lowcmd') model = shared_model(ros, models, robot) recorder = CommandRecorder(ros, config['topics']['low_cmd'], profile) controller = server_of(ros, model, config_path).controller @@ -330,7 +328,7 @@ def test_named_config_slow_mode(ros, models, robot): # the left shoulder pitch starts 0.15 rad from the named configuration q0 = body_pose(profile, name) q0[profile.body_joints.index('left_shoulder_pitch_joint')] += 0.15 - robot_ = FollowingRobot(ros, profile, q0, config['topics']['safety_out']) + robot_ = FollowingRobot(ros, profile, q0, '/lowcmd') model = shared_model(ros, models, robot) recorder = CommandRecorder(ros, config['topics']['low_cmd'], profile) server_of(ros, model, CONFIGS[robot]) @@ -373,8 +371,7 @@ def test_stale_state_aborts_goal(ros, models, monkeypatch, capfd): from golem_upper_ik.core.controller import upper_controller profile = profile_of('h12') config = config_of('h12', CONFIGS['h12']) - robot_ = FollowingRobot(ros, profile, body_pose(profile, START['h12']), - config['topics']['safety_out']) + robot_ = FollowingRobot(ros, profile, body_pose(profile, START['h12']), '/lowcmd') model = shared_model(ros, models, 'h12') recorder = CommandRecorder(ros, config['topics']['low_cmd'], profile) log = Log(capfd) @@ -412,9 +409,7 @@ def stop_state_mid_goal(): def test_stale_state_does_not_abort_a_goal_off_the_real_robots_domain(ros, models, capfd): from custom_ros_messages.action import FrameTask profile = profile_of('h12') - config = config_of('h12', CONFIGS['h12']) - robot_ = FollowingRobot(ros, profile, body_pose(profile, START['h12']), - config['topics']['safety_out']) + robot_ = FollowingRobot(ros, profile, body_pose(profile, START['h12']), '/lowcmd') model = shared_model(ros, models, 'h12') log = Log(capfd) # on the test domain the tick-lag check is off: its deadline is 0 @@ -437,7 +432,7 @@ def test_frame_task_target_never_raises_while_tasks_are_added(ros, models): from golem_upper_ik.core.controller.frame_controller import FrameController profile = profile_of('g1') config = config_of('g1', CONFIGS['g1']) - FollowingRobot(ros, profile, body_pose(profile, START['g1']), config['topics']['safety_out']) + FollowingRobot(ros, profile, body_pose(profile, START['g1']), '/lowcmd') model = shared_model(ros, models, 'g1') controller = FrameController(ros.node('controller'), profile, config, robot_model=model, init=False) @@ -480,9 +475,7 @@ def test_unknown_frame_is_refused_and_the_poses_keep_going(ros, models, capfd): from custom_ros_messages.action import FrameTask from rclpy.action import ActionClient profile = profile_of('h12') - config = config_of('h12', CONFIGS['h12']) - FollowingRobot(ros, profile, body_pose(profile, START['h12']), - config['topics']['safety_out']) + FollowingRobot(ros, profile, body_pose(profile, START['h12']), '/lowcmd') model = shared_model(ros, models, 'h12') log = Log(capfd) left = PoseRecorder(ros, '/left_ee_pose') @@ -517,8 +510,7 @@ def test_planned_goals(ros, models, robot): from custom_ros_messages.action import FrameTask, NamedConfig profile = profile_of(robot) config = config_of(robot, CONFIGS[robot]) - robot_ = FollowingRobot(ros, profile, body_pose(profile, START[robot]), - config['topics']['safety_out']) + robot_ = FollowingRobot(ros, profile, body_pose(profile, START[robot]), '/lowcmd') model = shared_model(ros, models, robot) server_of(ros, model, CONFIGS[robot]) threshold = config['controller']['threshold_joint'] @@ -545,7 +537,7 @@ def test_planning_turned_off(ros, models, capfd, tmp_path): profile = profile_of('g1') config_path = edited_config(tmp_path, 'g1', CONFIGS['g1'], planner={'enabled': False}) config = config_of('g1', str(config_path)) - FollowingRobot(ros, profile, body_pose(profile, START['g1']), config['topics']['safety_out']) + FollowingRobot(ros, profile, body_pose(profile, START['g1']), '/lowcmd') model = shared_model(ros, models, 'g1') log = Log(capfd) recorder = CommandRecorder(ros, config['topics']['low_cmd'], profile) @@ -572,15 +564,13 @@ def test_planning_turned_off(ros, models, capfd, tmp_path): @pytest.mark.parametrize('robot', ROBOTS) def test_goals_start_from_the_last_command(ros, models, robot): profile = profile_of(robot) - config = config_of(robot, CONFIGS[robot]) arms = ik_slots(profile) # the left elbow sits 0.05 rad below every command (a gravity sag inside the 0.1 rad # the seed accepts), the right elbow 0.2 rad below (an arm held off its command) sag = np.zeros(len(profile.body_joints)) left_elbow = profile.body_joints.index('left_elbow_joint') sag[left_elbow] = 0.05 - robot_ = FollowingRobot(ros, profile, body_pose(profile, START[robot]), - config['topics']['safety_out'], sag=sag) + robot_ = FollowingRobot(ros, profile, body_pose(profile, START[robot]), '/lowcmd', sag=sag) model = shared_model(ros, models, robot) controller = server_of(ros, model, CONFIGS[robot]).controller assert wait_for(lambda: controller.low_cmd_handler.following, 2.0) @@ -618,7 +608,7 @@ def test_hold_pinned_at_a_limit_is_not_converged(ros, models, robot): sag = np.zeros(len(profile.body_joints)) sag[elbow] = 0.0005 q0 = body_pose(profile, START[robot]) - robot_ = FollowingRobot(ros, profile, q0, config['topics']['safety_out'], sag=sag) + robot_ = FollowingRobot(ros, profile, q0, '/lowcmd', sag=sag) model = shared_model(ros, models, robot) from golem_upper_ik.core.controller.frame_controller import FrameController controller = FrameController(ros.node('controller'), profile, config, robot_model=model, @@ -659,7 +649,7 @@ def test_execute_path_paces_at_dt_minus_work(ros, models, monkeypatch): config = config_of('h12', CONFIGS['h12']) model = shared_model(ros, models, 'h12') q0 = body_pose(profile, START['h12']) - FollowingRobot(ros, profile, q0, config['topics']['safety_out']) + FollowingRobot(ros, profile, q0, '/lowcmd') from golem_upper_ik.core.controller.frame_controller import FrameController controller = FrameController(ros.node('controller'), profile, config, robot_model=model, init=False) @@ -802,9 +792,7 @@ def assert_zero_gain_last(robot, profile, before): (signal.SIGTERM, signal.SIGINT)], ids=['SIGINT', 'SIGTERM', 'SIGINT-twice', 'SIGTERM-SIGINT']) def test_frame_task_server_shuts_down_with_zero_gains_on_a_signal(ros, signals): profile = profile_of('g1') - config = config_of('g1', CONFIGS['g1']) - robot_ = FollowingRobot(ros, profile, body_pose(profile, 'home'), - config['topics']['safety_out']) + robot_ = FollowingRobot(ros, profile, body_pose(profile, 'home'), '/lowcmd') left = PoseRecorder(ros, '/left_ee_pose') process = NodeProcess('frame_task_server', 'robot:=g1', f'config:={CONFIGS["g1"]}') try: diff --git a/core_ws/src/golem_upper_ik/test/test_low_cmd_handler.py b/core_ws/src/golem_upper_ik/test/test_low_cmd_handler.py index 2b4c120a..5df471f0 100644 --- a/core_ws/src/golem_upper_ik/test/test_low_cmd_handler.py +++ b/core_ws/src/golem_upper_ik/test/test_low_cmd_handler.py @@ -68,9 +68,9 @@ def clip(self, q): return np.clip(q, limits[:, 0], limits[:, 1]) def echo(self, **kwargs): - '''An Echo of the commands on the config's safety output''' + '''An Echo of the commands on golem_safety's output (/lowcmd)''' topics = self.config['topics'] - return Echo(self.ros, topics['low_cmd'], topics['safety_out'], **kwargs) + return Echo(self.ros, topics['low_cmd'], '/lowcmd', **kwargs) def command(rig, handler, q): @@ -152,9 +152,7 @@ def test_follow_gate_holds_until_relayed(ros, models, robot): gap = np.abs(upper_q(msg, rig.profile) - rig.measured[rig.upper]) assert np.max(gap) <= tolerance + FLOAT32 - safety_out = rig.config['topics']['safety_out'] - assert safety_out == '/lowcmd' - Echo(ros, rig.config['topics']['low_cmd'], safety_out) + Echo(ros, rig.config['topics']['low_cmd'], '/lowcmd') assert wait_for(lambda: handler.following, 3.0) # open: the commands go where the controller sends them @@ -255,21 +253,20 @@ def test_start_refuses_a_non_finite_pose(ros, models): assert rig.recorder.count == 0 -# (robot, controller config, golem_safety's output, whether the shutdown keeps the held joints) -SHUTDOWNS = [('h12', 'safety_split', '/lowcmd', False), ('h12', 'safety_full', '/lowcmd', False), - ('g1', 'g1_split', '/lowcmd', True)] +# (robot, controller config, whether the shutdown keeps the held joints) +SHUTDOWNS = [('h12', 'safety_split', False), ('h12', 'safety_full', False), + ('g1', 'g1_split', True)] -@pytest.mark.parametrize('robot, config, safety_out, keeps_held', SHUTDOWNS, - ids=[f'{r}-{c}' for r, c, _, _ in SHUTDOWNS]) -def test_shutdown_command_then_silence(ros, models, robot, config, safety_out, keeps_held): +@pytest.mark.parametrize('robot, config, keeps_held', SHUTDOWNS, + ids=[f'{r}-{c}' for r, c, _ in SHUTDOWNS]) +def test_shutdown_command_then_silence(ros, models, robot, config, keeps_held): ''' The IK joints go limp: mode 0, kp, kd and tau 0, q as they were. The held joints too, except on the G1, where a walking policy may be on the legs: there they keep their hold angle, gains and torque ''' rig = Rig(ros, models, robot, config=config, follow_tolerance=0.0) - assert rig.config['topics']['safety_out'] == safety_out handler = rig.handler() handler.start() command(rig, handler, rig.measured) @@ -339,7 +336,6 @@ def test_zero_gain_output_keeps_the_gate_shut(ros, models): def test_g1_split_gate_opens_on_lowcmd(ros, models): rig = Rig(ros, models, 'g1', config='g1_split') - assert rig.config['topics']['safety_out'] == '/lowcmd' handler = rig.handler() handler.start() time.sleep(0.3) diff --git a/core_ws/src/golem_upper_ik/test/test_pairing.py b/core_ws/src/golem_upper_ik/test/test_pairing.py index e817d651..459f7ce7 100644 --- a/core_ws/src/golem_upper_ik/test/test_pairing.py +++ b/core_ws/src/golem_upper_ik/test/test_pairing.py @@ -1,12 +1,9 @@ ''' Which safety and controller configs may run together, and what the pairs agree on. -The G1's verdicts are compared with the previous G1 arm controller's config_pairing, through -test/fixtures/old_controller/g1_pairing.json (test/fixtures/README.md); this test imports no -old package. The pairs' topics and gain limits are read from golem_safety's installed configs. +The pairs' topics and gain limits are read from golem_safety's installed configs. ''' import inspect -import json from pathlib import Path import numpy as np @@ -14,8 +11,6 @@ from golem_upper_ik.pairing import default_controller_config, pairing_refusal -FIXTURE = Path(__file__).parent / 'fixtures' / 'old_controller' / 'g1_pairing.json' - H12_PAIRS = { 'relax_safety_split': 'safety_split', 'default_safety_split': 'safety_split', @@ -30,41 +25,6 @@ SIMS = (None, False, True) -def old_cases(): - return json.loads(FIXTURE.read_text()) - - -def test_g1_pairing_matches_old_rules(): - cases = old_cases()['cases'] - assert len(cases) == 147 - for case in cases: - refusal = pairing_refusal('g1', case['controller'], case['safety'], sim=case['sim']) - if case['verdict'] == 'accepted': - assert refusal is None, case - else: - assert case['verdict'] == 'refused', case - assert refusal == case['message'], case - - -def test_g1_default_controller_config_matches_old(): - defaults = old_cases()['default_controller_config'] - assert len(defaults) == 7 - for safety, controller in defaults.items(): - assert default_controller_config('g1', safety) == controller, safety - - -def test_g1_arm_sdk_configs_refused(): - '''golem_safety has no arm_sdk configs: they relay no upper-body channel, pair with none''' - refusal = pairing_refusal('g1', 'g1', 'arm_sdk', sim=False) - assert refusal == ('safety config "arm_sdk" must relay the upper-body channel: ' - 'one of split, sim_split') - refusal = pairing_refusal('g1', 'sim', 'sim_arm_sdk', sim=True) - assert refusal == ('safety config "sim_arm_sdk" must relay the upper-body channel: ' - 'one of split, sim_split') - for safety in ('arm_sdk', 'sim_arm_sdk'): - assert default_controller_config('g1', safety) is None - - def test_g1_pairs(): '''split runs with g1_split and sim_split with sim_split; sim and real configs never mix''' from golem_upper_ik import pairing @@ -169,12 +129,12 @@ def paired_configs(robot): @pytest.mark.parametrize('robot', ['h12', 'g1']) def test_paired_configs_agree_on_the_topics(robot): - from golem_upper_ik.config import load_controller_config + from golem_upper_ik.config import SAFETY_OUTPUT_TOPIC, load_controller_config for safety, controller in paired_configs(robot).items(): topics = safety_config(robot, safety)['topics'] config = load_controller_config(robot, controller) - # the follow gate reads the topic the safety config writes, by content, not by name - assert config['topics']['safety_out'] == topics['low_cmd_out'], (safety, controller) + # the follow gate reads the topic the safety config writes + assert SAFETY_OUTPUT_TOPIC == topics['low_cmd_out'], (safety, controller) # and the controller writes an input the safety config reads inputs = [topic for key, topic in topics.items() if key.startswith('low_cmd_') and key != 'low_cmd_out'] diff --git a/core_ws/src/golem_upper_ik/test/test_profile.py b/core_ws/src/golem_upper_ik/test/test_profile.py index bf3baa17..55ca0a73 100644 --- a/core_ws/src/golem_upper_ik/test/test_profile.py +++ b/core_ws/src/golem_upper_ik/test/test_profile.py @@ -1,11 +1,7 @@ ''' -The robot profiles against golem_safety's robot data and the old controllers' tables. - -test/fixtures/old_controller/.json holds the previous arm controllers' tables -(test/fixtures/README.md); these tests import no old package. +The robot profiles against golem_safety's robot data. ''' import dataclasses -import json import os from pathlib import Path import types @@ -15,7 +11,6 @@ import pytest import yaml -FIXTURES = Path(__file__).parent / 'fixtures' / 'old_controller' ROBOTS = ('h12', 'g1') EXPECTED = { @@ -31,7 +26,7 @@ 'urdf': 'h1_2_magpie.urdf', 'urdf_ros': 'h1_2_magpie_ros.urdf', 'collision': ('h1_2_magpie_sphere.urdf', 'h1_2_magpie_sphere_collision.srdf'), - 'shutdown_keeps_held_joints_on_lowcmd': False, + 'shutdown_keeps_held_joints': False, }, 'g1': { 'joints': 29, @@ -45,7 +40,7 @@ 'urdf': 'g1_29dof_with_hand_rev_1_0.urdf', 'urdf_ros': 'g1_29dof_with_hand_rev_1_0_ros.urdf', 'collision': ('g1_sphere.urdf', 'g1_sphere_collision.srdf'), - 'shutdown_keeps_held_joints_on_lowcmd': True, + 'shutdown_keeps_held_joints': True, }, } @@ -55,10 +50,6 @@ def profile_of(robot): return load_profile(robot) -def old_tables(robot): - return json.loads((FIXTURES / f'{robot}.json').read_text()) - - def safety_data(robot): share = Path(get_package_share_directory('golem_safety')) return yaml.safe_load((share / 'robots' / f'{robot}.yaml').read_text()) @@ -79,8 +70,8 @@ def test_profiles_load(robot): assert profile.planner_frames == expected['planner_frames'] assert profile.imu_frame == expected['imu_frame'] assert profile.held_joints == expected['held_joints'] - assert (profile.shutdown_keeps_held_joints_on_lowcmd - is expected['shutdown_keeps_held_joints_on_lowcmd']) + assert (profile.shutdown_keeps_held_joints + is expected['shutdown_keeps_held_joints']) for name in ('q_low', 'q_high', 'dq_max', 'tau_max'): values = getattr(profile, name) assert values.shape == (expected['joints'],), name @@ -138,41 +129,21 @@ def test_profile_body_joints_match_safety_data(robot): @pytest.mark.parametrize('robot', ROBOTS) -def test_profile_matches_old_controller_tables(robot): +def test_held_joints_are_the_upper_joints_the_ik_leaves(robot): profile = profile_of(robot) - old = old_tables(robot) - - assert profile.body_joints == tuple(old['body_joints']) - assert list(profile.upper_slots) == old['upper_body_index'] - assert list(profile.lower_slots) == old['lower_body_index'] - assert [profile.body_joints[i] for i in profile.upper_slots] == old['upper_body_joints'] - assert profile.enabled_joints == tuple(old['enabled_joints']) - # the held joints are the upper-body joints the IK does not solve for + upper = [profile.body_joints[i] for i in profile.upper_slots] assert profile.held_joints == tuple( - joint for joint in old['upper_body_joints'] if joint not in old['enabled_joints']) - assert profile.planner_frames == tuple(old['planner_frame_names']) - if 'ee_frames' in old: - assert profile.ee_frames == tuple(old['ee_frames']) - - limits = old['joint_limits'] - np.testing.assert_array_equal(profile.q_low, [low for low, _ in limits['position']]) - np.testing.assert_array_equal(profile.q_high, [high for _, high in limits['position']]) - np.testing.assert_array_equal(profile.dq_max, limits['velocity']) - np.testing.assert_array_equal(profile.tau_max, limits['effort']) + joint for joint in upper if joint not in profile.enabled_joints) + assert set(profile.enabled_joints) <= set(upper) @pytest.mark.parametrize('robot', ROBOTS) -def test_named_configs_match_old_controller(robot): +def test_named_configs_are_over_the_ik_joints(robot): profile = profile_of(robot) - old = old_tables(robot) - - assert list(profile.named_configs) == list(old['named_configs']) - for name, q in old['named_configs'].items(): - value = profile.named_configs[name] + for name, value in profile.named_configs.items(): assert value.shape == (len(profile.enabled_joints),), name assert value.dtype == np.float64, name - np.testing.assert_array_equal(value, q, err_msg=name) - assert profile.startup_sequence == tuple(old['startup_sequence']) + assert set(profile.startup_sequence) <= set(profile.named_configs) def test_unknown_robot_raises(): diff --git a/core_ws/src/golem_upper_ik/test/test_robot_model.py b/core_ws/src/golem_upper_ik/test/test_robot_model.py index 7fcc057b..5c7ceaae 100644 --- a/core_ws/src/golem_upper_ik/test/test_robot_model.py +++ b/core_ws/src/golem_upper_ik/test/test_robot_model.py @@ -1,27 +1,10 @@ ''' -RobotModel and RobotDynamics against the old controllers, on both robots. - -test/fixtures/fk_.json holds what the robot's previous arm controller's RobotModel -and RobotDynamics computed at 50 seeded random configurations (test/fixtures/README.md); -these tests import no old package. +RobotModel and RobotDynamics, on both robots. ''' -import json -from pathlib import Path - import numpy as np import pinocchio as pin import pytest -FIXTURES = Path(__file__).parent / 'fixtures' -# Pinocchio's own float noise is around 1e-15; the old and new models are built the same way -FK_ATOL = 1e-9 -# The Magpie gripper's finger and rocker hinges are continuous joints, frozen at Pinocchio's -# neutral configuration (angle 0). The old H1-2 model froze them at [cos, sin] = [0, 0], -# which is no rotation at all, so its fingers and rockers (8 links of 50 g) sat elsewhere: -# at the fixture's configurations the gravity torques differ by up to 6.4e-3 N*m. The frames -# before them (the end effectors, the grasp frames, the torso) are bit-identical. The G1 has -# no continuous joint: its dynamics are identical. -GRAVITY_ATOL = {'h12': 1e-2, 'g1': FK_ATOL} # N*m # each arm's seven IK joints, by /lowcmd slot: (left, right) ARM_SLOTS = { 'h12': (range(13, 20), range(20, 27)), @@ -39,94 +22,11 @@ def model_of(robot, **kwargs): return RobotModel(profile_of(robot), **kwargs) -def fixture_of(robot): - return json.loads((FIXTURES / f'fk_{robot}.json').read_text()) - - def fixed_state(q, dq=None, imu_quaternion=None): from golem_upper_ik.core.robot_model import FixedState return FixedState(q, dq=dq, imu_quaternion=imu_quaternion) -def placement(transform): - '''[x, y, z, R row-major], as the fixtures store a frame placement''' - transform = np.asarray(transform) - return np.concatenate([transform[:3, 3], transform[:3, :3].reshape(-1)]) - - -def frames_of(fixture): - return list(dict.fromkeys( - fixture['ee_frames'] + fixture['planner_frames'] + [fixture['imu_frame']])) - - -@pytest.mark.parametrize('robot', ['h12', 'g1']) -def test_fixture_matches_the_profile(robot): - fixture = fixture_of(robot) - profile = profile_of(robot) - assert fixture['urdf'] == Path(profile.urdf_path).name - assert tuple(fixture['body_joints']) == profile.body_joints - assert tuple(fixture['enabled_joints']) == profile.enabled_joints - assert tuple(fixture['ee_frames']) == profile.ee_frames - assert tuple(fixture['planner_frames']) == profile.planner_frames - assert fixture['imu_frame'] == profile.imu_frame - assert len(fixture['q']) == 50 - assert sorted(fixture['frames']) == sorted(frames_of(fixture)) - - -def check_fk(model, fixture): - for index, q in enumerate(np.asarray(fixture['q'])): - for frame in frames_of(fixture): - np.testing.assert_allclose( - placement(model.get_frame_transformation(frame, q)), - fixture['frames'][frame][index], rtol=0, atol=FK_ATOL, - err_msg=f'{frame} at q {index}') - # the IK's model: the IK joints of q, the others frozen at 0 - np.testing.assert_allclose( - placement(model.get_frame_transformation_reduced(frame, q[model.reduced_mask])), - fixture['reduced_frames'][frame][index], rtol=0, atol=FK_ATOL, - err_msg=f'{frame} at q {index}, reduced model') - np.testing.assert_allclose( - model.full_q(q, imu_quat=fixture['imu_quat'][index]), fixture['full_q'][index], - rtol=0, atol=FK_ATOL, err_msg=f'full_q at q {index}') - - -def test_h12_fk_matches_old_controller(models): - check_fk(models('h12'), fixture_of('h12')) - - -def test_g1_fk_matches_old_controller(models): - check_fk(models('g1'), fixture_of('g1')) - - -@pytest.mark.parametrize('robot', ['h12', 'g1']) -def test_dynamics_matches_old_controller(models, robot): - fixture = fixture_of(robot) - model = models(robot) - try: - for index, q in enumerate(np.asarray(fixture['q'])): - model.state_subscriber = fixed_state(q) # the twist is taken at the measured q - for frame in fixture['ee_frames']: - np.testing.assert_allclose( - model.dynamics.compute_frame_twist(frame, np.asarray(fixture['dq'][index])), - fixture['twist'][frame][index], rtol=0, atol=FK_ATOL, - err_msg=f'{frame} twist at q {index}') - np.testing.assert_allclose( - model.dynamics.get_gravity_compensation(q, fixture['imu_quat'][index]), - fixture['gravity'][index], rtol=0, atol=GRAVITY_ATOL[robot], - err_msg=f'gravity at q {index}') - finally: - model.state_subscriber = None - - -def test_h12_collision_matches_old_controller(models): - fixture = fixture_of('h12') - model = models('h12') - verdicts = [bool(model.check_collision_free_reduced(q[model.reduced_mask])) - for q in np.asarray(fixture['q'])] - assert verdicts == fixture['collision_free_reduced'] - assert any(verdicts) and not all(verdicts) - - @pytest.mark.parametrize('robot', ['h12', 'g1']) def test_models_follow_the_profile(models, robot): profile = profile_of(robot) diff --git a/core_ws/src/golem_upper_ik/tools/frame_task_server_rate.py b/core_ws/src/golem_upper_ik/tools/frame_task_server_rate.py index cccd6131..81df740d 100644 --- a/core_ws/src/golem_upper_ik/tools/frame_task_server_rate.py +++ b/core_ws/src/golem_upper_ik/tools/frame_task_server_rate.py @@ -4,7 +4,7 @@ Starts the installed frame_task_server for --robot with --config as a subprocess. This process plays the robot and golem_safety's relay: it publishes /lowstate at 500 Hz, executes every command on /safety/lowcmd_upper_in at once (each slot with kp > 0 jumps to its q) and relays -it on the config's topics.safety_out, and records the receive time of every command. Two +it on golem_safety's output, /lowcmd, and records the receive time of every command. Two windows are measured: the start-up (from the first command to the server's first /left_ee_pose, the start-up moves stepping the IK) and the goals (FrameTask goals that move both end effectors 10 cm forward and back, one after another, for --seconds). Then the @@ -42,7 +42,7 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy # noqa: E402 from unitree_hg.msg import LowCmd, LowState # noqa: E402 -from golem_upper_ik.config import load_controller_config # noqa: E402 +from golem_upper_ik.config import SAFETY_OUTPUT_TOPIC, load_controller_config # noqa: E402 from golem_upper_ik.profile import load_profile # noqa: E402 READY_TIMEOUT_S = 120.0 @@ -72,8 +72,7 @@ def __init__(self, node, profile, config): self._state.imu_state.quaternion = [1.0, 0.0, 0.0, 0.0] self._state_publisher = node.create_publisher(LowState, config['topics']['low_state'], WRITER_QOS) - self._relay_publisher = node.create_publisher(LowCmd, config['topics']['safety_out'], - WRITER_QOS) + self._relay_publisher = node.create_publisher(LowCmd, SAFETY_OUTPUT_TOPIC, WRITER_QOS) node.create_subscription(LowCmd, config['topics']['low_cmd'], self._on_command, RECORD_QOS) self._stop = threading.Event() diff --git a/docs/g1/safety.md b/docs/g1/safety.md index dc7dda51..124396be 100644 --- a/docs/g1/safety.md +++ b/docs/g1/safety.md @@ -122,7 +122,7 @@ the robot used to be. stops publishing. It does so on Ctrl+C or SIGTERM, and when its start-up fails after its first command. The arms get mode 0 and zero kp, kd and tau; the waist keeps its hold angle, gains and torque (the G1 profile's -`shutdown_keeps_held_joints_on_lowcmd`), because the walking policy was +`shutdown_keeps_held_joints`), because the walking policy was trained with a stiff waist and a limp one slumps onto it. `golem_safety` relays the command, so **the arms go limp** while the waist holds and the walking node keeps the legs. That stays so: the quiet upper input keeps that From 5bb205cbfe2b5e55585f043f1d64d7db3e7818a4 Mon Sep 17 00:00:00 2001 From: maxconway Date: Sat, 31 Oct 2026 11:49:57 -0600 Subject: [PATCH 003/179] h12_lowerbody_rl: one leg node; the Mac's GOLEM_LOWERBODY pins lowerbody_controller_node's policy; fix lower-body, lmg and MJPC text Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_01MezUeauFS9tKMgZKJxo1HC --- core_ws/src/deploy_mjpc/CMakeLists.txt | 2 +- .../deploy_mjpc/controller_launcher.py | 32 +-- .../deploy_mjpc/debug_visualizer_node.py | 4 +- .../deploy_mjpc/deploy_mjpc/estimator_node.py | 8 +- .../config/h12/lowerbody_real.yaml | 8 +- .../config/h12/lowerbody_sim.yaml | 3 +- .../golem_bringup/config/h12/mjpc_real.yaml | 8 +- .../golem_bringup/config/h12/mjpc_sim.yaml | 70 +++--- .../launch/h12/h12_sim_mac.launch.py | 69 +++--- .../h12_lowerbody_rl/policy.py | 27 +-- .../h12_lowerbody_rl/policy_manager.py | 14 +- .../h12_lowerbody_rl/scripts/fame_node.py | 195 ----------------- .../scripts/lowerbody_controller_node.py | 28 ++- .../h12_lowerbody_rl/scripts/walking_node.py | 201 ------------------ .../h12_lowerbody_rl/policies/almi/README.md | 8 +- .../h12_lowerbody_rl/policies/almi/almi.yaml | 2 +- .../h12_lowerbody_rl/policies/walk/walk.yaml | 19 +- .../src/h12_lowerbody_rl/reference/README.md | 5 +- core_ws/src/h12_lowerbody_rl/setup.py | 2 - core_ws/src/lmg_deploy/README.md | 9 +- .../lmg_deploy/lmg_deploy/marl_deploy_node.py | 8 +- docker/mac/docker-compose.yml | 14 +- docker/mac/scripts/launch_ros_mac.sh | 18 +- docs/MACOS.md | 6 +- 24 files changed, 165 insertions(+), 595 deletions(-) delete mode 100644 core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/fame_node.py delete mode 100644 core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/walking_node.py diff --git a/core_ws/src/deploy_mjpc/CMakeLists.txt b/core_ws/src/deploy_mjpc/CMakeLists.txt index 0cf711a2..9adbf0ce 100644 --- a/core_ws/src/deploy_mjpc/CMakeLists.txt +++ b/core_ws/src/deploy_mjpc/CMakeLists.txt @@ -71,7 +71,7 @@ if(H12_BUILD_CONTROLLER AND EXISTS "${MJPC_BUILD}" AND unitree_sdk2_FOUND) find_package(Threads REQUIRED) # Includes: mjpc source headers, plus the fetched mujoco + absl headers. - # ABI CRITICAL (found 2026-07-07): the controller MUST compile against the + # ABI CRITICAL: the controller MUST compile against the # SAME mujoco headers the build tree's libmujoco.so + libmjpc.a were built # with -- the _deps/mujoco-src pinned commit. The pip package reports the # same release (3.2.3) but its mjModel/mjData struct layout differs from the diff --git a/core_ws/src/deploy_mjpc/deploy_mjpc/controller_launcher.py b/core_ws/src/deploy_mjpc/deploy_mjpc/controller_launcher.py index d0279d37..6ed0c7b0 100644 --- a/core_ws/src/deploy_mjpc/deploy_mjpc/controller_launcher.py +++ b/core_ws/src/deploy_mjpc/deploy_mjpc/controller_launcher.py @@ -30,11 +30,11 @@ def _band_worker(node: Node, delay_sec: float, stop: threading.Event) -> None: """After delay_sec ON THE NODE CLOCK, call /elastic_band/toggle once. - With use_sim_time (the sim bringup) the node clock follows /clock, so the - delay is measured in SIM seconds -- the clock the robot lives on. The - kitchen sim runs far below 1x real-time, so a wall-clock delay would fire - at the wrong point of the (plant-clocked) bring-up. Without use_sim_time - the node clock is system time and this degrades to the old wall behavior. + With use_sim_time the node clock follows /clock, so the delay is measured + in SIM seconds -- the clock the robot lives on. The kitchen sim runs far + below 1x real-time, so a wall-clock delay would fire at the wrong point of + the (plant-clocked) bring-up. Without use_sim_time the node clock is + system time and the delay is wall seconds. spin_once() here also services the /clock subscription (nothing else spins this node).""" t0 = None @@ -78,8 +78,9 @@ def main() -> None: exe = p("controller_exe", "mjpc_lowerbody_core").value binary = p("controller_binary", os.path.join(_libdir, exe)).value # Same knob set + semantics as the fork's CLI (see mjpc/deploy/ - # h12_lower_body_controller.cc); defaults here are the SIM bringup's -- - # config/mjpc_sim.yaml / mjpc_real.yaml override. + # h12_lower_body_controller.cc). No golem_bringup launch file starts this + # node; golem_bringup's config/h12/mjpc_sim.yaml and mjpc_real.yaml hold + # the sim and real parameter sets to pass it. args = [ binary, "--task", str(p("task", "Stabilize H12 Magpie").value), @@ -94,22 +95,22 @@ def main() -> None: "--ankle_roll_offset_r_deg", str(p("ankle_roll_offset_r_deg", 0.0).value), "--domain_id", str(p("domain_id", int(os.environ.get("ROS_DOMAIN_ID", "0"))).value), - # gRPC monitor is compiled out of the container build; the flag is inert - # there but meaningful on a host build (native monitor attach). + # deploy_mjpc builds the cores without H12_NODE_GRPC, so the monitor + # server is compiled out and this flag is inert here; it hosts the + # monitor only in a fork build that defines H12_NODE_GRPC. "--grpc_port", str(p("grpc_port", 0).value), "--plan_trajectories", str(p("plan_trajectories", 0).value), "--plan_threads", str(p("plan_threads", 0).value), "--stale_sec", str(p("stale_sec", 0.05).value), - # latency-comp sim-time scale = measured RTF. 1.0 (default) is byte-identical - # to before (real/twin run RTF~1); a below-realtime sim (RoboCasa) passes its + # latency-comp sim-time scale = measured RTF. 1.0 (default) applies no + # scaling (real/twin run RTF~1); a below-realtime sim (RoboCasa) passes its # measured RTF so the predict-forward horizon stops over-leading by 1/RTF. "--latency_rtf", str(p("latency_rtf", 1.0).value), # DEBUG plan publish for mjpc_debug_visualizer's blue ghost. Hardcoded, not a # ROS param: there is exactly one right topic and the visualizer defaults to # the same string, so a param could only ever be set WRONG (silently costing - # you the ghost). The core still defaults this OFF -- an empty --plan_topic -- - # so the fork's binary, the full-body core and the upper-body core are all - # byte-unchanged; this launcher is the only thing that arms it. + # you the ghost). The core defaults this OFF -- an empty --plan_topic -- + # so only runs through this launcher publish the plan. # The publish happens on the PLANNER thread after PlanIteration returns, so it # costs the 200 Hz control loop nothing. ROS sees it as /mjpc/plan. "--plan_topic", "rt/mjpc/plan", @@ -118,8 +119,7 @@ def main() -> None: # Passed unconditionally with their defaults visible here (edit in place # or override with -p). Unlike --arm_aware below, BOTH cores define all # three, so no has_* guard is needed and mjpc_fullbody_core won't abort. - # Defaults reproduce the binary's own defaults exactly -> byte-identical - # to not passing them at all. + # The defaults are the binary's own, so passing them changes nothing. # # straighten_start: hold the measured (slumped) pose, wait for ENTER on # the core's STDIN, then hand authority to the planner (SETTLE->BLEND). diff --git a/core_ws/src/deploy_mjpc/deploy_mjpc/debug_visualizer_node.py b/core_ws/src/deploy_mjpc/deploy_mjpc/debug_visualizer_node.py index a28c44b1..21b81d96 100755 --- a/core_ws/src/deploy_mjpc/deploy_mjpc/debug_visualizer_node.py +++ b/core_ws/src/deploy_mjpc/deploy_mjpc/debug_visualizer_node.py @@ -18,8 +18,8 @@ DEBUG ONLY: nothing in the control path reads this node, and it consumes no ground truth -- only rt/lowstate, the estimator's estimate, and the planner's own plan. It -is a no-op unless the controller core was started with --plan_topic (the sim -launcher passes it; the real robot does not publish plans by default). +is a no-op unless the controller core was started with --plan_topic +(controller_launcher.py always passes it; the bare core does not). """ import os diff --git a/core_ws/src/deploy_mjpc/deploy_mjpc/estimator_node.py b/core_ws/src/deploy_mjpc/deploy_mjpc/estimator_node.py index f6c5ec83..9ac84cd2 100644 --- a/core_ws/src/deploy_mjpc/deploy_mjpc/estimator_node.py +++ b/core_ws/src/deploy_mjpc/deploy_mjpc/estimator_node.py @@ -10,11 +10,9 @@ stays the single source of truth, and the estimator's unitree_sdk2py DDS I/O (one-writer + iface auto-pinning) is deliberately NOT converted to rclpy pub/sub. -Replaces the earlier stripped port (velocity-only leg odometry, hardcoded -covariances, never read the accelerometer) — see git history. That version's -"H3 hard contact gate" (drop swing feet entirely) is NOT carried over: the fork -filter handles outliers via load-scaled measurement noise + chi2 rejection; -bench H3 on the twin before considering re-adding it. +There is no hard contact gate (dropping swing feet entirely): the fork filter +handles outliers via load-scaled measurement noise + chi2 rejection. Bench a +gate on the twin before adding one. Contract: the estimator's IMU_OFFSET must equal the controller core's kImuOffset (both are {-0.04452, -0.01891, 0.27756}; both ends of the IMU-site -> pelvis diff --git a/core_ws/src/golem_bringup/config/h12/lowerbody_real.yaml b/core_ws/src/golem_bringup/config/h12/lowerbody_real.yaml index ba34567a..9746d335 100644 --- a/core_ws/src/golem_bringup/config/h12/lowerbody_real.yaml +++ b/core_ws/src/golem_bringup/config/h12/lowerbody_real.yaml @@ -4,11 +4,9 @@ # ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger lowerbody_controller_node: ros__parameters: - # The elastic band is a sim fixture; on real there is no - # /elastic_band/toggle service, so the release path only costs a 1 s - # blocking wait_for_service inside the engage tick (first policy command - # then computed from 1 s-stale state) plus a redundant second LSTM reset. - # The operator lowers the gantry instead. + # The elastic band is a sim fixture; the real robot has no + # /elastic_band/toggle service. false means there is no band to release: + # the node never calls the service, and the operator lowers the gantry. disable_elastic_band: false # Startup seed for the live IMU trim (adjust during a run with # rqt_reconfigure or ros2 param set). Negative pitch is an anti-lean bias diff --git a/core_ws/src/golem_bringup/config/h12/lowerbody_sim.yaml b/core_ws/src/golem_bringup/config/h12/lowerbody_sim.yaml index 7a70f1ff..785f4416 100644 --- a/core_ws/src/golem_bringup/config/h12/lowerbody_sim.yaml +++ b/core_ws/src/golem_bringup/config/h12/lowerbody_sim.yaml @@ -1,5 +1,6 @@ # lowerbody_controller_node parameters for the simulators. active_policy comes -# from h12_lowerbody.launch.py's lowerbody:= launch argument. +# from h12_lowerbody.launch.py's lowerbody:= launch argument, or on the Mac from +# GOLEM_LOWERBODY (h12_sim_mac.launch.py). lowerbody_controller_node: ros__parameters: # Sim auto-engages after the pre-pose settles (unattended runs); the diff --git a/core_ws/src/golem_bringup/config/h12/mjpc_real.yaml b/core_ws/src/golem_bringup/config/h12/mjpc_real.yaml index 98a0f650..3dc2a25a 100644 --- a/core_ws/src/golem_bringup/config/h12/mjpc_real.yaml +++ b/core_ws/src/golem_bringup/config/h12/mjpc_real.yaml @@ -31,8 +31,8 @@ mjpc_deploy_lowerbody_controller: imu_pitch_offset_deg: 1.6 arm_aware: true drop_band: false # no elastic band outside sim - # Planner rollout threadpool size. Matches the 12 P-cores (0-11) the desktop - # bringup pins this controller to via taskset; default 0 would spawn one - # thread per hardware CPU (28) and oversubscribe the reserved 12. Drop to - # ~10 if the RW-EKF estimator (pinned to core 12) starves under load. + # Planner rollout threadpool size, sized for 12 P-cores. Nothing in this + # repo pins the controller or the estimator to CPUs; default 0 would spawn + # one thread per hardware CPU (28 on the desktop). Drop to ~10 if the + # RW-EKF estimator starves under load. plan_threads: 12 diff --git a/core_ws/src/golem_bringup/config/h12/mjpc_sim.yaml b/core_ws/src/golem_bringup/config/h12/mjpc_sim.yaml index fb23ff04..37c0a361 100644 --- a/core_ws/src/golem_bringup/config/h12/mjpc_sim.yaml +++ b/core_ws/src/golem_bringup/config/h12/mjpc_sim.yaml @@ -1,4 +1,6 @@ -# MJPC lower-body deploy — RoboCasa SIM params (h12_sim.launch.py). +# MJPC lower-body deploy — RoboCasa SIM params. No golem_bringup launch file +# loads this file: pass it to the deploy_mjpc nodes by hand (h12_lowerbody's +# estimator_params defaults to mjpc_real.yaml). # Controller + estimator are the fork's deploy core # and RW-EKF base estimator behind the deploy_mjpc shims; these params # mirror the fork's surviving CLI flags 1:1. Everything settled (ctrl 200 Hz, @@ -19,11 +21,11 @@ deploy_mjpc_estimator: # motion, feeding the planner an inconsistent (pos, vel) pair that diverges # laterally within seconds of policy authority. REAL runs at 1x, where wall # == plant time, so mjpc_real.yaml leaves this at 0. - tick_dt: 0.005 # must match the plant's --sim-dt + tick_dt: 0.005 # must match the plant's model.opt.timestep (see twin_dt) mjpc_debug_visualizer: ros__parameters: - # DEBUG ONLY (use_mjpc_viz:=false to drop it). Renders the MEASURED robot in its + # DEBUG ONLY. Renders the MEASURED robot in its # normal colours + a semi-transparent BLUE GHOST at the planner's current best # trajectory, and writes the mp4 when the node is signalled. Commands nothing; # subscribes only to rt/lowstate, the estimator's rt/sportmodestate_est, and @@ -86,40 +88,41 @@ mjpc_deploy_lowerbody_controller: # the planner half and requires staying slow, which leaves the latency # compensator (deploy_common.cc, wall-clock dt predicted as sim-seconds) # over-leading by 1/RTF -> controller<->plant overshoot. So run like real - # instead: 12 threads with RTF as near 1.0 as the box allows (HEADLESS, - # --no-sensors, no slowmo -> ~0.85, giving ~65 iters/sim-s against real's - # ~55 and only 1.18x latency over-lead). Do NOT slow the sim down; that + # instead: 12 threads with RTF as near 1.0 as the box allows (headless, + # cameras off via GOLEM_CAMERAS=0 -> ~0.85, giving ~65 iters/sim-s + # against real's ~55 and only 1.18x latency over-lead). Do NOT slow the sim down; that # breaks both halves. If 12 threads oversubscribe the machine and tank RTF # (check /clock hz), drop to ~10. plan_threads: 12 - # Full gravity feed-forward, with the 0.67 kg back_equipment ballast and - # rigid feet (--rigid-feet, default ON in h12_mujoco.py). Rigid feet - # eliminate the topple — foot-slip is its dominant driver — leaving a - # vertical sink as the residual, and 1.0 holds height (z ~0.92 over 40 s) - # where 0.85 sinks to z 0.64: unlike the twin, this plant has no leg - # gravcomp, only the two hands. The remaining 5-12 deg tilt wobble is not a - # free stand; the next parity lever is the contact/integrator mismatch + # Full gravity feed-forward, tuned on a plant with the 0.67 kg + # back_equipment ballast and rigid feet (foot-slip is the topple's dominant + # driver; the H1-2 RoboCasa sim has no rigid-feet option). 1.0 holds height + # (z ~0.92 over 40 s) where 0.85 sinks to z 0.64: unlike the twin, this + # plant has no leg gravcomp, only the two hands. The remaining 5-12 deg + # tilt wobble is not a free stand; the next parity lever is the + # contact/integrator mismatch # (RoboCasa runs impratio=20 / Euler / elliptic against the twin+planner's # 100 / implicitfast / pyramidal, and the task XML warns Euler diverges on # biped balance). REAL keeps 0.85 (mjpc_real.yaml). gravity_ff: 1.0 # MUST equal RoboCasa's model.opt.timestep: the sim publishes rt/lowstate # from a RecurrentThread at exactly that interval, and the core's planner - # clock is lowstate tick * twin_dt. The plant is run at the twin's dt - # (--sim-dt 0.005) because the deploy-chain stand is dt-sensitive: it hangs - # at 0.002 and stands at 0.005 even down at RTF 0.5. + # clock is lowstate tick * twin_dt. 0.005 is the twin's dt; the + # deploy-chain stand is dt-sensitive (it hangs at 0.002 and stands at 0.005 + # even down at RTF 0.5). The H1-2 RoboCasa sim takes no flag to set its + # timestep: check model.opt.timestep before running with this value. twin_dt: 0.005 # REAL-PARITY STATE SOURCE: the RW-EKF estimate on rt/sportmodestate_est is - # the SAME signal path the real robot uses (the bringup already launches the - # estimator). Its filtered velocity gives a quieter stand (tilt 3-5° steady) - # than raw ground truth, whose full-bandwidth base velocity the planner - # over-reacts to; the 30 ms LPF matches real. Ground truth is not the missing - # piece — feeding rt/sportmodestate (plant --truth-sportstate, verified at - # 191 Hz) does not stop the fall, it only turns the lateral fold into a - # sagittal one. The EKF's leg-odometry xy wanders ~0.3 m under sim foot - # micro-slip, which is irrelevant for STANDING (balance = orientation + base - # velocity + CoM). Point this at rt/sportmodestate (+ sim - # --truth-sportstate) only to A/B raw truth. + # the SAME signal path the real robot uses (deploy_mjpc_estimator above + # publishes it; h12_lowerbody's use_estimator starts it). Its filtered + # velocity gives a quieter stand (tilt 3-5° steady) than raw ground truth, + # whose full-bandwidth base velocity the planner over-reacts to; the 30 ms + # LPF matches real. Ground truth is not the missing piece — a ground-truth + # rt/sportmodestate does not stop the fall, it only turns the lateral fold + # into a sagittal one. The EKF's leg-odometry xy wanders ~0.3 m under sim + # foot micro-slip, which is irrelevant for STANDING (balance = orientation + + # base velocity + CoM). The H1-2 RoboCasa sim publishes no + # rt/sportmodestate. sportstate_topic: rt/sportmodestate_est imu_pitch_offset_deg: 0.0 # 1.6 is a REAL-robot IMU mount calibration; sim has none arm_aware: true # legs pre-compensate for measured arm/torso motion @@ -135,13 +138,12 @@ mjpc_deploy_lowerbody_controller: # plant-clocked, so it pauses during each freeze rather than racing ahead. # REAL stays at the compiled 0.05 (mjpc_real.yaml doesn't set this). stale_sec: 1.5 - # Band release is owned by the SIM's slack-triggered auto-release - # (h12_mujoco --band-auto-release: |band force| <= 30 N sustained 2 sim-s, - # the twin harness "handoff-timing" doctrine -- a self-supporting robot - # leaves the band slack). Keep the launcher's TIMER release OFF: the - # /elastic_band/toggle service is a stateful TOGGLE, so firing it after - # the sim already auto-released turns the band back ON. A timer release is - # also dishonest: it fires mid-sway and dumps the robot from whatever - # posture the tether wound it into. + # The H1-2 RoboCasa band stays on until /elastic_band/toggle is called (or + # SPACE in the viewer); the sim has no slack-triggered auto-release. With + # drop_band false the launcher leaves the band alone, so release it by + # hand once the robot supports itself. drop_band true has the launcher + # toggle it band_drop_after_secs (sim s) after start, whatever the robot's + # posture: a timer release can fire mid-sway, and the service is a + # stateful TOGGLE, so a second call turns the band back on. drop_band: false band_drop_after_secs: 25.0 diff --git a/core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py index 63a5ee7e..d19b4f5a 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py @@ -1,18 +1,20 @@ """ Minimal, CPU-only (Apple Silicon) bringup for the H1 MuJoCo sim. -A trimmed variant of h12_sim.launch.py that runs ONLY the core robot -nodes — no vision foundation models (gemini/sam/graspgen), no golem_skills, no -Nav2/SLAM, no rviz/sliders. Those need the heavy x86-tuned ML stack that the -slim arm64 ROS image intentionally omits. +A trimmed variant of h12_sim.launch.py that runs the core robot nodes — no +vision foundation models (gemini/sam/graspgen), no golem_skills, no FAST-LIO. +Those need the heavy x86-tuned ML stack that the slim arm64 ROS image +intentionally omits. RViz runs outside this launch file (launch_ros_mac.sh, +GOLEM_RVIZ=vnc). Nodes started: - * static camera_link -> camera_color_optical_frame TF * static livox_link -> lidar_link TF (so RViz can place /livox/pointcloud) * joint_state_publisher (golem_upper_ik) * robot_state_publisher * frame_task_server (golem_upper_ik, Pink IK) * safety_node (golem_safety, sim:=true) + * lowerbody_controller_node (h12_lowerbody_rl; GOLEM_LOWERBODY) + * pointcloud_to_laserscan + slam_toolbox (GOLEM_SLAM), nav2 (GOLEM_NAV2) Launch by path from the slim ROS container: ros2 launch /home/code/core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py @@ -35,17 +37,6 @@ def generate_launch_description(): sim_time_param = {'use_sim_time': True} nodes = [ - Node( - package='tf2_ros', - executable='static_transform_publisher', - name='camera_optical_frame_broadcaster', - arguments=['0', '0', '0', - '-1.5707963267948966', '0', '-1.5707963267948966', - 'camera_link', 'camera_color_optical_frame'], - parameters=[sim_time_param], - output='screen', - emulate_tty=True, - ), # The MuJoCo lidar bridge stamps /livox/{lidar,pointcloud} with frame_id # 'lidar_link', which is co-located with the URDF's 'livox_link' mount but # is not itself a URDF link. Publish the identity bridge so RViz (and any @@ -94,30 +85,28 @@ def generate_launch_description(): ), ] - # Optional lower-body locomotion controller (GOLEM_LOWERBODY). Off by default: - # the elastic-band tether then holds the robot upright. The node self-sequences - # its band release (waits for the IK to finish) and publishes leg setpoints on + # Optional lower-body controller (GOLEM_LOWERBODY). Off by default: the + # elastic-band tether then holds the robot upright. Set, it starts + # lowerbody_controller_node pinned to that policy with the x86 sim's + # parameters (h12_lowerbody.launch.py, lowerbody_sim.yaml): it waits for + # /safety/heartbeat, pre-poses the legs, engages the policy, releases the + # band once frame_task_server is ready, and publishes leg setpoints on # /safety/lowcmd_lower_in for the safety_node to merge. - # GOLEM_LOWERBODY=fame RMA standing/squatting policy — balances the robot - # standing UNSUPPORTED (verified). Does not locomote. - # GOLEM_LOWERBODY=walk TorchScript walk policy on its own (marches in place; - # topples in the cluttered kitchen). - # GOLEM_LOWERBODY=almi lower-body controller pinned to ALMI: stands, and - # walks on /cmd_vel. This is the one to use for the nav - # demo — nav2's /cmd_vel makes it walk; it stands again - # when the command drops to ~0. + # GOLEM_LOWERBODY=fame RMA standing/squatting policy. Does not locomote. + # GOLEM_LOWERBODY=walk TorchScript walk policy (follows /cmd_vel). + # GOLEM_LOWERBODY=almi ALMI: stands, and walks on /cmd_vel. This is the one + # to use for the nav demo — nav2's /cmd_vel makes it + # walk; it stands again when the command drops to ~0. lowerbody = os.environ.get('GOLEM_LOWERBODY', '').strip().lower() - _lowerbody_exec = {'fame': 'fame_node', 'walk': 'walking_node', - 'almi': 'lowerbody_controller_node'}.get(lowerbody) - if _lowerbody_exec: - _lb_params = [sim_time_param] - if _lowerbody_exec == 'lowerbody_controller_node': - _lb_params.append({'active_policy': 'almi'}) + if lowerbody in ('fame', 'walk', 'almi'): nodes.append(Node( package='h12_lowerbody_rl', - executable=_lowerbody_exec, - name=_lowerbody_exec, - parameters=_lb_params, + executable='lowerbody_controller_node', + name='lowerbody_controller_node', # MUST match the yaml key + parameters=[sim_time_param, + os.path.join(get_package_share_directory('golem_bringup'), + 'config', 'h12', 'lowerbody_sim.yaml'), + {'active_policy': lowerbody}], output='screen', emulate_tty=True, )) @@ -171,14 +160,16 @@ def generate_launch_description(): # Optional autonomous navigation (GOLEM_NAV2=1, implies SLAM for the map): the # nav2 stack plans a collision-free path on the SLAM map + lidar costmap and - # drives it via /cmd_vel, which the walk policy consumes (nav2's + # drives it via /cmd_vel, which the lower-body policy follows (nav2's # velocity_smoother publishes /cmd_vel). Send goals with the RViz "2D Nav Goal" # tool or the /navigate_to_pose action. With GOLEM_LOWERBODY=almi the robot # walks as soon as nav2 publishes /cmd_vel (no manual step). The # robot must be standing in open floor — nav2 rejects a goal if the robot's start # cell is occupied (e.g. pressed against a counter). - # nav2_config_mac.yaml is nav2_config.yaml with the odom frame swapped from - # FAST-LIO's camera_init to the sim's odom. + # nav2_config_mac.yaml is its own nav2 config for this SLAM-only setup: + # robot_base_frame pelvis (no base_footprint here), Regulated Pure Pursuit + # and the Smac Hybrid planner, costmaps built from slam_toolbox's /map plus + # the live scan and Livox cloud, and tighter goal tolerances; see the file. if os.environ.get('GOLEM_NAV2', '').strip().lower() in ('1', 'true', 'on'): nav2_launch = os.path.join( get_package_share_directory('nav2_bringup'), 'launch', 'navigation_launch.py') diff --git a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/policy.py b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/policy.py index 09cf840e..c5b826b9 100644 --- a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/policy.py +++ b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/policy.py @@ -3,11 +3,11 @@ A ``Policy`` turns the current robot state into a 12-joint lower-body PD setpoint (``LegCommand``). Each policy is fully self-describing — it owns its observation layout, scales, PD gains, and nominal pose, all loaded from its own YAML — so the -controller node can run any of them through the same loop and switch between them. +controller node runs whichever one is pinned through the same loop. Three concrete policies are provided: -* ``WalkPolicy`` — the original TorchScript walking policy (47-d obs, gait clock). +* ``WalkPolicy`` — a TorchScript walking policy (47-d obs, gait clock). * ``FamePolicy`` — the RMA standing/squatting policy: an env-factor encoder (e_t -> z_t) plus a base policy over a history of proprio + z_t (252-d actor obs). It reads all 27 joints (legs + torso + arms) but only commands the 12 legs; the @@ -27,7 +27,7 @@ import collections import os from abc import ABC, abstractmethod -from dataclasses import dataclass, field +from dataclasses import dataclass import numpy as np import torch @@ -35,7 +35,6 @@ NUM_POLICY_JOINTS = 27 # legs(12) + torso(1) + left arm(7) + right arm(7) NUM_LEG_JOINTS = 12 -NUM_UPPER_JOINTS = 15 # torso + arms, indices 12:27 # --------------------------------------------------------------------------- # @@ -181,14 +180,13 @@ def _clip_leg_positions( # Policy interface # --------------------------------------------------------------------------- # class Policy(ABC): - """A swappable lower-body controller. Stateful (keeps history); reset on switch.""" + """A lower-body controller. Stateful (keeps history); reset when it engages.""" name: str - #: 12 leg default angles, in /lowstate order — the nominal pose used by the - #: handover gate to decide a switch is safe, and to pre-pose the legs before - #: the policy is engaged. ``_kps``/``_kds`` are the leg PD gains each concrete - #: policy loads from its YAML. + #: 12 leg default angles, in /lowstate order — the nominal pose the controller + #: pre-poses the legs to before the policy is engaged. ``_kps``/``_kds`` are + #: the leg PD gains each concrete policy loads from its YAML. nominal_lower: np.ndarray _kps: np.ndarray _kds: np.ndarray @@ -215,7 +213,7 @@ def nominal_command(self) -> LegCommand: # --------------------------------------------------------------------------- # -# Walking policy (original) +# Walking policy # --------------------------------------------------------------------------- # class WalkPolicy(Policy): name = "walk" @@ -226,10 +224,7 @@ def __init__(self, config_path: str): cfg_dir = os.path.dirname(os.path.abspath(config_path)) policy_path = cfg.get("policy_path", "walkingPolicy.pt") if not os.path.isabs(policy_path): - # walk.yaml's policy_path points at a training tree; fall back to the - # weight shipped alongside the config. - local = os.path.join(cfg_dir, "walkingPolicy.pt") - policy_path = local if os.path.isfile(local) else os.path.join(cfg_dir, policy_path) + policy_path = os.path.join(cfg_dir, policy_path) self._kps = np.asarray(cfg["kps"], dtype=np.float32) self._kds = np.asarray(cfg["kds"], dtype=np.float32) @@ -301,7 +296,7 @@ class AlmiPolicy(Policy): observations are forced to 0 and the policy balances in place — it does not march at zero command the way WalkPolicy does. - Stand->walk onset resets the LSTM (verified in RoboCasa 2026-07-19): the + Stand->walk onset resets the LSTM (checked in RoboCasa): the policy only enters locomotion if the command is already present when its recurrent state starts — in the upstream rollouts every episode begins with the command applied from tick 0, so "quiet stand, then a command appears" @@ -485,7 +480,7 @@ def _resolve(key, default): self._policy.eval() self._encoder = None - enc_path = _resolve("encoder_path", "encoder_3800.pt") + enc_path = _resolve("encoder_path", "encoder_3600.pt") if os.path.isfile(enc_path): from h12_lowerbody_rl.rma import EnvFactorEncoder, EnvFactorEncoderCfg diff --git a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/policy_manager.py b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/policy_manager.py index ff075360..13d124ce 100644 --- a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/policy_manager.py +++ b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/policy_manager.py @@ -3,8 +3,8 @@ The manager holds the lower-body policies and at most one *active* policy. ``active_name`` is ``None`` when **idle**: the robot is held by the elastic band and no policy drives the legs. A requested policy becomes active once the node -releases the band and calls ``commit``, which ``reset()``s it for a clean -free-standing warm-up. The band itself is owned by the node, not here. +has pre-posed the legs and calls ``commit``, which ``reset()``s it for a clean +warm-up. The band itself is owned by the node, not here. """ from __future__ import annotations @@ -37,17 +37,14 @@ def is_pending(self) -> bool: def names(self) -> list[str]: return list(self._policies) - def has(self, name: str) -> bool: - return name in self._policies - def desired_policy(self) -> Policy | None: """The pending (requested but not yet committed) policy, or None if idle.""" return self._policies.get(self._desired) if self._desired is not None else None # -- control ------------------------------------------------------------- def request(self, name: str) -> tuple[bool, str]: - """Request a policy. Returns (accepted, message). It is committed later, - after band release.""" + """Request a policy. Returns (accepted, message). The node commits it + once the legs are pre-posed.""" if name not in self._policies: return False, f"unknown policy {name!r}; have {self.names()}" if name == self._active and not self.is_pending(): @@ -59,8 +56,7 @@ def request(self, name: str) -> tuple[bool, str]: return True, f"requested {name!r}" def commit(self, state: RobotState) -> str: - """Activate the desired policy now (reset for a clean warm-up). Used for - the first activation right after band release.""" + """Activate the desired policy now (reset for a clean warm-up).""" incoming = self._policies[self._desired] incoming.reset(state) old = self._active diff --git a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/fame_node.py b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/fame_node.py deleted file mode 100644 index f9f73127..00000000 --- a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/fame_node.py +++ /dev/null @@ -1,195 +0,0 @@ -"""FAME standing/squatting policy ROS 2 node. - -Standalone single-policy node — the ROS adaptation of -``reference/mujoco_deploy_h12_rma.py``, mirroring ``walking_node`` for the -walking policy. Runs the RMA FAME policy against ``/lowstate`` and publishes -12-joint lower-body PD setpoints on ``/safety/lowcmd_lower_in`` for golem_safety to merge -with upper-body commands. - -It controls only the 12 legs; the torso + arms are driven by the upper-body IK -and are merely *observed* here (the encoder adapts the legs to them). Reuses -``FamePolicy`` so the RMA observation/encoder math has a single source of truth; -the generic controller is ``lowerbody_controller_node``. - -Interfaces ----------- -sub /lowstate (unitree_hg/LowState) robot state -sub /cmd_vel (geometry_msgs/Twist) velocity command -sub /lowerbody/squat_cmd (std_msgs/Float32) base-height / squat command -pub /safety/lowcmd_lower_in (unitree_hg/LowCmd) 12-joint leg setpoints -""" - -import os -import time - -import numpy as np -import rclpy -from ament_index_python.packages import get_package_share_directory -from geometry_msgs.msg import PoseStamped, Twist -from rclpy.node import Node -from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy -from std_msgs.msg import Float32 -from std_srvs.srv import Trigger -from unitree_hg.msg import LowCmd, LowState - -from h12_lowerbody_rl.policy import ( - NUM_LEG_JOINTS, - NUM_POLICY_JOINTS, - FamePolicy, - RobotState, -) - -MOTOR_MODE_PR = 1 - - -def _default_fame_config() -> str: - return os.path.join( - get_package_share_directory("h12_lowerbody_rl"), - "policies", "fame", "fame.yaml", - ) - - -class FameNode(Node): - def __init__(self): - super().__init__("fame_node") - - self.declare_parameter("config_path", _default_fame_config()) - self.declare_parameter("control_hz", 50.0) - self.declare_parameter("default_height_cmd", 1.0) - self.declare_parameter("disable_elastic_band", True) - # Release the band only after frame_task_server finishes its open-loop - # init (it publishes /left_ee_pose only then); releasing earlier crashes - # frame_task (base moves mid-init) and free-falls the robot. - self.declare_parameter("band_wait_for_frame_task", True) - self.declare_parameter("band_release_topic", "/left_ee_pose") - self.declare_parameter("band_max_wait", 30.0) - - config_path = self.get_parameter("config_path").get_parameter_value().string_value - control_hz = self.get_parameter("control_hz").get_parameter_value().double_value - self._height_cmd = float(self.get_parameter("default_height_cmd").value) - - self._policy = FamePolicy(config_path) - if not self._policy.has_encoder: - self.get_logger().warn( - "FAME encoder not loaded — z_t will be zeros (out-of-distribution). " - "Check the encoder weight referenced by the config." - ) - - self._lowstate: LowState | None = None - self._cmd = np.zeros(3, dtype=np.float32) - - lowstate_qos = QoSProfile( - reliability=ReliabilityPolicy.BEST_EFFORT, - history=HistoryPolicy.KEEP_LAST, - depth=1, - ) - self.create_subscription(LowState, "/lowstate", self._on_lowstate, lowstate_qos) - self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10) - self.create_subscription(Float32, "/lowerbody/squat_cmd", self._on_squat_cmd, 10) - self._cmd_pub = self.create_publisher(LowCmd, "/safety/lowcmd_lower_in", 10) - - self.create_timer(1.0 / float(control_hz), self._tick) - self.get_logger().info( - f"fame_node ready: config={config_path}, control_hz={control_hz}, " - f"encoder={'on' if self._policy.has_encoder else 'OFF'}" - ) - - self._band_released = not self.get_parameter("disable_elastic_band").value - self._band_cli = self.create_client(Trigger, "/elastic_band/toggle") - self._band_wait_start = time.monotonic() - self._pending_policy_reset = False # reset FAME warm-up free-standing at band release - if not self._band_released: - if self.get_parameter("band_wait_for_frame_task").value: - topic = self.get_parameter("band_release_topic").value - self.create_subscription(PoseStamped, topic, self._on_frame_task_ready, 10) - self.get_logger().info( - f"waiting for frame_task ready ({topic}) before releasing elastic band" - ) - else: - self._release_band("disable_elastic_band (no wait)") - - def _on_lowstate(self, msg: LowState) -> None: - self._lowstate = msg - - def _on_cmd_vel(self, msg: Twist) -> None: - self._cmd[0] = msg.linear.x - self._cmd[1] = msg.linear.y - self._cmd[2] = msg.angular.z - np.clip(self._cmd, -1.0, 1.0, out=self._cmd) - - def _on_squat_cmd(self, msg: Float32) -> None: - self._height_cmd = float(msg.data) - - def _on_frame_task_ready(self, _msg: PoseStamped) -> None: - if not self._band_released: - self._release_band("frame_task ready (ee_pose received)") - - def _release_band(self, reason: str) -> None: - if self._band_released: - return - self._band_released = True - self._pending_policy_reset = True # restart FAME warm-up free-standing - if not self._band_cli.service_is_ready() and not self._band_cli.wait_for_service(timeout_sec=1.0): - self.get_logger().warn("elastic band toggle service unavailable — band NOT released") - return - self.get_logger().info(f"releasing elastic band ({reason})") - fut = self._band_cli.call_async(Trigger.Request()) - fut.add_done_callback( - lambda f: self.get_logger().info( - f"elastic band toggle: {f.result().message}" if f.result() else "elastic band toggle call failed" - ) - ) - - def _tick(self) -> None: - if not self._band_released and \ - (time.monotonic() - self._band_wait_start) > self.get_parameter("band_max_wait").value: - self.get_logger().warn("upper-body command not seen within band_max_wait — releasing band anyway") - self._release_band("band_max_wait timeout") - - if self._lowstate is None: - return - msg = self._lowstate - state = RobotState( - q=np.array([msg.motor_state[i].q for i in range(NUM_POLICY_JOINTS)], dtype=np.float32), - dq=np.array([msg.motor_state[i].dq for i in range(NUM_POLICY_JOINTS)], dtype=np.float32), - quat=np.asarray(msg.imu_state.quaternion, dtype=np.float32), - gyro=np.asarray(msg.imu_state.gyroscope, dtype=np.float32), - cmd=self._cmd.copy(), - height_cmd=self._height_cmd, - t=self.get_clock().now().nanoseconds * 1e-9, - ) - - if self._pending_policy_reset: - self._policy.reset(state) - self._pending_policy_reset = False - self.get_logger().info("reset FAME for fresh free-standing warm-up (band released)") - - leg = self._policy.compute(state) - - cmd_msg = LowCmd() - for i in range(NUM_LEG_JOINTS): - m = cmd_msg.motor_cmd[i] - m.mode = MOTOR_MODE_PR - m.q = float(leg.target_q[i]) - m.dq = 0.0 - m.tau = 0.0 - m.kp = float(leg.kp[i]) - m.kd = float(leg.kd[i]) - self._cmd_pub.publish(cmd_msg) - - -def main(): - rclpy.init() - node = FameNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/lowerbody_controller_node.py b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/lowerbody_controller_node.py index 2d1a8ccb..dea05b1f 100644 --- a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/lowerbody_controller_node.py +++ b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/lowerbody_controller_node.py @@ -3,7 +3,8 @@ Flow ---- 1. The robot starts held by the elastic band, **idle** (no policy drives the legs). -2. At startup the ``active_policy`` (default ``fame``) is auto-activated: once the +2. At startup the ``active_policy`` (``fame``, ``walk`` or ``almi``; the bringups + set it from ``lowerbody:=``) is auto-activated: once the safety layer's /safety/heartbeat arrives, the node pre-poses the legs (ramping the target from the measured pose to the policy's nominal crouch over ``prepose_ramp_s`` — never a step at full gains), releases @@ -79,12 +80,6 @@ # latches this node silent on a False beat or on heartbeat_timeout of silence. SAFETY_HEARTBEAT_TOPIC = "/safety/heartbeat" -# Policy registry keys (see LowerBodyControllerNode.__init__). -FAME = "fame" # RMA balance/stand policy (does not locomote) -WALK = "walk" # TorchScript walk policy (follows /cmd_vel) -ALMI = "almi" # ALMI LSTM policy: stands (incl. under arm motion) and walks on - # /cmd_vel; AlmiPolicy resets its LSTM at gait onset - # Pre-pose: before engaging the policy, PD-drive the legs to the incoming policy's # nominal crouch (band-held) and wait until they settle there. The RMA policy warms # up from its trained default pose; released from the straight-leg spawn it can't @@ -110,9 +105,9 @@ def __init__(self): super().__init__("lowerbody_controller_node") self.declare_parameter("control_hz", 50.0) - # Policy to auto-activate at startup (releases the band when ready). - # Default "fame" (stand). "none" leaves the robot band-held idle forever, - # so set a real policy here. + # Policy to auto-activate at startup (releases the band when ready): + # fame (stands), walk or almi (both follow /cmd_vel). "none" leaves the + # robot band-held idle forever. self.declare_parameter("active_policy", "fame") self.declare_parameter("walk_config", _share("policies", "walk", "walk.yaml")) self.declare_parameter("fame_config", _share("policies", "fame", "fame.yaml")) @@ -182,7 +177,7 @@ def __init__(self): if not policies["fame"].has_encoder: self.get_logger().warn( "FAME encoder not loaded — z_t will be zeros (out-of-distribution). " - "Check policies/fame/encoder_3800.pt." + "Check fame.yaml's encoder_path." ) self._manager = PolicyManager( policies, log=lambda m: self.get_logger().info(m) @@ -231,16 +226,18 @@ def __init__(self): self.get_logger().info( f"lowerbody_controller ready: policies={self._manager.names()}, " - f"control_hz={control_hz}. Policy follows /cmd_vel; no start services." + f"control_hz={control_hz}." ) if startup_policy and startup_policy != "none": self.get_logger().info(f"active_policy={startup_policy!r}: auto-activating at startup") - self._request_policy(startup_policy) + ok, msg = self._request_policy(startup_policy) + if not ok: + self.get_logger().error(f"active_policy: {msg} -> robot stays band-held idle") else: self.get_logger().warn( - "active_policy='none' with start services removed -> robot stays " - "band-held idle forever. Set active_policy to 'fame' or 'walk'." + "active_policy='none' -> robot stays band-held idle forever. " + f"Set active_policy to one of {self._manager.names()}." ) # -- request entry points ------------------------------------------------ @@ -481,7 +478,6 @@ def _tick(self) -> None: # history (which filled with band-held states) is cleared and it # warms up fresh on free-standing — otherwise FAME's first free # actions are computed from stale band-held obs and it topples. - # (This is exactly what fame_node does, and why it stays up.) self._manager.reset_active(state) self._awaiting_band_release = False diff --git a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/walking_node.py b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/walking_node.py deleted file mode 100644 index 2f96c76d..00000000 --- a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/walking_node.py +++ /dev/null @@ -1,201 +0,0 @@ -'''Walking policy ROS 2 node. - -Runs the TorchScript walking policy from policies/walk/walkingPolicy.pt against -live robot state on /lowstate and publishes lower-body PD setpoints on -safety/lowcmd_lower_in for golem_safety to merge with upper-body commands. - -Kept as a standalone single-policy node; the generic controller is -lowerbody_controller_node (see policy.py / policy_manager.py). Direct adaptation -of reference/deploy_mujoco.py to a real-robot ROS 2 stack. -''' - -from pathlib import Path - -import numpy as np -import rclpy -import torch -import yaml -from ament_index_python.packages import get_package_share_directory -from geometry_msgs.msg import Twist -from rclpy.node import Node -from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy -from rclpy.time import Time -from std_srvs.srv import Trigger -from unitree_hg.msg import LowCmd, LowState - - -GAIT_PERIOD = 0.8 -NUM_LOWER_JOINTS = 12 -MOTOR_MODE_PR = 1 - - -def get_gravity_orientation(quaternion): - qw = quaternion[0] - qx = quaternion[1] - qy = quaternion[2] - qz = quaternion[3] - - gravity_orientation = np.zeros(3) - gravity_orientation[0] = 2 * (-qz * qx + qw * qy) - gravity_orientation[1] = -2 * (qz * qy + qw * qx) - gravity_orientation[2] = 1 - 2 * (qw * qw + qz * qz) - return gravity_orientation - - -def _default_example_path(filename: str) -> str: - pkg_share = get_package_share_directory('h12_lowerbody_rl') - return str(Path(pkg_share) / 'policies' / 'walk' / filename) - - -class WalkingNode(Node): - def __init__(self): - super().__init__('walking_node') - - self.declare_parameter('config_path', _default_example_path('walk.yaml')) - self.declare_parameter('policy_path', _default_example_path('walkingPolicy.pt')) - self.declare_parameter('control_hz', 50.0) - - config_path = self.get_parameter('config_path').get_parameter_value().string_value - policy_path = self.get_parameter('policy_path').get_parameter_value().string_value - control_hz = self.get_parameter('control_hz').get_parameter_value().double_value - - with open(config_path, 'r') as f: - config = yaml.safe_load(f) - - self._kps = np.array(config['kps'], dtype=np.float32) - self._kds = np.array(config['kds'], dtype=np.float32) - self._default_angles = np.array(config['default_angles'], dtype=np.float32) - self._ang_vel_scale = float(config['ang_vel_scale']) - self._dof_pos_scale = float(config['dof_pos_scale']) - self._dof_vel_scale = float(config['dof_vel_scale']) - self._action_scale = float(config['action_scale']) - self._cmd_scale = np.array(config['cmd_scale'], dtype=np.float32) - self._num_actions = int(config['num_actions']) - self._num_obs = int(config['num_obs']) - - self._policy = torch.jit.load(policy_path) - self._policy.eval() - - self._cmd = np.array(config['cmd_init'], dtype=np.float32) - self._action = np.zeros(self._num_actions, dtype=np.float32) - self._target_dof_pos = self._default_angles.copy() - self._obs = np.zeros(self._num_obs, dtype=np.float32) - - self._lowstate: LowState | None = None - self._start_time: Time | None = None - - lowstate_qos = QoSProfile( - reliability=ReliabilityPolicy.BEST_EFFORT, - history=HistoryPolicy.KEEP_LAST, - depth=1, - ) - self._lowstate_sub = self.create_subscription( - LowState, '/lowstate', self._on_lowstate, lowstate_qos - ) - self._cmd_vel_sub = self.create_subscription( - Twist, '/cmd_vel', self._on_cmd_vel, 10 - ) - self._cmd_pub = self.create_publisher(LowCmd, '/safety/lowcmd_lower_in', 10) - - self._timer = self.create_timer(1.0 / control_hz, self._tick) - - self.get_logger().info( - f'walking_node ready: config={config_path}, policy={policy_path}, ' - f'control_hz={control_hz}' - ) - - self._disable_elastic_band_if_present() - - def _disable_elastic_band_if_present(self) -> None: - cli = self.create_client(Trigger, '/elastic_band/toggle') - if not cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('elastic band toggle service not available — skipping') - return - future = cli.call_async(Trigger.Request()) - rclpy.spin_until_future_complete(self, future, timeout_sec=2.0) - result = future.result() - if result is None: - self.get_logger().warn('elastic band toggle call timed out') - return - self.get_logger().info(f'elastic band toggle: {result.message}') - - def _on_lowstate(self, msg: LowState) -> None: - self._lowstate = msg - if self._start_time is None: - self._start_time = self.get_clock().now() - - def _on_cmd_vel(self, msg: Twist) -> None: - # Policy was trained with SI commands in roughly [-1, 1] m/s (linear) - # and [-1, 1] rad/s (yaw). Clip to keep the policy's observation inside - # its training distribution. - self._cmd[0] = msg.linear.x - self._cmd[1] = msg.linear.y - self._cmd[2] = msg.angular.z - np.clip(self._cmd, -1.0, 1.0, out=self._cmd) - - def _tick(self) -> None: - if self._lowstate is None or self._start_time is None: - return - - motor_state = self._lowstate.motor_state - q = np.array( - [motor_state[i].q for i in range(NUM_LOWER_JOINTS)], dtype=np.float32 - ) - dq = np.array( - [motor_state[i].dq for i in range(NUM_LOWER_JOINTS)], dtype=np.float32 - ) - quat = np.asarray(self._lowstate.imu_state.quaternion, dtype=np.float32) - omega = np.asarray(self._lowstate.imu_state.gyroscope, dtype=np.float32) - - qj = (q - self._default_angles) * self._dof_pos_scale - dqj = dq * self._dof_vel_scale - gravity_orientation = get_gravity_orientation(quat) - omega_obs = omega * self._ang_vel_scale - - elapsed = (self.get_clock().now() - self._start_time).nanoseconds * 1e-9 - phase = (elapsed % GAIT_PERIOD) / GAIT_PERIOD - sin_phase = np.sin(2 * np.pi * phase) - cos_phase = np.cos(2 * np.pi * phase) - - n = self._num_actions - self._obs[0:3] = omega_obs - self._obs[3:6] = gravity_orientation - self._obs[6:9] = self._cmd * self._cmd_scale - self._obs[9 : 9 + n] = qj - self._obs[9 + n : 9 + 2 * n] = dqj - self._obs[9 + 2 * n : 9 + 3 * n] = self._action - self._obs[9 + 3 * n : 9 + 3 * n + 2] = np.array([sin_phase, cos_phase]) - - obs_tensor = torch.from_numpy(self._obs).unsqueeze(0) - with torch.no_grad(): - self._action = self._policy(obs_tensor).detach().numpy().squeeze() - self._target_dof_pos = self._action * self._action_scale + self._default_angles - - cmd_msg = LowCmd() - for i in range(NUM_LOWER_JOINTS): - motor = cmd_msg.motor_cmd[i] - motor.mode = MOTOR_MODE_PR - motor.q = float(self._target_dof_pos[i]) - motor.dq = 0.0 - motor.tau = 0.0 - motor.kp = float(self._kps[i]) - motor.kd = float(self._kds[i]) - - self._cmd_pub.publish(cmd_msg) - - -def main(): - rclpy.init() - node = WalkingNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/core_ws/src/h12_lowerbody_rl/policies/almi/README.md b/core_ws/src/h12_lowerbody_rl/policies/almi/README.md index c1ac5ef9..0d52af15 100644 --- a/core_ws/src/h12_lowerbody_rl/policies/almi/README.md +++ b/core_ws/src/h12_lowerbody_rl/policies/almi/README.md @@ -62,10 +62,10 @@ What happens, in order (`lowerbody_controller_node`): If the log says `NOT SETTLED (timeout)`, do **not** confirm — the legs never reached the crouch; investigate first. (The sim bringup passes `engage_wait_for_confirm: false` and auto-engages for unattended runs.) -4. On confirm the LSTM engages at zero command (stand). The - `elastic band toggle service unavailable` WARN right after is expected on - real — the band service is sim-only; the policy re-seeds its memory at - that moment (clean trained stand-start). +4. On confirm the LSTM engages at zero command (stand), a clean trained + stand-start. The elastic band is a sim fixture: `lowerbody_real.yaml` + sets `disable_elastic_band: false`, so on the real robot the node never + calls `/elastic_band/toggle`. 5. Slacken the hoist gradually. There is no post-engage ramp — the hoist is the ramp. diff --git a/core_ws/src/h12_lowerbody_rl/policies/almi/almi.yaml b/core_ws/src/h12_lowerbody_rl/policies/almi/almi.yaml index 5c68e831..20c81756 100644 --- a/core_ws/src/h12_lowerbody_rl/policies/almi/almi.yaml +++ b/core_ws/src/h12_lowerbody_rl/policies/almi/almi.yaml @@ -38,7 +38,7 @@ cmd_scale: [2.0, 2.0, 0.25] # Deployment-side command gain [vx, vy, wz], applied before clipping to +/-1 # (so the policy obs never leaves the trained command range). The checkpoint # undertracks its command — yaw worst (~0.2x actual/commanded), vx ~0.5x — -# which starves nav2's pure-pursuit curvature. Tuned in RoboCasa 2026-07-19. +# which starves nav2's pure-pursuit curvature. Tuned in RoboCasa. cmd_gain: [1.5, 1.0, 2.0] # See policy.py: smooths sign-flipping follower corrections (1.0 = off). cmd_smooth_alpha: 0.15 diff --git a/core_ws/src/h12_lowerbody_rl/policies/walk/walk.yaml b/core_ws/src/h12_lowerbody_rl/policies/walk/walk.yaml index 96277db5..6a806f32 100644 --- a/core_ws/src/h12_lowerbody_rl/policies/walk/walk.yaml +++ b/core_ws/src/h12_lowerbody_rl/policies/walk/walk.yaml @@ -1,13 +1,6 @@ -# -policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/h1_2/motion.pt" -xml_path: "{LEGGED_GYM_ROOT_DIR}/resources/robots/h1_2/scene.xml" - -# Total simulation time -simulation_duration: 60.0 -# Simulation time step -simulation_dt: 0.002 -# Controller update frequency (meets the requirement of simulation_dt * controll_decimation=0.02; 50Hz) -control_decimation: 10 +# TorchScript walking policy for the Unitree H1-2 — consumed by +# h12_lowerbody_rl.policy.WalkPolicy. Paths are relative to this file. +policy_path: "walkingPolicy.pt" kps: [200, 200, 200, 300, 40, 40, 200, 200, 200, 300, 40, 40] kds: [2.5, 2.5, 2.5, 4, 2, 2, 2.5, 2.5, 2.5, 4, 2, 2] @@ -18,8 +11,8 @@ legs_motor_pos_lower_limit_list: [-0.37, -3.13, -0.42, -0.11, -0.887334, -0.1817 legs_motor_pos_upper_limit_list: [0.37, 2.49, 3.13, 2.18, 0.513599, 0.181799, 0.37, 2.49, 0.42, 2.18, 0.513599, 0.181799] -# Training nominal: hip_pitch = -0.16 (slight forward lean). FAME shares this -# exact nominal by joint name, so the walk<->FAME handover has no leg-target jump. +# Training nominal: hip_pitch = -0.16 (slight forward lean); the controller +# pre-poses the legs here before the policy engages. default_angles: [0.0, -0.16, 0.0, 0.36, -0.2, 0.0, 0.0, -0.16, 0.0, 0.36, -0.2, 0.0] @@ -30,5 +23,3 @@ action_scale: 0.25 cmd_scale: [2.0, 2.0, 0.25] num_actions: 12 num_obs: 47 - -cmd_init: [0, 0, 0] diff --git a/core_ws/src/h12_lowerbody_rl/reference/README.md b/core_ws/src/h12_lowerbody_rl/reference/README.md index 63c430b2..24202d30 100644 --- a/core_ws/src/h12_lowerbody_rl/reference/README.md +++ b/core_ws/src/h12_lowerbody_rl/reference/README.md @@ -11,6 +11,7 @@ this repo). - `mujoco_deploy_h12_rma.py` — original FAME/RMA deploy → now `FamePolicy`. The live integration is in `h12_lowerbody_rl/`: -`policy.py` (WalkPolicy, FamePolicy), `policy_manager.py` (policy -registry and activation), `scripts/lowerbody_controller_node.py` (the ROS node), and the +`policy.py` (WalkPolicy, FamePolicy, and AlmiPolicy, the vendored ALMI +policy with no standalone script here — see `policies/almi/README.md`), +`policy_manager.py` (policy registry and activation), `scripts/lowerbody_controller_node.py` (the ROS node), and the trimmed inference-only encoder under `h12_lowerbody_rl/rma/`. diff --git a/core_ws/src/h12_lowerbody_rl/setup.py b/core_ws/src/h12_lowerbody_rl/setup.py index f2a743fc..392d744c 100644 --- a/core_ws/src/h12_lowerbody_rl/setup.py +++ b/core_ws/src/h12_lowerbody_rl/setup.py @@ -35,8 +35,6 @@ }, entry_points={ 'console_scripts': [ - 'walking_node = h12_lowerbody_rl.scripts.walking_node:main', - 'fame_node = h12_lowerbody_rl.scripts.fame_node:main', 'lowerbody_controller_node = ' 'h12_lowerbody_rl.scripts.lowerbody_controller_node:main', ], diff --git a/core_ws/src/lmg_deploy/README.md b/core_ws/src/lmg_deploy/README.md index 64ee9147..08687300 100644 --- a/core_ws/src/lmg_deploy/README.md +++ b/core_ws/src/lmg_deploy/README.md @@ -2,8 +2,9 @@ Runs a two-agent MARL policy from Locomanipulation_game (branch `marl-direct`, `LocoManip-Marl-Direct-v0`: LSTM actors, explicit PD at 500 Hz, a 0-20 ms target -delay in training) on the H1-2. No policy ships with the package: export one into -`policies//` (below) and launch with `policy:=`. A +delay in training) on the H1-2. One policy ships with the package, +`policies/mid360v2/`; export others into `policies//` (below) and launch with +`policy:=`. A **legs** agent that walks on `/cmd_vel`, and an **arms** agent that reaches wrist-pose goals through a damped-least-squares IK step plus a learned residual. One node, `marl_deploy_node`, owns all 27 joints. It publishes @@ -25,7 +26,7 @@ release) and uses the same heartbeat watchdog. | `lmg_deploy/livox_attitude.py` | the policy's attitude from FAST-LIO's `/Odometry`, carried by the gyro, levelled at the pre-pose | | `lmg_deploy/marl_deploy_node.py` | the ROS node: safety channels, heartbeat, pre-pose, band, operator interface | | `config/policy_template.yaml` | the deploy config a new policy starts from: joint order, defaults, gains, scales, bounds, command ranges (each from the training env), and the SLAM body frame | -| `policies//.yaml`, `{legs,arms}.pt`, `export.yaml` | a policy: its deploy config and what the export writes, below; the `.pt` files in Git LFS (`policies/.gitattributes`). Mid360v2 since 10-20 (Explicit500, Tune3Track2 and Mid360 were removed 10-19) | +| `policies//.yaml`, `{legs,arms}.pt`, `export.yaml` | a policy: its deploy config and what the export writes, below; the `.pt` files in Git LFS (`policies/.gitattributes`). `mid360v2` ships | | `bags//` | rosbag2 recordings of Explicit500 on the real robot (`policy_bag_walk`: walking to nav2 goals; `reach`: an arm goal, ended by the hardware e-stop), the `.db3` files in Git LFS | | `test/test_controller.py` | unit tests (rotations vs Isaac Lab, Jacobian, IK, observation layout, SLAM odometry, modes) on `test/data/explicit500.yaml`, Explicit500's deploy config | | `test/sim_drive.py` | a scripted drive in the Robocasa sim (stand, walk, turn, side-step, arm goal), measured on FAST-LIO's `/Odometry` | @@ -160,4 +161,4 @@ ros2 service call /marl_deploy_node/confirm_engage std_srvs/srv/Trigger - `imu_link` on `torso_link` for Explicit500 and older (the default when the export has no `imu` block). The attitude (so projected gravity) comes from FAST-LIO by default (`livox_attitude:=fastlio`): `/Odometry` is the IMU body's pose in `camera_init`, carried forward between poses by the gyro and levelled once from the accelerometer while the robot holds the pre-pose (`lmg_deploy/livox_attitude.py`; state `waiting_imu` until the head is still, so FAST-LIO must run, `use_slam:=true`). On the real walking bag it is 0.23 deg off FAST-LIO's poses (p95 0.62), where the complementary filter (`lmg_deploy/livox_imu.py`) is 1.9 deg off (p95 4.4): its pull toward the accelerometer follows the gait. With no `/Odometry` for `livox_odom_timeout` (1 s) the node falls back to the filter and warns; `livox_attitude:=filter` uses the filter throughout. Restart the node with FAST-LIO (it moves `camera_init`). The angular velocity passes the gyro low-pass the run trained with, `imu.gyro_lowpass_hz` in `export.yaml` (absent: none, as for Explicit500). `livox_imu.GyroLowpass` is a 2nd-order Butterworth, discretized exactly over each sample's dt, so training's physics rate and the MID360's 200 Hz realize the same filter. At 8 Hz it takes the head's ~20 Hz pitch resonance while walking from 0.5-0.9 to ~0.1 rad/s rms. `livox_joint` and `livox_imu_joint` carry the real robot's measured mount (point frame 8.77 deg, chip 9.45 deg pitch); `imu_offset_{roll,pitch,yaw}_deg` (default 0) trims what is left, live. -- **Safety-layer gaps.** The safety layer holds the last command on a silent channel: neither input channel has a staleness watchdog in the sim or real configs (`upper_stale_seconds: 0.0`). If this node latches silent, the safety layer keeps sending its last targets until it is stopped. +- **Safety-layer gaps.** The safety layer holds the last command on a silent channel. Its stale times only report a quiet input (`input_quiet`): in `relax_safety_split`, the lmg bringups' preset, `stale.lower_seconds` is 0.2 and `stale.upper_seconds` 0 (off), and neither stops the relay. If this node latches silent, the safety layer keeps sending its last targets until it is stopped. diff --git a/core_ws/src/lmg_deploy/lmg_deploy/marl_deploy_node.py b/core_ws/src/lmg_deploy/lmg_deploy/marl_deploy_node.py index 5848c4f7..75f0eeb3 100644 --- a/core_ws/src/lmg_deploy/lmg_deploy/marl_deploy_node.py +++ b/core_ws/src/lmg_deploy/lmg_deploy/marl_deploy_node.py @@ -47,7 +47,7 @@ * A non-finite observation or target latches the node silent instead of sending it: the safety layer would latch its estop (all motors limp) on it. Silent means the safety layer keeps holding the last targets until it is -stopped; it has no staleness watchdog on these channels. +stopped: its stale times (stale.*_seconds) only report a quiet input. IMU --- @@ -148,7 +148,7 @@ class MarlDeployNode(Node): def __init__(self): super().__init__("marl_deploy_node") - self.declare_parameter("config", "") # the policy's yaml, policies//.yaml (none ships) + self.declare_parameter("config", "") # the policy's yaml, policies//.yaml self.declare_parameter("urdf_path", "/home/code/CL_Assets/ros_assets/h1_2_magpie.urdf") self.declare_parameter("control_hz", 50.0) self.declare_parameter("prepose_ramp_s", 6.0) # s, minimum ramp duration @@ -170,8 +170,8 @@ def __init__(self): cfg_path = self.get_parameter("config").value if not cfg_path: - raise ValueError("config:= the policy's deploy yaml, policies//.yaml (lmg_deploy ships no " - "policy: export one, README 'Exporting a checkpoint')") + raise ValueError("config:= the policy's deploy yaml, policies//.yaml (lmg_deploy ships " + "mid360v2; README 'Exporting a checkpoint' adds others)") with open(cfg_path) as f: cfg = yaml.safe_load(f) cfg_dir = os.path.dirname(os.path.abspath(cfg_path)) diff --git a/docker/mac/docker-compose.yml b/docker/mac/docker-compose.yml index 1f004c80..bd950f04 100644 --- a/docker/mac/docker-compose.yml +++ b/docker/mac/docker-compose.yml @@ -136,19 +136,19 @@ services: # can run either or both. View via docker/mac/scripts/mac_vnc_tunnel.sh: # GOLEM_RVIZ=vnc docker compose -f docker/mac/docker-compose.yml up - GOLEM_RVIZ=${GOLEM_RVIZ:-0} - # Optional lower-body controller: GOLEM_LOWERBODY=fame runs the RMA policy - # that balances the robot standing unsupported (releases the elastic band); - # =walk runs the raw walk policy (marches in place); =almi runs the controller - # pinned to ALMI, which stands and walks on /cmd_vel -- the one to use for the - # nav demo. Default (unset) keeps the band tether + upper-body IK. + # Optional lower-body controller: GOLEM_LOWERBODY=fame|walk|almi runs + # lowerbody_controller_node pinned to that policy (it pre-poses the legs, + # then releases the elastic band). fame stands only; walk and almi follow + # /cmd_vel; almi stands and walks -- the one to use for the nav demo. + # Default (unset) keeps the band tether + upper-body IK. - GOLEM_LOWERBODY=${GOLEM_LOWERBODY:-} # GOLEM_SLAM=1 adds pointcloud_to_laserscan + slam_toolbox to the bringup: # the Livox cloud -> 2D scan -> occupancy /map against the sim's /odom. # Needs the robocasa service started with GOLEM_SIM_ODOM=1 to supply /odom. - GOLEM_SLAM=${GOLEM_SLAM:-0} # GOLEM_NAV2=1 adds the nav2 stack (implies GOLEM_SLAM for the map): plans a - # collision-free path on the SLAM map and drives /cmd_vel (the walk policy - # follows it). Send goals via RViz "2D Nav Goal" or /navigate_to_pose. + # collision-free path on the SLAM map and drives /cmd_vel (the lower-body + # policy follows it). Send goals via RViz "2D Nav Goal" or /navigate_to_pose. - GOLEM_NAV2=${GOLEM_NAV2:-0} # GOLEM_ROS_MCP=1 starts an in-container ROS debugging MCP server (warm rclpy # node + robot_status/wait_for/costmap/drive tools) on localhost:6082. Reach diff --git a/docker/mac/scripts/launch_ros_mac.sh b/docker/mac/scripts/launch_ros_mac.sh index 667381ba..c07ef575 100755 --- a/docker/mac/scripts/launch_ros_mac.sh +++ b/docker/mac/scripts/launch_ros_mac.sh @@ -1,9 +1,8 @@ #!/bin/bash # Apple-Silicon / CPU launcher for the SLIM ROS workspace. -# Builds only the core bringup package subset (skips the vision / grasp / nav2 -# / FAST-LIO packages that need the heavy ML stack), then launches the minimal -# Mac bringup (robot_state_publisher, joint_state_publisher, Pink IK -# frame_task_server, safety_node) against the running MuJoCo sim. +# Builds only the core bringup package subset (skips the vision / grasp / +# FAST-LIO packages that need the heavy ML stack), then launches the minimal +# Mac bringup (h12_sim_mac.launch.py) against the running MuJoCo sim. # # Pass `bash` as the first arg to drop to a shell instead of launching. # @@ -118,12 +117,11 @@ cd "$WS" # so they never show up in `ros2 pkg list`. NOTE only h12_sim_mac.launch.py runs # here — the other launch files exec-launch the omitted packages (nav2, # FAST_LIO, model_server...). -# unitree_hg + h12_lowerbody_rl provide the optional lower-body stack -# (walk/FAME policies). They build cheaply and are only *launched* when -# GOLEM_LOWERBODY is set (see the bringup), but building them always keeps -# `ros2 run h12_lowerbody_rl fame_node` available in a shell. unitree_hg needs -# ros-humble-rosidl-generator-dds-idl and torch, both in the image. Use -# GOLEM_LOWERBODY=almi to stand and walk (see docs/MACOS.md). +# unitree_hg + h12_lowerbody_rl provide the optional lower-body controller +# (lowerbody_controller_node with the FAME, walk or ALMI policy). They build +# cheaply and are only *launched* when GOLEM_LOWERBODY is set (see the bringup). +# unitree_hg needs ros-humble-rosidl-generator-dds-idl and torch, both in the +# image. Use GOLEM_LOWERBODY=almi to stand and walk (see docs/MACOS.md). PKGS="custom_ros_messages magpie_msgs golem_ros2_models golem_safety golem_upper_ik golem_slam golem_bringup unitree_hg h12_lowerbody_rl" BRINGUP_STUB_DEPS="estop livox_ros_driver2 fast_lio model_server cl_realsense lmg_deploy dex_3 g1_homie deploy_mjpc golem_skills" diff --git a/docs/MACOS.md b/docs/MACOS.md index 7662c96f..3d913b91 100644 --- a/docs/MACOS.md +++ b/docs/MACOS.md @@ -56,7 +56,7 @@ changing one needs the stack `restart` command to recreate both containers. |---|---|---| | `GOLEM_DISPLAY` | off | `vnc` → MuJoCo viewer on noVNC port 6080 | | `GOLEM_RVIZ` | off | `vnc` → RViz on noVNC port 6081 | -| `GOLEM_LOWERBODY` | off (tethered) | `fame` stand-only, `walk` raw policy, `almi` stand + walk (one policy) | +| `GOLEM_LOWERBODY` | off (tethered) | `fame` stand-only, `walk` walk-only policy, `almi` stand + walk (one policy) | | `GOLEM_SLAM` | off | `pointcloud_to_laserscan` + `slam_toolbox` → `/map` | | `GOLEM_NAV2` | off | nav2 stack (implies SLAM) | | `GOLEM_SIM_ODOM` | off | sim ground-truth `/odom` + `odom→pelvis` TF (SLAM needs it) | @@ -118,8 +118,8 @@ GOLEM_DISPLAY=vnc GOLEM_LOWERBODY=almi docker/mac/scripts/docker_stack_mac.sh up `almi` starts band-held idle, pre-poses the legs, then runs ALMI for the whole run: `rob_stand` (zero `/cmd_vel`) makes it stand, and `rob_go ` -makes it walk — there is no policy switch. `GOLEM_LOWERBODY=walk` launches the -raw walk policy directly and only marches in place. +makes it walk — there is no policy switch. `fame` and `walk` pin the same +controller to the FAME stand/squat policy or the walk-only policy instead. ## Demos From ffffb2f33e398e9fedde1c35c3379094ada118a2 Mon Sep 17 00:00:00 2001 From: maxconway Date: Sat, 31 Oct 2026 11:50:09 -0600 Subject: [PATCH 004/179] G1 navigation: FAST-LIO is the only odometry; drop odom_source, sim_gt and the unread sim argument; fix golem_slam and bringup text Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_01MezUeauFS9tKMgZKJxo1HC --- .../golem_bringup/launch/g1/g1_core.launch.py | 29 ++---------- .../launch/g1/g1_real_drivers.launch.py | 6 --- .../golem_bringup/launch/g1/g1_sim.launch.py | 13 ++---- .../h12/h12_locomanipulationgame.launch.py | 4 +- .../launch/h12/h12_real_desktop.launch.py | 2 +- .../launch/h12/h12_real_drivers.launch.py | 1 - .../launch/h12/h12_sim.launch.py | 16 +++---- .../launch/h12/lmg_real.launch.py | 2 +- core_ws/src/golem_bringup/rviz/h12/main.rviz | 46 ------------------- .../src/golem_bringup/test/test_nav_launch.py | 41 ++++------------- core_ws/src/golem_slam/README.md | 18 ++++---- core_ws/src/golem_slam/config/g1_fastlio.yaml | 4 +- .../src/golem_slam/config/g1_fastlio_sim.yaml | 7 ++- core_ws/src/golem_slam/config/g1_nav2.yaml | 2 +- core_ws/src/golem_slam/config/mid360.yaml | 2 +- .../src/golem_slam/config/nav2_config.yaml | 2 +- .../golem_slam/config/slam_toolbox_h1.yaml | 2 +- .../golem_slam/g1_body_to_pelvis.py | 9 +--- .../golem_slam/navigation_launch.py | 20 +++----- .../src/golem_slam/test/test_g1_pipeline.py | 8 ++-- .../golem_slam/test/test_navigation_launch.py | 16 +------ tests/g1/README.md | 2 +- tests/g1/common.sh | 5 +- tests/g1/test_sim_explore.py | 19 ++++---- tests/g1/test_sim_explore.sh | 20 ++++---- tests/g1/test_sim_nav.py | 32 ++++++------- tests/g1/test_sim_nav.sh | 25 +++++----- 27 files changed, 110 insertions(+), 243 deletions(-) diff --git a/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py index 8d6b110c..c84d3cd2 100644 --- a/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py +++ b/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py @@ -53,7 +53,7 @@ golem_slam (odometry, slam_toolbox and Nav2, which drives the walking node through /cmd_vel), with the core: on the sim's FAST_LIO config and clock in the sim, and on the real robot g1_fastlio.yaml, wall time and the MID-360 -driver (g1_real_drivers.launch.py). odom_source must be fastlio: the shared +driver (g1_real_drivers.launch.py). FAST_LIO is the only odometry: the shared pipeline needs its deskewed cloud. Leave GOLEM_SIM_ODOM unset to avoid a second odom -> pelvis publisher. FAST_LIO starts while the sim's elastic band still holds the robot up: the @@ -74,7 +74,6 @@ sim refuses the real robot's (split, g1_split, and the walking node's g1). Since g1_split is the real robot's and sim_split the sim's, this also keeps each controller config to its own safety config. -- With use_nav, odom_source must be fastlio. start_position_verified refuses nothing: it decides whether the real robot's walking node starts. @@ -111,8 +110,6 @@ False: 'g1', True: 'sim', } -# Shared pipeline's odom sources (its launch file checks them too) -ODOM_SOURCES = ('fastlio',) def _as_bool(context, name): @@ -132,20 +129,10 @@ def locomotion_refusal(sim, locomotion_config): return None -def nav_refusal(sim, odom_source): - """Why shared navigation must not start with these settings, or None.""" - if odom_source not in ODOM_SOURCES: - return 'golem_slam requires FAST-LIO for its deskewed cloud' - return None - - -def startup_refusal(sim, safety_config, controller_config, locomotion_config, - use_nav=False, odom_source='fastlio'): +def startup_refusal(sim, safety_config, controller_config, locomotion_config): """Why g1_core must not start with these settings, or None.""" - refusal = nav_refusal(sim, odom_source) if use_nav else None # golem_upper_ik's pairing rules for the G1 - if refusal is None: - refusal = pairing_refusal(ROBOT, controller_config, safety_config, sim=sim) + refusal = pairing_refusal(ROBOT, controller_config, safety_config, sim=sim) if refusal is None: refusal = locomotion_refusal(sim, locomotion_config) return refusal @@ -154,7 +141,6 @@ def startup_refusal(sim, safety_config, controller_config, locomotion_config, def _setup(context): sim = _as_bool(context, 'sim') use_nav = _as_bool(context, 'use_nav') - odom_source = _config(context, 'odom_source') use_sim_time = {'use_sim_time': _as_bool(context, 'use_sim_time')} default_safety, default_controller = DEFAULT_CONFIGS[sim] safety_config = _config(context, 'safety_config') or default_safety @@ -164,8 +150,7 @@ def _setup(context): locomotion_config = _config(context, 'locomotion_config') or LOCOMOTION_CONFIGS[sim] delay = float(LaunchConfiguration('controller_delay').perform(context)) locomotion_delay = float(LaunchConfiguration('locomotion_delay').perform(context)) - refusal = startup_refusal(sim, safety_config, controller_config, locomotion_config, - use_nav, odom_source) + refusal = startup_refusal(sim, safety_config, controller_config, locomotion_config) if refusal is not None: raise RuntimeError(f'g1_core: not starting: {refusal}') @@ -267,7 +252,6 @@ def _setup(context): PythonLaunchDescriptionSource(os.path.join( get_package_share_directory('golem_bringup'), 'launch', 'g1', 'g1_real_drivers.launch.py')), - launch_arguments={'use_lidar': 'true'}.items(), )) if use_nav: # with the core: FAST_LIO initialises on its first scans, in the sim @@ -283,8 +267,6 @@ def _setup(context): actions.append(IncludeLaunchDescription( PythonLaunchDescriptionSource(nav), launch_arguments={ - 'odom_source': odom_source, - 'sim': str(sim).lower(), 'fast_lio_config_file': 'g1_fastlio_sim.yaml' if sim else 'g1_fastlio.yaml', 'use_sim_time': str(use_sim_time['use_sim_time']).lower(), }.items(), @@ -318,9 +300,6 @@ def generate_launch_description(): 'use_nav', default_value='true', description='golem_slam: odometry, SLAM and Nav2 on /cmd_vel, and on the real ' 'robot the MID-360 driver (g1_real_drivers.launch.py)'), - DeclareLaunchArgument( - 'odom_source', default_value='fastlio', - description='with use_nav: fastlio (shared pipeline requires its deskewed cloud)'), DeclareLaunchArgument( 'use_skills', default_value='false', description='Start the golem_skills action servers (/skill/*)'), diff --git a/core_ws/src/golem_bringup/launch/g1/g1_real_drivers.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_real_drivers.launch.py index be2fa5a7..b59bb226 100644 --- a/core_ws/src/golem_bringup/launch/g1/g1_real_drivers.launch.py +++ b/core_ws/src/golem_bringup/launch/g1/g1_real_drivers.launch.py @@ -14,9 +14,6 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -24,8 +21,6 @@ def generate_launch_description(): mid360_config = os.path.join( get_package_share_directory('golem_slam'), 'config', 'g1_MID360_config.json') return LaunchDescription([ - DeclareLaunchArgument('use_lidar', default_value='true', - description='Start the MID-360 driver'), Node( package='livox_ros_driver2', executable='livox_ros_driver2_node', @@ -42,6 +37,5 @@ def generate_launch_description(): 'user_config_path': mid360_config, 'cmdline_input_bd_code': 'livox0000000001', }], - condition=IfCondition(LaunchConfiguration('use_lidar')), ), ]) diff --git a/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py index 8c64f8e6..35baf9e8 100644 --- a/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py +++ b/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py @@ -22,11 +22,10 @@ use_nav (default true) adds golem_slam on the sim's MID-360 (/livox/lidar, /livox/imu), with sim time and g1_fastlio_sim.yaml, and Nav2 drives the walking node on /cmd_vel; send goals to /navigate_to_pose. -odom_source:=fastlio is required. Start the sim with GOLEM_SIM_ODOM unset; -otherwise odom -> pelvis has two publishers. The shared pipeline needs -FAST-LIO's deskewed cloud and refuses sim_gt. use_skills:=true -adds golem_skills, which serves /skill/frontier_explore to walk the robot -through Nav2 to the frontiers of /map. +FAST-LIO is the odometry. Start the sim with GOLEM_SIM_ODOM unset; +otherwise odom -> pelvis has two publishers. use_skills:=true adds +golem_skills, which serves /skill/frontier_explore to walk the robot through +Nav2 to the frontiers of /map. tests/g1/test_sim_nav.sh is the closed-loop test. """ @@ -52,9 +51,6 @@ def generate_launch_description(): DeclareLaunchArgument( 'use_nav', default_value='true', description='golem_slam: odometry, SLAM and Nav2 on /cmd_vel'), - DeclareLaunchArgument( - 'odom_source', default_value='fastlio', - description='with use_nav: fastlio (deskewed cloud required)'), DeclareLaunchArgument( 'use_skills', default_value='false', description='Start the golem_skills action servers (/skill/*)'), @@ -69,7 +65,6 @@ def generate_launch_description(): # the one safety_config runs with: sim_split 'controller_config': '', 'use_nav': LaunchConfiguration('use_nav'), - 'odom_source': LaunchConfiguration('odom_source'), 'use_skills': LaunchConfiguration('use_skills'), }.items(), ), diff --git a/core_ws/src/golem_bringup/launch/h12/h12_locomanipulationgame.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_locomanipulationgame.launch.py index b7c0fbc9..120acf67 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_locomanipulationgame.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_locomanipulationgame.launch.py @@ -33,8 +33,8 @@ def generate_launch_description(): description='Start marl_deploy_node'), DeclareLaunchArgument( 'policy', - description='policies// of lmg_deploy: .yaml plus the export (required: ' - 'lmg_deploy ships none)'), + description='policies// of lmg_deploy: .yaml plus the export (required; ' + 'lmg_deploy ships mid360v2)'), DeclareLaunchArgument('engage_wait_for_confirm', default_value='true', description='hold the pre-pose until ~/confirm_engage'), DeclareLaunchArgument('prepose_ramp_s', default_value='6.0', diff --git a/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py index 1bb6d675..e481212b 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py @@ -61,7 +61,7 @@ def generate_launch_description(): Node( package='rviz2', executable='rviz2', - name='rviz2_sim', + name='rviz2', arguments=['-d', default_rviz], parameters=[{'use_sim_time': False}], output='screen', diff --git a/core_ws/src/golem_bringup/launch/h12/h12_real_drivers.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_real_drivers.launch.py index b374cbec..5d1c2163 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_real_drivers.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_real_drivers.launch.py @@ -27,7 +27,6 @@ def generate_launch_description(): {'publish_freq': 30.0}, # publish frequency (Hz) {'output_data_type': 0}, {'frame_id': 'livox_frame'}, - {'lvx_file_path': '/home/livox/livox_test.lvx'}, {'user_config_path': mid360_config}, {'cmdline_input_bd_code': 'livox0000000001'}, ] diff --git a/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py index 8c34ae98..eb79b0f1 100644 --- a/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py @@ -32,7 +32,6 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_rviz', default_value='true'), - DeclareLaunchArgument('use_sliders', default_value='true'), DeclareLaunchArgument('use_nav', default_value='true'), DeclareLaunchArgument('use_skills', default_value='true'), DeclareLaunchArgument('record_runs', default_value='true'), @@ -75,13 +74,14 @@ def generate_launch_description(): _include('golem_slam', 'h12_navigation.launch.py', sim_time, condition=IfCondition(LaunchConfiguration('use_nav'))), - # The MuJoCo bridge back-projects depth into 3D using REP-103 optical - # convention (+z = forward, +x = right, +y = down) but stamps the - # resulting camera_info / image / depth messages with the optical - # frame name below. The URDF defines only camera_link (a ROS link - # frame, +x = forward, +z = up); without this static TF the vision - # pipeline would treat optical-convention points as if they were in - # camera_link, rotating every detection ~90 deg out of place. + # For Isaac: its head camera's images are stamped + # camera_color_optical_frame (CL_isaaclab_sim's ros_sensor_bridge.py, + # REP-103 optical axes: +z forward, +x right, +y down) and it publishes + # no TF, so this rotates that frame under camera_link (+x forward, + # +z up). The URDF loaded here has no camera_link (its head camera is + # head_camera_link), so the frame joins the robot's tree only where + # something publishes camera_link. RoboCasa stamps + # head_color_optical_frame and publishes its own camera TF. Node( package='tf2_ros', executable='static_transform_publisher', diff --git a/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py b/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py index 6b7031ed..eac312ff 100644 --- a/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py @@ -13,7 +13,7 @@ # state publishers, estop, safety layer and golem_slam navigation stack # (use_slam), but without frame_task_server, # because marl_deploy_node owns both safety channels. Keep the lower-body RL -# and MJPC controllers (h12_real_robot.launch.py's leg control) off. +# controller (h12_real_robot.launch.py's leg control) off. # # Nothing commands the robot until both operator gates pass: # 1. start_position_verified:=true starts marl_deploy_node; it ramps all 27 diff --git a/core_ws/src/golem_bringup/rviz/h12/main.rviz b/core_ws/src/golem_bringup/rviz/h12/main.rviz index 42ff6a86..3499b212 100644 --- a/core_ws/src/golem_bringup/rviz/h12/main.rviz +++ b/core_ws/src/golem_bringup/rviz/h12/main.rviz @@ -1093,52 +1093,6 @@ Visualization Manager: Reliability Policy: Reliable Value: /global_costmap/published_footprint Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Tracked Objects PointCloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /tracked_objects/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Tracked Objects Markers - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /tracked_objects/markers - Value: false - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 diff --git a/core_ws/src/golem_bringup/test/test_nav_launch.py b/core_ws/src/golem_bringup/test/test_nav_launch.py index 54524243..eaf23cca 100644 --- a/core_ws/src/golem_bringup/test/test_nav_launch.py +++ b/core_ws/src/golem_bringup/test/test_nav_launch.py @@ -10,7 +10,6 @@ import os import pathlib -import re import pytest from ament_index_python.packages import get_package_share_directory @@ -90,7 +89,6 @@ def test_nav_is_on_by_default(): passed = _passed_to_core(launch_file) assert passed['use_nav'] == 'true', launch_file assert _includes(**dict(passed, use_nav='false')) == [] - assert _passed_to_core('g1_sim.launch.py')['odom_source'] == 'fastlio' def test_real_bringup_starts_the_lidar_driver_and_the_real_fastlio_config(): @@ -100,8 +98,7 @@ def test_real_bringup_starts_the_lidar_driver_and_the_real_fastlio_config(): 'g1_real_drivers.launch.py') assert sorted(_location(i) for i in includes) == sorted([drivers, NAV_LAUNCH]) path, arguments = _nav_include(**passed) - assert arguments == {'odom_source': 'fastlio', 'sim': 'false', 'use_sim_time': 'false', - 'fast_lio_config_file': 'g1_fastlio.yaml'} + assert arguments == {'use_sim_time': 'false', 'fast_lio_config_file': 'g1_fastlio.yaml'} nodes = _nav_nodes(path, arguments) _, values, files = nodes['fast_lio'] assert files == [os.path.join(os.path.dirname(os.path.dirname(NAV_LAUNCH)), 'config', @@ -120,27 +117,20 @@ def test_real_bringup_starts_the_lidar_driver_and_the_real_fastlio_config(): assert [_location(i) for i in _includes(**sim)] == [NAV_LAUNCH] -@pytest.mark.parametrize('odom_source', ['fastlio']) -def test_sim_bringup_includes_navigation(odom_source): - args = dict(use_nav='true') - if odom_source != 'fastlio': - args['odom_source'] = odom_source - passed = _passed_to_core('g1_sim.launch.py', **args) - assert (passed['use_nav'], passed['odom_source']) == ('true', odom_source) +def test_sim_bringup_includes_navigation(): + passed = _passed_to_core('g1_sim.launch.py', use_nav='true') + assert passed['use_nav'] == 'true' # the walking node starts; navigation comes with the core, on sim time assert 'locomotion_node' in _core_nodes(**passed) path, arguments = _nav_include(**passed) assert path == NAV_LAUNCH - assert arguments == {'odom_source': odom_source, 'sim': 'true', 'use_sim_time': 'true', - 'fast_lio_config_file': 'g1_fastlio_sim.yaml'} + assert arguments == {'use_sim_time': 'true', 'fast_lio_config_file': 'g1_fastlio_sim.yaml'} nodes = _nav_nodes(path, arguments) - assert ('fast_lio' in nodes) == (odom_source == 'fastlio') - if odom_source == 'fastlio': - # the sim's FAST_LIO config: the raw sensor frame, every point kept - _, _, files = nodes['fast_lio'] - assert files == [os.path.join(os.path.dirname(os.path.dirname(NAV_LAUNCH)), 'config', - 'g1_fastlio_sim.yaml')] + # the sim's FAST_LIO config: the raw sensor frame, every point kept + _, _, files = nodes['fast_lio'] + assert files == [os.path.join(os.path.dirname(os.path.dirname(NAV_LAUNCH)), 'config', + 'g1_fastlio_sim.yaml')] for name, (_, values, _) in nodes.items(): if name.endswith('_tf'): continue # static_transform_publisher: arguments only @@ -181,18 +171,7 @@ def test_sim_nav_without_use_locomotion(): assert nodes['locomotion_node'].arguments == ['--config', 'sim.yaml'] path, arguments = _nav_include(**passed) assert path == NAV_LAUNCH - assert arguments['sim'] == 'true' - - -@pytest.mark.parametrize('args, message', [ - (dict(SIM, use_nav='true', odom_source='sim_gt'), 'golem_slam requires FAST-LIO'), - (dict(SIM, use_nav='yes', odom_source='wheel'), - "golem_slam requires FAST-LIO"), - (dict(REAL_VERIFIED, use_nav='true', odom_source='sim_gt'), 'golem_slam requires FAST-LIO'), -]) -def test_core_refuses_nav(args, message): - with pytest.raises(RuntimeError, match='g1_core: not starting: ' + re.escape(message)): - core._setup(_context(core.generate_launch_description(), **args)) + assert arguments['fast_lio_config_file'] == 'g1_fastlio_sim.yaml' def test_real_nav_with_the_legs(): diff --git a/core_ws/src/golem_slam/README.md b/core_ws/src/golem_slam/README.md index a1800461..151c17e3 100644 --- a/core_ws/src/golem_slam/README.md +++ b/core_ws/src/golem_slam/README.md @@ -1,14 +1,16 @@ # golem_slam -Shared SLAM/navigation for H1-2 and G1. This is the renamed `h12_slam`, with -robot-specific configuration and the dynamic waist transform (`g1_body_to_pelvis`) -for the G1. +Shared SLAM/navigation for the H1-2 and the G1, with robot-specific +configuration and, for the G1, the dynamic waist transform (`g1_body_to_pelvis`). +Both robots' launch files build the same pipeline (`golem_slam/navigation_launch.py`). -- `h12_navigation.launch.py`: unchanged H1-2 defaults. +- `h12_navigation.launch.py`: the H1-2's pipeline, on `mid360.yaml`. - `h12_real_slam.launch.py`: H1-2 sensors and navigation without motor control. -- `g1_navigation.launch.py sim:=true use_sim_time:=true`: G1 shared pipeline, - requiring state publishing and the sim sensor stream. Real mode and sim_gt - are refused. Bringup selects this launch with `use_nav:=true`. +- `g1_navigation.launch.py`: the G1's pipeline, on the sim's FAST-LIO config + (`g1_fastlio_sim.yaml`) by default or the real robot's with + `fast_lio_config_file:=g1_fastlio.yaml`; it needs the robot's state + (robot_state_publisher) and the MID-360's `/livox/lidar` and `/livox/imu`. + FAST-LIO is the only odometry. The G1 bringups include it with `use_nav:=true`. FAST-LIO's deskewed cloud is self-filtered against the URDF, then used for floor alignment and OctoMap. slam_toolbox receives `/converted_scan`; Nav2 @@ -19,7 +21,7 @@ pointcloud_to_laserscan, avoiding the Humble TF-filter stall observed while walking. A G1-only remap routes recovery commands through the velocity smoother. See [G1 navigation](../../../docs/g1/navigation.md) and -[cache migration](../../../docs/RUNNING.md#package-name-migration). +[stale installs after a package rename](../../../docs/RUNNING.md#troubleshooting). From this package directory in a built and sourced ROS container: diff --git a/core_ws/src/golem_slam/config/g1_fastlio.yaml b/core_ws/src/golem_slam/config/g1_fastlio.yaml index 8d277e7d..0ca7d7fa 100644 --- a/core_ws/src/golem_slam/config/g1_fastlio.yaml +++ b/core_ws/src/golem_slam/config/g1_fastlio.yaml @@ -42,8 +42,8 @@ fov_degree: 360.0 det_range: 100.0 # The MID-360's lidar origin in its own IMU's frame (p_imu = R p_lidar + T): - # FAST_LIO's upstream MID-360 value. Not estimated online: the launch - # file's transforms use these values. + # FAST_LIO's upstream MID-360 value. Not estimated online: + # g1_body_to_pelvis reads these values for its dynamic body -> pelvis. extrinsic_est_en: false extrinsic_T: [-0.011, -0.02329, 0.04412] # R is identity for the raw sensor frame: g1_MID360_config.json's diff --git a/core_ws/src/golem_slam/config/g1_fastlio_sim.yaml b/core_ws/src/golem_slam/config/g1_fastlio_sim.yaml index d9034256..62ff4247 100644 --- a/core_ws/src/golem_slam/config/g1_fastlio_sim.yaml +++ b/core_ws/src/golem_slam/config/g1_fastlio_sim.yaml @@ -9,8 +9,7 @@ feature_extract_enable: false point_filter_num: 1 # keep every point: a synthetic scan is sparser than the real one max_iteration: 3 - # 0.2 m voxels left FAST_LIO 5 cm off on test_fastlio.py's synthetic scan, - # 0.1 m about 1 cm; check again on the G1 sim's scans + # m; 0.1 m kept FAST_LIO within about 1 cm on a synthetic scan, 0.2 m 5 cm off filter_size_surf: 0.1 filter_size_map: 0.1 cube_side_length: 1000.0 @@ -39,8 +38,8 @@ det_range: 100.0 # The MID-360's lidar origin in its own IMU's frame (p_imu = R p_lidar + T): # FAST_LIO's upstream MID-360 value, which the sim has to match. R is - # identity for the raw sensor frame. Not estimated online: the launch - # file's static transforms use these values. + # identity for the raw sensor frame. Not estimated online: + # g1_body_to_pelvis reads these values for its dynamic body -> pelvis. extrinsic_est_en: false extrinsic_T: [-0.011, -0.02329, 0.04412] extrinsic_R: [1., 0., 0., diff --git a/core_ws/src/golem_slam/config/g1_nav2.yaml b/core_ws/src/golem_slam/config/g1_nav2.yaml index 042f202f..e37bd64a 100644 --- a/core_ws/src/golem_slam/config/g1_nav2.yaml +++ b/core_ws/src/golem_slam/config/g1_nav2.yaml @@ -123,7 +123,7 @@ controller_server: cost_power: 1 cost_weight: 10.0 # Shared humanoid recovery tree, without Spin; see behavior_trees/navigate_to_pose_humanoid.xml. -# h12_navigation.launch.py rewrites this key to the installed path. +# golem_slam/navigation_launch.py rewrites this key to the installed path. bt_navigator: ros__parameters: default_nav_to_pose_bt_xml: "" diff --git a/core_ws/src/golem_slam/config/mid360.yaml b/core_ws/src/golem_slam/config/mid360.yaml index 77511e3d..fd83fac7 100644 --- a/core_ws/src/golem_slam/config/mid360.yaml +++ b/core_ws/src/golem_slam/config/mid360.yaml @@ -49,7 +49,7 @@ det_range: 100.0 extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic # extrinsic_T / extrinsic_R (the lidar in the IMU frame) come from the URDF's - # livox_link and livox_imu_link: h12_navigation.launch.py sets them. + # livox_link and livox_imu_link: golem_slam/navigation_launch.py sets them. publish: path_en: true # true: publish Path diff --git a/core_ws/src/golem_slam/config/nav2_config.yaml b/core_ws/src/golem_slam/config/nav2_config.yaml index 375fb743..abbe835a 100644 --- a/core_ws/src/golem_slam/config/nav2_config.yaml +++ b/core_ws/src/golem_slam/config/nav2_config.yaml @@ -129,7 +129,7 @@ controller_server: cost_weight: 10.0 # The default behavior tree, minus the Spin recovery and with a BackUp fast # enough for ALMI to act on; see behavior_trees/navigate_to_pose_humanoid.xml. -# h12_navigation.launch.py rewrites this key to the installed path. +# golem_slam/navigation_launch.py rewrites this key to the installed path. bt_navigator: ros__parameters: default_nav_to_pose_bt_xml: "" diff --git a/core_ws/src/golem_slam/config/slam_toolbox_h1.yaml b/core_ws/src/golem_slam/config/slam_toolbox_h1.yaml index 853c064b..2ff48443 100644 --- a/core_ws/src/golem_slam/config/slam_toolbox_h1.yaml +++ b/core_ws/src/golem_slam/config/slam_toolbox_h1.yaml @@ -1,4 +1,4 @@ -# slam_toolbox for the H1-2, shared by the real robot and the simulators: the +# slam_toolbox for both robots (the H1-2 and the G1), real and simulated: the # stock mapper_params_online_async.yaml (slam_toolbox 2.6.10, Humble) that the # real-verified reference stack (correlllab/fast_lio_nav2_humanoid) loads, # copied so a different installed slam_toolbox release cannot change it. diff --git a/core_ws/src/golem_slam/golem_slam/g1_body_to_pelvis.py b/core_ws/src/golem_slam/golem_slam/g1_body_to_pelvis.py index 36fec570..fd947b3c 100644 --- a/core_ws/src/golem_slam/golem_slam/g1_body_to_pelvis.py +++ b/core_ws/src/golem_slam/golem_slam/g1_body_to_pelvis.py @@ -1,5 +1,5 @@ """ -Publish body -> pelvis for the G1 FAST-LIO pipeline and compute G1 URDF sensor transforms. +Publish the G1's dynamic body -> pelvis for FAST-LIO, with the transform helpers around it. ros2 run golem_slam g1_body_to_pelvis --ros-args -p fastlio_config:= @@ -137,13 +137,6 @@ def body_to_pelvis(pelvis_lidar, body_from_lidar): return body_from_lidar @ invert(pelvis_lidar) -def fastlio_transforms(urdf_file, fastlio_config, pelvis_height, lidar_frame=np.eye(4)): - """Return (T_odom_camera_init, T_body_pelvis at the start pose, the waist at 0).""" - pelvis_body = pelvis_to_body(urdf_file, fastlio_extrinsic(fastlio_config), lidar_frame) - odom_pelvis = transform((0.0, 0.0, pelvis_height)) - return odom_pelvis @ pelvis_body, invert(pelvis_body) - - def static_transform_arguments(t, parent, child): """Return tf2_ros static_transform_publisher arguments that publish T_parent_child.""" values = list(t[:3, 3]) + list(matrix_to_quat(t[:3, :3])) # x y z qx qy qz qw diff --git a/core_ws/src/golem_slam/golem_slam/navigation_launch.py b/core_ws/src/golem_slam/golem_slam/navigation_launch.py index 66bea218..b5e304eb 100644 --- a/core_ws/src/golem_slam/golem_slam/navigation_launch.py +++ b/core_ws/src/golem_slam/golem_slam/navigation_launch.py @@ -50,10 +50,6 @@ def _fast_lio(context, config_dir, fast_lio_config_file, use_sim_time): return [fast_lio_node, static_tf_broadcaster] -def _as_bool(context, name): - return LaunchConfiguration(name).perform(context).strip().lower() in ('true', '1', 'yes') - - def _g1_fast_lio(context, config_dir, use_sim_time): config = os.path.join(config_dir, LaunchConfiguration('fast_lio_config_file').perform(context)) return [ @@ -69,10 +65,6 @@ def _g1_fast_lio(context, config_dir, use_sim_time): def _pipeline(context, robot): - # Refuse unsupported G1 modes before constructing any executable action. - if robot == 'g1': - if LaunchConfiguration('odom_source').perform(context) != 'fastlio': - raise RuntimeError('golem_slam requires FAST-LIO for its deskewed cloud') config_dir = os.path.join(get_package_share_directory('golem_slam'), 'config') use_sim_time = LaunchConfiguration('use_sim_time') fast_lio_config_file = LaunchConfiguration('fast_lio_config_file') @@ -136,8 +128,9 @@ def _pipeline(context, robot): name='cloud_registered_to_laserscan', output='screen', parameters=[ - # Level frame on the floor, so body sway doesn't tilt the slice - # into the floor. + # H1-2: base_footprint, level and on the floor, so body sway + # doesn't tilt the slice into the floor. G1: none, level_cloud + # has already put /cloud_base_filtered in that frame. {'target_frame': '' if robot == 'g1' else 'base_footprint'}, {'use_sim_time': use_sim_time}, # m above the floor: clears floor returns and FAST-LIO z drift, @@ -241,13 +234,14 @@ def generate_navigation_launch_description(robot): config, 'slam_toolbox_h1.yaml')), DeclareLaunchArgument('octomap_params_file', default_value=os.path.join( config, 'g1_octomap.yaml' if g1 else 'octomap.yaml')), - DeclareLaunchArgument('urdf_file', default_value=urdf_path(robot, ros=True)), ] if g1: arguments += [ - DeclareLaunchArgument('sim', default_value='false'), - DeclareLaunchArgument('odom_source', default_value='fastlio'), DeclareLaunchArgument('pipeline_params_file', default_value=os.path.join(config, 'g1_pipeline.yaml')), ] + else: + arguments += [ + DeclareLaunchArgument('urdf_file', default_value=urdf_path(robot, ros=True)), + ] return LaunchDescription(arguments + [OpaqueFunction(function=_pipeline, args=[robot])]) diff --git a/core_ws/src/golem_slam/test/test_g1_pipeline.py b/core_ws/src/golem_slam/test/test_g1_pipeline.py index 84117270..7083a5f2 100644 --- a/core_ws/src/golem_slam/test/test_g1_pipeline.py +++ b/core_ws/src/golem_slam/test/test_g1_pipeline.py @@ -4,7 +4,7 @@ from ament_index_python.packages import get_package_share_directory from golem_ros2_models.paths import urdf_path from golem_slam.g1_body_to_pelvis import ( - body_to_pelvis, fastlio_extrinsic, fastlio_transforms, invert, link_pose, + body_to_pelvis, fastlio_extrinsic, invert, link_pose, pelvis_to_body, static_transform_arguments, transform, ) from golem_slam.robot_geometry import link_boxes, quat_to_matrix, rpy_to_matrix @@ -70,10 +70,10 @@ def test_static_transform_arguments_round_trip(): urdf = urdf_path('g1') config = os.path.join( get_package_share_directory('golem_slam'), 'config', 'g1_fastlio_sim.yaml') - t = fastlio_transforms(urdf, config, 0.744)[0] - args = static_transform_arguments(t, 'odom', 'camera_init') + t = pelvis_to_body(urdf, fastlio_extrinsic(config)) + args = static_transform_arguments(t, 'pelvis', 'body') values = dict(zip(args[0::2], args[1::2])) - assert values['--frame-id'] == 'odom' and values['--child-frame-id'] == 'camera_init' + assert values['--frame-id'] == 'pelvis' and values['--child-frame-id'] == 'body' xyz = [float(values[f'--{k}']) for k in 'xyz'] quat = [float(values[f'--{k}']) for k in ('qx', 'qy', 'qz', 'qw')] np.testing.assert_allclose(transform(xyz, quat_to_matrix(*quat)), t, atol=1e-10) diff --git a/core_ws/src/golem_slam/test/test_navigation_launch.py b/core_ws/src/golem_slam/test/test_navigation_launch.py index 0af0db82..ae44057c 100644 --- a/core_ws/src/golem_slam/test/test_navigation_launch.py +++ b/core_ws/src/golem_slam/test/test_navigation_launch.py @@ -2,7 +2,6 @@ import importlib import os -import pytest import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchContext @@ -51,12 +50,10 @@ def test_h12_preserves_floor_and_scan_defaults(): def test_g1_uses_shared_cloud_pipeline_and_dynamic_waist(): - context, actions = pipeline('g1', sim='true', use_sim_time='true') + context, actions = pipeline('g1', use_sim_time='true') ns = nodes(context, actions) assert {'self_filter', 'level_odom', 'octomap_server', 'slam_toolbox', 'cloud_registered_to_laserscan', 'body_to_pelvis', 'fast_lio'} <= set(ns) - assert not {'level_frame', 'livox_to_pointcloud', 'odom_camera_init_tf', - 'body_to_pelvis_broadcaster'} & set(ns) assert 'level_cloud' in ns assert ns['cloud_registered_to_laserscan'][1]['target_frame'] == '' assert ns['level_odom'][1]['dynamic_base_transform'] is True @@ -74,15 +71,6 @@ def test_g1_real_mode_builds_the_pipeline(): assert ns['level_odom'][1]['use_sim_time'] is False -@pytest.mark.parametrize('overrides,match', [ - ({'sim': 'true', 'odom_source': 'sim_gt'}, 'FAST-LIO'), - ({'sim': 'true', 'odom_source': 'wheel'}, 'FAST-LIO'), -]) -def test_g1_invalid_modes_refused_before_actions(overrides, match): - with pytest.raises(RuntimeError, match=match): - pipeline('g1', **overrides) - - def test_g1_motion_limits_and_static_map_source_agree(): path = os.path.join(get_package_share_directory('golem_slam'), 'config', 'g1_nav2.yaml') assert os.path.exists(path), 'G1 Nav2 profile is missing' @@ -102,7 +90,7 @@ def test_g1_motion_limits_and_static_map_source_agree(): def test_g1_recovery_commands_use_velocity_smoother(): from launch.actions import GroupAction from launch_ros.actions import SetRemap - context, actions = pipeline('g1', sim='true') + context, actions = pipeline('g1') groups = [a for a in actions if isinstance(a, GroupAction)] assert len(groups) == 1, 'G1 Nav2 needs a scoped recovery remap' remaps = [a for a in groups[0].get_sub_entities() if isinstance(a, SetRemap)] diff --git a/tests/g1/README.md b/tests/g1/README.md index 1693d0ab..a580a2f2 100644 --- a/tests/g1/README.md +++ b/tests/g1/README.md @@ -103,7 +103,7 @@ must update the tests in the same commit. ## Output Each run writes to `tests/results/g1--