From fc7187cfe9d5c59221321a4f4e1153a895fb3145 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Thu, 23 Jul 2026 17:36:51 +0200 Subject: [PATCH 1/8] support real Kinova control with ft and gripper --- code-generator/introspection.stg | 30 ++- code-generator/main.stg | 108 ++++---- code-generator/mj_kdl_backend.stg | 16 +- code-generator/robif2b_backend.stg | 377 ++++++++++++++++++++++++++- code-generator/runtime.stg | 18 +- code-generator/solver.stg | 17 +- src/motion_spec/codegen.py | 9 + src/motion_spec/codegen_artifacts.py | 8 +- src/motion_spec/entities.py | 2 + src/motion_spec/ir_gen.py | 64 ++++- tests/test_cli.py | 1 + thirdparty/kinova/GEN3_URDF_V12.urdf | 63 +++-- 12 files changed, 600 insertions(+), 113 deletions(-) diff --git a/code-generator/introspection.stg b/code-generator/introspection.stg index d8da78c..028e1b7 100644 --- a/code-generator/introspection.stg +++ b/code-generator/introspection.stg @@ -353,6 +353,10 @@ public: : shm_(shm_name()), logger_(log_path(), kRuntimeProducerAgentId, kRuntimeActivityId) {} + // Flush the protobuf writer before a backend tears down its GUI or hardware + // resources. The destructor remains a second, harmless close for early exits. + void close() { logger_.close(); } + void begin_tick(double t, std::uint64_t step, std::int64_t active_motion) { begin_tick(t, step, active_motion, active_motion); \} @@ -485,8 +489,32 @@ introspection-controller-sample(slot, views) ::= << pub.constraint_sample(, , , , , motion_spec::runtime::constraint_satisfied()); >> +introspection-cond-term(t, motion_id) ::= << +<({introspection-cond-term-})(t, motion_id)> +>> + +introspection-cond-term-elapsed(t, motion_id) ::= << +(shared._motion_start_time ) +>> + +introspection-cond-term-constraint(t, motion_id) ::= << +motion_spec::runtime::constraint_satisfied(shared.) +>> + +introspection-cond-term-flag(t, motion_id) ::= << +_state_instance. +>> + +introspection-cond-term-event(t, motion_id) ::= << +_state_instance._event_triggered +>> + +introspection-bool-condition(terms, any, motion_id) ::= << +(}; separator=" || ">}; separator=" && ">) +>> + introspection-monitor-sample(slot) ::= << - pub.monitor_sample(, false, false); pub.monitor_sample(, motion_spec::runtime::constraint_error_value(), motion_spec::runtime::constraint_satisfied()); + pub.monitor_sample(, false, false); pub.monitor_sample(, motion_spec::runtime::constraint_error_value(), motion_spec::runtime::constraint_satisfied()); >> introspection-quantity-sample(slot, views) ::= << diff --git a/code-generator/main.stg b/code-generator/main.stg index 4ced975..1f52588 100644 --- a/code-generator/main.stg +++ b/code-generator/main.stg @@ -102,27 +102,32 @@ app-main-base-setup() ::= << }; >> -app-arm-includes(backend, has_arm) ::= << -<({app-arm-includes-})(has_arm)> +app-arm-includes(backend, has_arm, wrench_outputs, has_wrench_outputs) ::= << +<({app-arm-includes-})(has_arm, wrench_outputs, has_wrench_outputs)> >> -app-arm-setup(backend, arm_solvers, scene) ::= << -<({app-arm-setup-})(arm_solvers, scene)> +app-arm-setup(backend, arm_solvers, scene, wrench_outputs, has_wrench_outputs) ::= << +<({app-arm-setup-})(arm_solvers, scene, wrench_outputs, has_wrench_outputs)> >> app-loop-condition(backend, has_arm) ::= << <({app-loop-condition-})(has_arm)> >> -app-arm-cleanup(backend, has_arm) ::= << -<({app-arm-cleanup-})(has_arm)> +app-arm-cleanup(backend, has_arm, wrench_outputs, has_wrench_outputs) ::= << +<({app-arm-cleanup-})(has_arm, wrench_outputs, has_wrench_outputs)> >> -app-arm-post-setup(backend, arm_solvers, motions) ::= << -<({app-arm-post-setup-})(arm_solvers, motions)> +app-arm-start(backend, arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx) ::= << +<({app-arm-start-})(arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx)> >> -app-arm-post-setup-robif2b(arm_solvers, motions) ::= << +app-arm-post-setup(backend, arm_solvers, motions, wrench_outputs, has_wrench_outputs) ::= << +<({app-arm-post-setup-})(arm_solvers, motions, wrench_outputs, has_wrench_outputs)> +>> + +app-arm-post-setup-robif2b(arm_solvers, motions, wrench_outputs, has_wrench_outputs) ::= << + >> app-arm-loop-reset(backend, arm_solvers, fsm_namespace) ::= << @@ -132,8 +137,8 @@ app-arm-loop-reset(backend, arm_solvers, fsm_namespace) ::= << app-arm-loop-reset-robif2b(arm_solvers, fsm_namespace) ::= << >> -app-arm-runtime-step(backend, arm_solvers) ::= << -<({app-arm-runtime-step-})(arm_solvers)> +app-arm-runtime-step(backend, arm_solvers, wrench_outputs, has_wrench_outputs, has_wrench_data) ::= << +<({app-arm-runtime-step-})(arm_solvers, wrench_outputs, has_wrench_outputs, has_wrench_data)> >> clock-time-source(backend, arm_solvers, has_arm) ::= << @@ -157,33 +162,7 @@ app-headless-telemetry(backend, shared_data, arm_solvers, scene) ::= << <({app-headless-telemetry-})(shared_data, arm_solvers, scene)> >> -// ROS publish is decoupled from the control loop: each RealtimePublisher owns a background -// thread that does the DDS publish; the RT loop only does a non-blocking trylock/unlockAndPublish. -ros-includes(ros_publishers) ::= << -#include \ -#include \ -"}; separator="\n"> ->> - -ros-io-members(ros_publishers) ::= << - rclcpp::Node::SharedPtr ros_node = nullptr; -\>\> = nullptr;}; separator="\n"> ->> - -ros-setup(ros_publishers, ros_node_name) ::= << - rclcpp::init(argc, argv); - robot.ros_node = std::make_shared\(""); - = std::make_shared\\>\>( - robot.ros_node->create_publisher\<\>("", rclcpp::QoS(10)));}; separator="\n"> ->> - -ros-shutdown(ros_publishers) ::= << -.reset();}; separator="\n"> - robot.ros_node.reset(); - rclcpp::shutdown(); ->> - -app_main(motions, wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, needs_clock_time, control_period_ns) ::= << +app_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, needs_clock_time, control_period_ns) ::= << #include "runtime.hpp" #include "shared_state.hpp" #ifdef MOTION_SPEC_ENABLE_INTROSPECTION @@ -194,7 +173,7 @@ app_main(motions, wrench_outputs, has_arm, has_mobile_base, shared_schedule, clo }; separator="\n"> - + #include \ int main(int argc, char **argv) { @@ -206,7 +185,7 @@ int main(int argc, char **argv) { - + }; separator="\n"> = &_measurement;}; separator="\n"> @@ -243,14 +222,14 @@ int main(int argc, char **argv) { - + motion_spec::runtime::sleep_until_next(_next_tick, ); } - + }; separator="\n"> @@ -271,6 +250,9 @@ static void step_(_state &_state_instance, shar if (!_state_instance.active) { _state_instance.active = true; _state_instance.active_steps = 0; + shared._motion_start_time = shared.clock_time_s; + _state_instance.motion_start_time = shared.clock_time_s; + }; separator="\n"> } update_(_state_instance, shared, robot); @@ -302,7 +284,7 @@ fsm-dispatch(motions, fsm_namespace) ::= << \}; >> -ref_main(motions, wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, shared_data, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, fsm_namespace, fsm_step_event, fsm_step_event_idx, needs_clock_time, control_period_ns, has_ros, ros_publishers, ros_node_name) ::= << +ref_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, shared_data, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, fsm_namespace, fsm_step_event, fsm_step_event_idx, needs_clock_time, control_period_ns) ::= << #include "runtime.hpp" #include "shared_state.hpp" @@ -310,7 +292,7 @@ ref_main(motions, wrench_outputs, has_arm, has_mobile_base, shared_schedule, clo }; separator="\n"> - + #include \ @@ -320,17 +302,13 @@ ref_main(motions, wrench_outputs, has_arm, has_mobile_base, shared_schedule, clo int main(int argc, char **argv) { robot_io robot{}; shared_data shared{}; - - - - - + }; separator="\n"> = &_measurement;}; separator="\n"> @@ -350,7 +328,7 @@ int main(int argc, char **argv) { - + @@ -360,6 +338,13 @@ int main(int argc, char **argv) { + + + // Initialize the runtime clock before FSM priming so active-elapsed + // motions snapshot their start time in the same clock domain as the main loop. + shared.clock_time_s = ; + + int _step = 0; @@ -402,6 +387,8 @@ int main(int argc, char **argv) { if (!_state_instance.active && can_start_()) { _state_instance.active = true; _state_instance.active_steps = 0; + shared._motion_start_time = shared.clock_time_s; + _state_instance.motion_start_time = shared.clock_time_s; _state_instance._previous = false; @@ -435,7 +422,7 @@ int main(int argc, char **argv) { - + fsm_step_nbx(fsm); @@ -455,10 +442,13 @@ int main(int argc, char **argv) { ++_step; motion_spec::runtime::sleep_until_next(_next_tick, ); } +#ifdef MOTION_SPEC_ENABLE_INTROSPECTION + _introspection_pub.close(); +#endif - + }; separator="\n"> @@ -470,17 +460,13 @@ int main(int argc, char **argv) { ::destroy_fsm(fsm); - - - - return 0; } >> -shared_state_header(shared_data, wrench_outputs, has_arm, has_mobile_base, arm_solvers, base_velocity_solvers, base_force_solvers, backend, fsm_namespace, fsm_header, has_ros, ros_publishers) ::= << +shared_state_header(shared_data, wrench_outputs, has_arm, has_mobile_base, arm_solvers, base_velocity_solvers, base_force_solvers, backend, fsm_namespace, fsm_header, motions) ::= << #pragma once #include "frame_layout.h" @@ -492,9 +478,6 @@ shared_state_header(shared_data, wrench_outputs, has_arm, has_mobile_base, arm_s #include \ - - - #include \ #include \ #include \ @@ -627,9 +610,6 @@ struct robot_io { struct events *fsm_events = nullptr; - - - }; separator="\n"> }; separator="\n"> }; @@ -650,6 +630,8 @@ struct shared_data { ++_motion_spec_event_count; \} double clock_time_s = 0.0; // runtime clock seconds; MuJoCo sim time or real monotonic time + double _motion_start_time = -1.0; +}; separator="\n"> }; separator="\n"> }; >> diff --git a/code-generator/mj_kdl_backend.stg b/code-generator/mj_kdl_backend.stg index 41e6fdb..d5725e8 100644 --- a/code-generator/mj_kdl_backend.stg +++ b/code-generator/mj_kdl_backend.stg @@ -56,7 +56,7 @@ robot-shutdown-mj_kdl-KinovaGen3(solver) ::= << >> -app-arm-includes-mj_kdl(has_arm) ::= << +app-arm-includes-mj_kdl(has_arm, wrench_outputs, has_wrench_outputs) ::= << #ifdef MOTION_SPEC_ENABLE_INTROSPECTION #include "introspect_model.hpp" @@ -71,7 +71,7 @@ app-arm-includes-mj_kdl(has_arm) ::= << >> -app-arm-setup-mj_kdl(arm_solvers, scene) ::= << +app-arm-setup-mj_kdl(arm_solvers, scene, wrench_outputs, has_wrench_outputs) ::= << bool headless = false; int headless_steps = 2000; for (int i = 1; i \< argc; ++i) { @@ -258,7 +258,7 @@ app-arm-setup-mj_kdl(arm_solvers, scene) ::= << } >> -app-arm-post-setup-mj_kdl(arm_solvers, motions) ::= << +app-arm-post-setup-mj_kdl(arm_solvers, motions, wrench_outputs, has_wrench_outputs) ::= << mj_env.on_reset = [&](mj_kdl::ResetContext *) { const double _q_home[7] = {0.0, 0.2618, 3.1416, -2.2689, 0.0, 0.9599, 1.5708}; { @@ -290,18 +290,22 @@ app-loop-condition-mj_kdl(has_arm) ::= << (!headless || headless_steps-- > 0) >> -app-arm-cleanup-mj_kdl(has_arm) ::= << +app-arm-cleanup-mj_kdl(has_arm, wrench_outputs, has_wrench_outputs) ::= << if (!headless) { mj_kdl::cleanup(&mj_viewer); } mj_kdl::cleanup(&mj_env); >> -app-arm-runtime-step-mj_kdl(arm_solvers) ::= << +app-arm-start-mj_kdl(arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx) ::= << +>> + +app-arm-runtime-step-mj_kdl(arm_solvers, wrench_outputs, has_wrench_outputs, has_wrench_data) ::= << mj_kdl::update(&mj_); }; separator=""> if (!mj_kdl::step(&mj_)) { - std::exit(0); + std::cerr \<\< "mj_kdl viewer closed\n"; + break; } >> diff --git a/code-generator/robif2b_backend.stg b/code-generator/robif2b_backend.stg index 19bbd42..6fa43de 100644 --- a/code-generator/robif2b_backend.stg +++ b/code-generator/robif2b_backend.stg @@ -15,6 +15,7 @@ robot-state-struct-robif2b-KinovaGen3(solver) ::= << struct kinova_state { bool success = false; enum robif2b_ctrl_mode ctrl_mode = ROBIF2B_CTRL_MODE_FORCE; + double cycle_time = 0.001; double pos_msr[KINOVA_NUM_JOINTS] = {0.0}; double vel_msr[KINOVA_NUM_JOINTS] = {0.0}; double eff_msr[KINOVA_NUM_JOINTS] = {0.0}; @@ -23,6 +24,8 @@ struct kinova_state { double vel_cmd[KINOVA_NUM_JOINTS] = {0.0}; double eff_cmd[KINOVA_NUM_JOINTS] = {0.0}; double cur_cmd[KINOVA_NUM_JOINTS] = {0.0}; + double imu_ang_vel_msr[3] = {0.0}; + double imu_lin_acc_msr[3] = {0.0}; }; >> @@ -42,13 +45,15 @@ robot-state-init-robif2b-KinovaGen3(solver) ::= << robot-init-robif2b-KinovaGen3(solver) ::= << robif2b_kinova_gen3_nbx kinova_{}; - kinova_.conf.ip_address = ""; + const char *kinova__ip = std::getenv("MOTION_SPEC_KINOVA_IP"); + kinova_.conf.ip_address = (kinova__ip && *kinova__ip) ? kinova__ip : "192.168.1.12"; kinova_.conf.port = 10000; kinova_.conf.port_real_time = 10001; kinova_.conf.user = "admin"; kinova_.conf.password = "admin"; kinova_.conf.session_timeout = 60000; kinova_.conf.connection_timeout = 2000; + kinova_.cycle_time = &kinova__state.cycle_time; kinova_.ctrl_mode = &kinova__state.ctrl_mode; kinova_.jnt_pos_msr = &kinova__state.pos_msr[0]; kinova_.jnt_vel_msr = &kinova__state.vel_msr[0]; @@ -58,13 +63,37 @@ robot-init-robif2b-KinovaGen3(solver) ::= << kinova_.jnt_vel_cmd = &kinova__state.vel_cmd[0]; kinova_.jnt_trq_cmd = &kinova__state.eff_cmd[0]; kinova_.act_cur_cmd = &kinova__state.cur_cmd[0]; + kinova_.imu_ang_vel_msr = &kinova__state.imu_ang_vel_msr[0]; + kinova_.imu_lin_acc_msr = &kinova__state.imu_lin_acc_msr[0]; kinova_.success = &kinova__state.success; >> robot-chain-robif2b-KinovaGen3(solver) ::= << KDL::Chain chain_; - kinova_tree.getChain("", "", chain_); + std::string kinova_chain_root = ""; + std::string kinova_chain_end = ""; + if (!kinova_tree.getChain(kinova_chain_root, kinova_chain_end, chain_)) { + // URDF link names are case-sensitive. The scene model uses the canonical + // lower-case Kinova names, while the supplied Gen3 URDF uses names such as + // Shoulder_Link and Bracelet_Link. Resolve that naming-only difference before + // extracting the chain; the physical model and joint order remain unchanged. + const auto resolve_kinova_link = [&kinova_tree](std::string_view requested) { + for (const auto &entry : kinova_tree.getSegments()) { + if (motion_spec::runtime::runtime_name_matches(entry.first, requested)) { + return entry.first; + } + } + return std::string{}; + }; + kinova_chain_root = resolve_kinova_link(kinova_chain_root); + kinova_chain_end = resolve_kinova_link(kinova_chain_end); + } + if (kinova_chain_root.empty() || kinova_chain_end.empty() || + !kinova_tree.getChain(kinova_chain_root, kinova_chain_end, chain_)) { + std::cerr \<\< "Failed to extract the Kinova KDL chain\n"; + return 1; + \} >> @@ -73,13 +102,22 @@ robot-assign-robif2b-KinovaGen3(solver) ::= << .state = &kinova__state, .robot = &kinova_, .chain = &chain_, - }; + \}; >> robot-configure-robif2b-KinovaGen3(solver) ::= << + if (!kinova__state.success) { + std::cerr \<\< "robif2b Kinova configure failed\n"; + return 1; + \} - + if (!kinova__state.success) { + std::cerr \<\< "robif2b Kinova recover failed\n"; + kinova__state.success = true; + robif2b_kinova_gen3_shutdown(&kinova_); + return 1; + \} >> @@ -89,18 +127,39 @@ robot-shutdown-robif2b-KinovaGen3(solver) ::= << >> -app-arm-includes-robif2b(has_arm) ::= << +app-arm-includes-robif2b(has_arm, wrench_outputs, has_wrench_outputs) ::= << +#ifdef MOTION_SPEC_ENABLE_INTROSPECTION +#include "introspect_model.hpp" +#endif +#include \ +#include \ +#include \ +#include \ +#include \ #include \ #include \ #include \ + +#include \ +#include \ +#include \ +#include \ + >> -app-arm-setup-robif2b(arm_solvers, scene) ::= << +app-arm-setup-robif2b(arm_solvers, scene, wrench_outputs, has_wrench_outputs) ::= << urdf::ModelInterfaceSharedPtr kinova_model = urdf::parseURDFFile(""); + if (!kinova_model) { + std::cerr \<\< "Failed to parse Kinova URDF: \n"; + return 1; + \} KDL::Tree kinova_tree; - kdl_parser::treeFromUrdfModel(*kinova_model, kinova_tree); + if (!kdl_parser::treeFromUrdfModel(*kinova_model, kinova_tree)) { + std::cerr \<\< "Failed to convert Kinova URDF to a KDL tree\n"; + return 1; + \} @@ -111,21 +170,319 @@ app-arm-setup-robif2b(arm_solvers, scene) ::= << }; separator="\n"> + + struct robif2b_ft_runtime { + robif2b_robotiq_ft_sensor_nbx sensor{\}; + std::string port; + float force_msr[3] = {0.0f, 0.0f, 0.0f\}; + float force_offset[3] = {0.0f, 0.0f, 0.0f\}; + float moment_msr[3] = {0.0f, 0.0f, 0.0f\}; + float moment_offset[3] = {0.0f, 0.0f, 0.0f\}; + bool success = false; + std::atomic_bool stop{false\}; + std::atomic_bool error{false\}; + std::mutex mutex; + KDL::Wrench wrench; + std::thread worker; + bool active = false; + \} robif2b_ft; + + struct robif2b_gripper_runtime { + robif2b_robotiq_gripper_nbx driver{\}; + uint8_t position_msr = 0; + uint8_t position_cmd = 0; + uint8_t speed_cmd = 0x40; + uint8_t force_cmd = 0x40; + bool is_moving = false; + enum robif2b_robotiq_gripper_obj_status object_status = ROBIF2B_ROBOTIQ_OBJ_UNKNOWN; + enum robif2b_robotiq_gripper_status status = ROBIF2B_ROBOTIQ_UNKNOWN; + enum robif2b_robotiq_gripper_error error_code = ROBIF2B_ROBOTIQ_NO_ERROR; + bool success = false; + std::atomic_bool stop{false\}; + std::atomic_bool error{false\}; + std::thread worker; + bool active = false; + \} robif2b_gripper; + + const char *_devices_env = std::getenv("MOTION_SPEC_COMMUNICATION_DEVICES"); + const std::string _devices = (_devices_env && *_devices_env) ? _devices_env : "ft,gripper"; + const auto _selected = [&](const char *name) { + return _devices == "all" || _devices.find(std::string(name)) != std::string::npos; + }; + const bool _use_ft = _selected("ft"); + const bool _use_gripper = _selected("gripper"); + >> app-loop-condition-robif2b(has_arm) ::= << true >> -app-arm-runtime-step-robif2b(arm_solvers) ::= << - robif2b_kinova_gen3_update(&kinova_); +app-arm-runtime-step-robif2b(arm_solvers, wrench_outputs, has_wrench_outputs, has_wrench_data) ::= << + bool _torque_finite = true; + for (int i = 0; i \< KINOVA_NUM_JOINTS; ++i) { + if (!std::isfinite(kinova__state.eff_cmd[i])) { + _torque_finite = false; + \} + \} + if (!_torque_finite) { + std::cerr \<\< "invalid Kinova torque command\n"; + break; + \} + robif2b_kinova_gen3_update(&kinova_); + if (!kinova__state.success) { + std::cerr \<\< "robif2b Kinova update failed\n"; + break; + \} }; separator=""> + if (robif2b_ft.error.load() || robif2b_gripper.error.load()) { + std::cerr \<\< "robif2b peripheral update failed\n"; + break; + \} + + if (robot.ext_force != nullptr) { + std::lock_guard\ lock(robif2b_ft.mutex); + *robot.ext_force = robif2b_ft.wrench; + \} + + >> app-arm-trace-update-robif2b(arm_solvers, trace) ::= << >> -app-arm-cleanup-robif2b(has_arm) ::= << +robif2b-stop-arm(arm_solvers) ::= << + robif2b_kinova_gen3_stop(&kinova_); + robif2b_kinova_gen3_shutdown(&kinova_); +}; separator=""> +>> + +robif2b-hardware-post-setup(arm_solvers, wrench_outputs, has_wrench_outputs) ::= << + + if (_use_ft) { + const char *ft_port = std::getenv("MOTION_SPEC_ROBOTIQ_FT_PORT"); + robif2b_ft.port = (ft_port && *ft_port) ? ft_port : "ttyUSB0"; + if (robif2b_ft.port.rfind("/dev/", 0) == 0) { + robif2b_ft.port.erase(0, 5); + } + robif2b_ft.sensor.serial.port = robif2b_ft.port.c_str(); + robif2b_ft.sensor.serial.baudrate = 19200; + robif2b_ft.sensor.serial.timeout_ms = 1.0; + robif2b_ft.sensor.serial.slave_address = 0x09; + robif2b_ft.sensor.force_msr = &robif2b_ft.force_msr[0]; + robif2b_ft.sensor.force_offset = &robif2b_ft.force_offset[0]; + robif2b_ft.sensor.moment_msr = &robif2b_ft.moment_msr[0]; + robif2b_ft.sensor.moment_offset = &robif2b_ft.moment_offset[0]; + robif2b_ft.sensor.success = &robif2b_ft.success; + robif2b_robotiq_ft_configure(&robif2b_ft.sensor); + if (!robif2b_ft.success) { + std::cerr \<\< "Robotiq FT sensor configure failed\n"; + + return 1; + \} + float force_sum[3] = {0.0f, 0.0f, 0.0f\}; + float moment_sum[3] = {0.0f, 0.0f, 0.0f\}; + for (int sample = 0; sample \< 100; ++sample) { + robif2b_robotiq_ft_update(&robif2b_ft.sensor); + if (!robif2b_ft.success) { + std::cerr \<\< "Robotiq FT sensor zeroing failed\n"; + robif2b_robotiq_ft_shutdown(&robif2b_ft.sensor); + + return 1; + \} + for (int axis = 0; axis \< 3; ++axis) { + force_sum[axis] += robif2b_ft.force_msr[axis]; + moment_sum[axis] += robif2b_ft.moment_msr[axis]; + \} + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + \} + for (int axis = 0; axis \< 3; ++axis) { + robif2b_ft.force_offset[axis] = force_sum[axis] / 100.0f; + robif2b_ft.moment_offset[axis] = moment_sum[axis] / 100.0f; + \} + robif2b_ft.worker = std::thread([&robif2b_ft]() { + while (!robif2b_ft.stop.load()) { + robif2b_robotiq_ft_update(&robif2b_ft.sensor); + if (!robif2b_ft.success) { + robif2b_ft.error.store(true); + break; + \} + { + std::lock_guard\ lock(robif2b_ft.mutex); + robif2b_ft.wrench = KDL::Wrench( + KDL::Vector(robif2b_ft.force_msr[0], robif2b_ft.force_msr[1], robif2b_ft.force_msr[2]), + KDL::Vector(robif2b_ft.moment_msr[0], robif2b_ft.moment_msr[1], robif2b_ft.moment_msr[2])); + \} + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + \} + \}); + robif2b_ft.active = true; + } + + if (_use_gripper) { + const char *gripper_port = std::getenv("MOTION_SPEC_ROBOTIQ_GRIPPER_PORT"); + robif2b_gripper.driver.serial.port = (gripper_port && *gripper_port) ? gripper_port : "/dev/ttyUSB1"; + robif2b_gripper.driver.serial.baudrate = 115200; + robif2b_gripper.driver.serial.timeout_ms = 1.0; + robif2b_gripper.driver.serial.slave_address = 0x09; + robif2b_gripper.driver.position_msr = &robif2b_gripper.position_msr; + robif2b_gripper.driver.is_gripper_moving = &robif2b_gripper.is_moving; + robif2b_gripper.driver.obj_detection_status = &robif2b_gripper.object_status; + robif2b_gripper.driver.gripper_status = &robif2b_gripper.status; + robif2b_gripper.driver.position_cmd = &robif2b_gripper.position_cmd; + robif2b_gripper.driver.speed_cmd = &robif2b_gripper.speed_cmd; + robif2b_gripper.driver.force_cmd = &robif2b_gripper.force_cmd; + robif2b_gripper.driver.error = &robif2b_gripper.error_code; + robif2b_gripper.driver.success = &robif2b_gripper.success; + robif2b_robotiq_gripper_configure(&robif2b_gripper.driver); + if (!robif2b_gripper.success) { + std::cerr \<\< "Robotiq gripper configure failed\n"; + if (robif2b_ft.active) { + robif2b_ft.stop.store(true); + if (robif2b_ft.worker.joinable()) robif2b_ft.worker.join(); + robif2b_robotiq_ft_shutdown(&robif2b_ft.sensor); + } + + return 1; + \} + robif2b_gripper.position_cmd = robif2b_gripper.position_msr; + robif2b_gripper.worker = std::thread([&robif2b_gripper]() { + while (!robif2b_gripper.stop.load()) { + robif2b_robotiq_gripper_update(&robif2b_gripper.driver); + if (!robif2b_gripper.success) { + robif2b_gripper.error.store(true); + break; + \} + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + \} + \}); + robif2b_gripper.active = true; + } + +>> + +app-arm-cleanup-robif2b(has_arm, wrench_outputs, has_wrench_outputs) ::= << + + if (robif2b_gripper.active) { + robif2b_gripper.stop.store(true); + if (robif2b_gripper.worker.joinable()) robif2b_gripper.worker.join(); + robif2b_robotiq_gripper_shutdown(&robif2b_gripper.driver); + robif2b_gripper.active = false; + \} + if (robif2b_ft.active) { + robif2b_ft.stop.store(true); + if (robif2b_ft.worker.joinable()) robif2b_ft.worker.join(); + robif2b_robotiq_ft_shutdown(&robif2b_ft.sensor); + robif2b_ft.active = false; + \} + +>> + +app-arm-start-robif2b(arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx) ::= << + + // Prime the FSM while the Kinova is still in its configured position state. This + // snapshots the current TCP pose and computes the first bounded ACHD torque. + produce_event(fsm->eventData, ::); + fsm_step_nbx(fsm); + reconfig_event_buffers(fsm->eventData); + fsm_dispatch(fsm); + + + + if (!kinova__state.success) { + std::cerr \<\< "robif2b Kinova start failed\n"; + + return 1; + \} + for (int i = 0; i \< KINOVA_NUM_JOINTS; ++i) { + if (!std::isfinite(kinova__state.eff_cmd[i])) { + std::cerr \<\< "invalid initial Kinova torque command\n"; + + return 1; + \} + \} + + if (!kinova__state.success) { + std::cerr \<\< "robif2b Kinova initial torque update failed\n"; + + return 1; + \} +}; separator="\n"> +>> + +cmake_robif2b(motions, fsm_namespace, wrench_outputs, has_wrench_outputs) ::= << +cmake_minimum_required(VERSION 3.16) +project(motion_spec_robif2b_target LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(Eigen3 REQUIRED) +find_package(orocos_kdl REQUIRED CONFIG) +find_package(urdfdom_headers REQUIRED) +find_package(urdfdom REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(robif2b REQUIRED) +find_package(Threads REQUIRED) + +find_package(serial REQUIRED) +find_package(robotiq_driver_noros REQUIRED) +find_package(robotiq_ft REQUIRED) + + +find_package(coord2b REQUIRED) + + +option(MOTION_SPEC_ENABLE_INTROSPECTION "Enable generated runtime introspection logging" OFF) + +add_executable(main + ref_main.cpp +) +target_include_directories(main PRIVATE + ${EIGEN3_INCLUDE_DIRS} + ${orocos_kdl_INCLUDE_DIRS} + ${urdfdom_headers_INCLUDE_DIRS} + ${urdfdom_INCLUDE_DIRS} + ${kdl_parser_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_SOURCE_DIR}/headers + + ${coord2b_INCLUDE_DIRS} + +) +target_link_libraries(main PRIVATE + robif2b::kinova_gen3 + + robif2b::robotiq_ft_sensor + robif2b::robotiq_gripper + serial::serial + robotiq::robotiq_driver_noros + robotiq::robotiq_ft_driver + + orocos-kdl + urdfdom_headers::urdfdom_headers + urdfdom::urdf_parser + Threads::Threads + + ${coord2b_LIBRARIES} + +) +if(TARGET kdl_parser::kdl_parser) + target_link_libraries(main PRIVATE kdl_parser::kdl_parser) +elseif(TARGET kdl_parser) + target_link_libraries(main PRIVATE kdl_parser) +else() + target_link_libraries(main PRIVATE kdl_parser) +endif() +if(MOTION_SPEC_ENABLE_INTROSPECTION) + find_package(Protobuf REQUIRED) + target_sources(main PRIVATE frame_log.pb.cc) + target_include_directories(main PRIVATE ${Protobuf_INCLUDE_DIRS}) + target_compile_definitions(main PRIVATE MOTION_SPEC_ENABLE_INTROSPECTION) + target_link_libraries(main PRIVATE protobuf::libprotobuf) +endif() +target_compile_options(main PRIVATE -Wall -Wextra -ftemplate-depth=2048) >> robif2b-include-ethercat() ::= << diff --git a/code-generator/runtime.stg b/code-generator/runtime.stg index 3391c76..b89a8c9 100644 --- a/code-generator/runtime.stg +++ b/code-generator/runtime.stg @@ -7,6 +7,7 @@ runtime_header(has_mobile_base, control_period_ns) ::= << #include \ #include \ #include \ +#include \ #include \ #include \ #include \ @@ -80,7 +81,7 @@ inline double smoothstep(double alpha) { inline double monotonic_time_s() { using clock = std::chrono::steady_clock; static const auto start = clock::now(); - return std::chrono::duration(clock::now() - start).count(); + return std::chrono::duration\(clock::now() - start).count(); } inline bool timespec_less(const timespec &a, const timespec &b) { @@ -448,8 +449,19 @@ inline void warn_produce_event_not_implemented(std::string_view event_id) { } inline bool runtime_name_matches(std::string_view actual, std::string_view target) { - return actual == target || - (actual.size() > target.size() && actual.substr(actual.size() - target.size()) == target); + if (target.empty()) { + return false; + } + const auto equal_ignore_case = [](std::string_view lhs, std::string_view rhs) { + return lhs.size() == rhs.size() && std::equal(lhs.begin(), lhs.end(), rhs.begin(), + [](char a, char b) { + return std::tolower(static_cast\(a)) == + std::tolower(static_cast\(b)); + }); + }; + return equal_ignore_case(actual, target) || + (actual.size() > target.size() && + equal_ignore_case(actual.substr(actual.size() - target.size()), target)); } inline unsigned int find_joint_index(const KDL::Chain &chain, std::string_view joint_name) { diff --git a/code-generator/solver.stg b/code-generator/solver.stg index 05f89b6..20b2aa7 100644 --- a/code-generator/solver.stg +++ b/code-generator/solver.stg @@ -260,8 +260,9 @@ solver-output-Wrench-mj_kdl(solver, out) ::= << >> solver-output-Wrench-robif2b(solver, out) ::= << - // robif2b has no sim FT sensor; a real build wires a hardware FT driver here. - shared..force = KDL::Vector::Zero(); + if (robot. != nullptr) { + shared. = *robot.; + } >> solver-output(solver, out, backend) ::= << @@ -417,14 +418,20 @@ solver-stage-output(solver, backend) ::= << >> solver-stage-output-robif2b(solver) ::= << for (int i = 0; i \< state..num_joints; ++i) { - robot..state->eff_cmd[i] = state..tau_ctrl(i); + const double _tau_cmd = motion_spec::runtime::clamp_range(state..tau_ctrl(i), , ); + const double _tau_cmd = state..tau_ctrl(i); + +) shared. = _tau_cmd;}; separator="\n"> + robot..state->eff_cmd[i] = _tau_cmd; } >> solver-stage-output-mj_kdl(solver) ::= << for (int i = 0; i \< state..num_joints; ++i) { - robot..robot->jnt_trq_cmd[i] = motion_spec::runtime::clamp_range(state..tau_ctrl(i), , ); - robot..robot->jnt_trq_cmd[i] = state..tau_ctrl(i); + const double _tau_cmd = motion_spec::runtime::clamp_range(state..tau_ctrl(i), , ); + const double _tau_cmd = state..tau_ctrl(i); +) shared. = _tau_cmd;}; separator="\n"> + robot..robot->jnt_trq_cmd[i] = _tau_cmd; } >> diff --git a/src/motion_spec/codegen.py b/src/motion_spec/codegen.py index 4e07af7..dc1276a 100644 --- a/src/motion_spec/codegen.py +++ b/src/motion_spec/codegen.py @@ -81,6 +81,12 @@ def _source_path(relative_path: Path) -> Path | None: def resource_path(relative_path: Path) -> Path: """Absolute path to a packaged resource (installed dist or source tree); raises if missing.""" + # Editable checkouts keep templates in the source tree. Prefer those files so + # template changes are used immediately instead of a stale copied resource. + if _source_root_from_distribution() is not None: + source_path = _source_path(relative_path) + if source_path is not None: + return source_path path = _distribution_path(relative_path) or _source_path(relative_path) if path is not None: return path @@ -245,6 +251,7 @@ def generate_code(ir_path: Path, output_dir: Path, stst_bin: str): "closures": ir["closures"], "views": ir["views"], "wrench_outputs": ir["wrench_outputs"], + "has_wrench_outputs": ir.get("has_wrench_outputs", bool(ir["wrench_outputs"])), "base_velocity_solvers": ir["base_velocity_solvers"], "base_force_solvers": ir["base_force_solvers"], "has_mobile_base": ir["has_mobile_base"], @@ -261,6 +268,8 @@ def generate_code(ir_path: Path, output_dir: Path, stst_bin: str): render_template(stst_bin, "ref_main", ir_payload_path, output_dir / "ref_main.cpp") if ir["backend"] == "mj_kdl": render_template(stst_bin, "cmake_mj_kdl", ir_payload_path, output_dir / "CMakeLists.txt") + elif ir["backend"] == "robif2b": + render_template(stst_bin, "cmake_robif2b", ir_payload_path, output_dir / "CMakeLists.txt") def main(argv: list[str] | None = None): diff --git a/src/motion_spec/codegen_artifacts.py b/src/motion_spec/codegen_artifacts.py index 4d4fb2b..27af4ae 100644 --- a/src/motion_spec/codegen_artifacts.py +++ b/src/motion_spec/codegen_artifacts.py @@ -541,6 +541,7 @@ def build_introspection_model(schema: dict, ir: dict) -> dict: "active_terms": slot.get("active_terms"), "active_terms_present": slot.get("active_terms_present", False), "active_any": slot.get("active_any", False), + "motion": state.get("motion"), } ) else: @@ -554,7 +555,12 @@ def build_introspection_model(schema: dict, ir: dict) -> dict: } ) states.append( - {"index": state.get("index", -1), "controllers": controllers, "monitors": monitors} + { + "index": state.get("index", -1), + "motion": state.get("motion"), + "controllers": controllers, + "monitors": monitors, + } ) spatial = schema.get("spatial", {"poses": [], "twists": [], "wrenches": []}) return { diff --git a/src/motion_spec/entities.py b/src/motion_spec/entities.py index 064f0e9..819cd6f 100644 --- a/src/motion_spec/entities.py +++ b/src/motion_spec/entities.py @@ -750,6 +750,7 @@ class HandlerArmSolver: chain_root: str = "" chain_end: str = "" torque_saturation: Saturation | None = None + commanded_torque_samples: list = field(default_factory=list) runtime_id: str = "" runtime_owner: bool = False type: str = field(default="HandlerArmSolver") @@ -767,6 +768,7 @@ class SolverWithInputAndOutput: urdf: str = "" chain_root: str = "" chain_end: str = "" + commanded_torque_samples: list = field(default_factory=list) chain_tip: str = "" robot_model: str = "" tool_body: str = "" diff --git a/src/motion_spec/ir_gen.py b/src/motion_spec/ir_gen.py index 0989ea6..749446f 100644 --- a/src/motion_spec/ir_gen.py +++ b/src/motion_spec/ir_gen.py @@ -2447,6 +2447,7 @@ def _arm_solvers_for_handler(handler, slv_arm, solver_ids): chain_root=solver.chain_root, chain_end=solver.chain_end, torque_saturation=solver.torque_saturation, + commanded_torque_samples=solver.commanded_torque_samples, ) ) @@ -3931,6 +3932,7 @@ def _build_introspection( closures, views, shared_data, + arm_solvers, ): """Build the introspection artifact (uris, motions, controllers, monitors, quantities, provenance) and fold in the controller-state and frame-log samples. @@ -4172,6 +4174,7 @@ def _build_introspection( # then the frame-log quantity/spatial samples that read them. _annotate_controller_signals(introspection["controllers"], closures) add_controller_internal_state_logging(closures, shared_data, introspection, motions) + add_solver_command_torque_logging(arm_solvers, motions, shared_data, introspection) add_quantity_samples(introspection, shared_data, views) add_spatial_samples(introspection, shared_data) return introspection @@ -5071,6 +5074,59 @@ def add_quantity(item_id: str, controller_id: str, state_name: str) -> None: closure["internal_state_samples"] = closure_samples +KINOVA_NUM_JOINTS = 7 + + +def add_solver_command_torque_logging( + arm_solvers: list, motions: list, shared_data: list, introspection: dict +) -> None: + """Add post-saturation per-joint torque channels to the frame log.""" + quantities = introspection.setdefault("quantities", []) + shared_ids = {_field(item, "id") for item in shared_data if _field(item, "id")} + quantity_ids = {_field(item, "id") for item in quantities if _field(item, "id")} + samples_by_solver = {} + + for solver in arm_solvers: + samples = [] + for joint_index in range(KINOVA_NUM_JOINTS): + sample_id = f"commanded_torque_{solver.id}_joint_{joint_index + 1}" + samples.append({"id": sample_id, "joint_index": joint_index}) + if sample_id not in shared_ids: + shared_data.append( + { + "id": sample_id, + "type": "Quantity", + "role": "commanded_joint_torque", + "solver": solver.id, + "joint_index": joint_index, + } + ) + shared_ids.add(sample_id) + if sample_id not in quantity_ids: + quantities.append( + { + "id": sample_id, + "type": "Quantity", + "unit": ["N_M"], + "quantity_kind": ["Torque"], + "role": "commanded_joint_torque", + "solver": solver.id, + "joint_index": joint_index, + } + ) + quantity_ids.add(sample_id) + _set_field(solver, "commanded_torque_samples", samples) + samples_by_solver[solver.id] = samples + + for motion in motions: + for solver in _field(motion, "arm_solvers", []) or []: + _set_field( + solver, + "commanded_torque_samples", + samples_by_solver.get(_field(solver, "id"), []), + ) + + def add_quantity_samples(introspection: dict, shared_data: list, views: dict) -> None: """Build the per-quantity frame-log sample descriptors from the introspection quantities and shared data. @@ -5484,7 +5540,7 @@ def add_motion_function_interfaces(motions: list) -> None: _set_field(motion, "monitor_needs_robot", when_fsm or until_fsm) _set_field(motion, "apply_needs_state", has_arm) - _set_field(motion, "apply_needs_shared", has_forwarded_commands) + _set_field(motion, "apply_needs_shared", has_forwarded_commands or has_arm) _set_field(motion, "apply_needs_robot", has_arm or has_forwarded_commands) @@ -5753,6 +5809,9 @@ def generate_ir(manifest_path): if item.type == "Wrench" and item.id not in closure_output_map ] ) + # A declared FT sensor must initialize the real peripheral backend even when + # its wrench is monitoring-only and is not consumed by a solver constraint. + has_ft_sensor = any(g.triples((None, RDF.type, SENSORS.ForceTorqueSensor)) ) motions, fsm_meta = build_motion_units( g, @@ -5808,6 +5867,7 @@ def generate_ir(manifest_path): closures=closures, views=view_map, shared_data=shared_data, + arm_solvers=slv_arm, ) schedule = sched1 + sched2 + sched3 + sched4 @@ -5852,6 +5912,8 @@ def generate_ir(manifest_path): data_structures, pose_components ), "wrench_outputs": wrench_outputs, + "has_wrench_data": bool(wrench_outputs), + "has_wrench_outputs": bool(wrench_outputs) or has_ft_sensor, "has_arm": bool(slv_arm), "has_mobile_base": bool(slv_base_vel or slv_base_frc), "has_ros": bool(ros_publishers), diff --git a/tests/test_cli.py b/tests/test_cli.py index c8c9f24..9913a13 100644 --- a/tests/test_cli.py +++ b/tests/test_cli.py @@ -92,6 +92,7 @@ def build(generation, *, prefixes, jobs): assert result.exit_code == 0 assert received["stages"] == ["ir", "code"] assert received["run"][1]["executable_args"] == ["--headless", "--steps", "10"] + assert received["run"][1]["recover_runtime_ttl"] is True assert str(run_generation / "runs" / "run-1") in result.output diff --git a/thirdparty/kinova/GEN3_URDF_V12.urdf b/thirdparty/kinova/GEN3_URDF_V12.urdf index 7b3f242..c131e32 100644 --- a/thirdparty/kinova/GEN3_URDF_V12.urdf +++ b/thirdparty/kinova/GEN3_URDF_V12.urdf @@ -1,4 +1,9 @@ + + + + + @@ -21,7 +26,7 @@ - + @@ -46,11 +51,11 @@ - + - + @@ -74,12 +79,12 @@ - - + + - + @@ -103,12 +108,12 @@ - - + + - + @@ -132,12 +137,12 @@ - - + + - + @@ -161,12 +166,12 @@ - - + + - + @@ -190,12 +195,12 @@ - - + + - + @@ -219,16 +224,28 @@ - - + + - + + + + + + + - - + + + + + + + + From 63dd268b1c62435a0f5cc9b15e1a320a33ea0084 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Thu, 23 Jul 2026 18:12:38 +0200 Subject: [PATCH 2/8] Preserve ROS publishing in generated controllers --- code-generator/main.stg | 44 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/code-generator/main.stg b/code-generator/main.stg index 1f52588..0adf29a 100644 --- a/code-generator/main.stg +++ b/code-generator/main.stg @@ -162,6 +162,32 @@ app-headless-telemetry(backend, shared_data, arm_solvers, scene) ::= << <({app-headless-telemetry-})(shared_data, arm_solvers, scene)> >> +// ROS publish is decoupled from the control loop: each RealtimePublisher owns a background +// thread that does the DDS publish; the RT loop only does a non-blocking trylock/unlockAndPublish. +ros-includes(ros_publishers) ::= << +#include \ +#include \ +"}; separator="\n"> +>> + +ros-io-members(ros_publishers) ::= << + rclcpp::Node::SharedPtr ros_node = nullptr; +\>\> = nullptr;}; separator="\n"> +>> + +ros-setup(ros_publishers, ros_node_name) ::= << + rclcpp::init(argc, argv); + robot.ros_node = std::make_shared\(""); + = std::make_shared\\>\>( + robot.ros_node->create_publisher\<\>("", rclcpp::QoS(10)));}; separator="\n"> +>> + +ros-shutdown(ros_publishers) ::= << +.reset();}; separator="\n"> + robot.ros_node.reset(); + rclcpp::shutdown(); +>> + app_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, needs_clock_time, control_period_ns) ::= << #include "runtime.hpp" #include "shared_state.hpp" @@ -284,7 +310,7 @@ fsm-dispatch(motions, fsm_namespace) ::= << \}; >> -ref_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, shared_data, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, fsm_namespace, fsm_step_event, fsm_step_event_idx, needs_clock_time, control_period_ns) ::= << +ref_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, shared_data, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, fsm_namespace, fsm_step_event, fsm_step_event_idx, needs_clock_time, control_period_ns, has_ros, ros_publishers, ros_node_name) ::= << #include "runtime.hpp" #include "shared_state.hpp" @@ -302,6 +328,10 @@ ref_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, int main(int argc, char **argv) { robot_io robot{}; shared_data shared{}; + + + + @@ -460,13 +490,17 @@ int main(int argc, char **argv) { ::destroy_fsm(fsm); + + + + return 0; } >> -shared_state_header(shared_data, wrench_outputs, has_arm, has_mobile_base, arm_solvers, base_velocity_solvers, base_force_solvers, backend, fsm_namespace, fsm_header, motions) ::= << +shared_state_header(shared_data, wrench_outputs, has_arm, has_mobile_base, arm_solvers, base_velocity_solvers, base_force_solvers, backend, fsm_namespace, fsm_header, motions, has_ros, ros_publishers) ::= << #pragma once #include "frame_layout.h" @@ -478,6 +512,9 @@ shared_state_header(shared_data, wrench_outputs, has_arm, has_mobile_base, arm_s #include \ + + + #include \ #include \ #include \ @@ -610,6 +647,9 @@ struct robot_io { struct events *fsm_events = nullptr; + + + }; separator="\n"> }; separator="\n"> }; From 5b84d0a96b8773c75c6808b6b532b1606952b765 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Thu, 23 Jul 2026 18:32:34 +0200 Subject: [PATCH 3/8] Use automatic wrench outputs in backend generation --- code-generator/main.stg | 46 +++++++++++++++--------------- code-generator/mj_kdl_backend.stg | 10 +++---- code-generator/robif2b_backend.stg | 28 +++++++++--------- src/motion_spec/codegen.py | 1 - src/motion_spec/ir_gen.py | 6 ---- 5 files changed, 41 insertions(+), 50 deletions(-) diff --git a/code-generator/main.stg b/code-generator/main.stg index 0adf29a..e164dbc 100644 --- a/code-generator/main.stg +++ b/code-generator/main.stg @@ -102,32 +102,32 @@ app-main-base-setup() ::= << }; >> -app-arm-includes(backend, has_arm, wrench_outputs, has_wrench_outputs) ::= << -<({app-arm-includes-})(has_arm, wrench_outputs, has_wrench_outputs)> +app-arm-includes(backend, has_arm, wrench_outputs) ::= << +<({app-arm-includes-})(has_arm, wrench_outputs)> >> -app-arm-setup(backend, arm_solvers, scene, wrench_outputs, has_wrench_outputs) ::= << -<({app-arm-setup-})(arm_solvers, scene, wrench_outputs, has_wrench_outputs)> +app-arm-setup(backend, arm_solvers, scene, wrench_outputs) ::= << +<({app-arm-setup-})(arm_solvers, scene, wrench_outputs)> >> app-loop-condition(backend, has_arm) ::= << <({app-loop-condition-})(has_arm)> >> -app-arm-cleanup(backend, has_arm, wrench_outputs, has_wrench_outputs) ::= << -<({app-arm-cleanup-})(has_arm, wrench_outputs, has_wrench_outputs)> +app-arm-cleanup(backend, has_arm, wrench_outputs) ::= << +<({app-arm-cleanup-})(has_arm, wrench_outputs)> >> app-arm-start(backend, arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx) ::= << <({app-arm-start-})(arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx)> >> -app-arm-post-setup(backend, arm_solvers, motions, wrench_outputs, has_wrench_outputs) ::= << -<({app-arm-post-setup-})(arm_solvers, motions, wrench_outputs, has_wrench_outputs)> +app-arm-post-setup(backend, arm_solvers, motions, wrench_outputs) ::= << +<({app-arm-post-setup-})(arm_solvers, motions, wrench_outputs)> >> -app-arm-post-setup-robif2b(arm_solvers, motions, wrench_outputs, has_wrench_outputs) ::= << - +app-arm-post-setup-robif2b(arm_solvers, motions, wrench_outputs) ::= << + >> app-arm-loop-reset(backend, arm_solvers, fsm_namespace) ::= << @@ -137,8 +137,8 @@ app-arm-loop-reset(backend, arm_solvers, fsm_namespace) ::= << app-arm-loop-reset-robif2b(arm_solvers, fsm_namespace) ::= << >> -app-arm-runtime-step(backend, arm_solvers, wrench_outputs, has_wrench_outputs, has_wrench_data) ::= << -<({app-arm-runtime-step-})(arm_solvers, wrench_outputs, has_wrench_outputs, has_wrench_data)> +app-arm-runtime-step(backend, arm_solvers, wrench_outputs) ::= << +<({app-arm-runtime-step-})(arm_solvers, wrench_outputs)> >> clock-time-source(backend, arm_solvers, has_arm) ::= << @@ -188,7 +188,7 @@ ros-shutdown(ros_publishers) ::= << rclcpp::shutdown(); >> -app_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, needs_clock_time, control_period_ns) ::= << +app_main(motions, wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, needs_clock_time, control_period_ns) ::= << #include "runtime.hpp" #include "shared_state.hpp" #ifdef MOTION_SPEC_ENABLE_INTROSPECTION @@ -199,7 +199,7 @@ app_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, }; separator="\n"> - + #include \ int main(int argc, char **argv) { @@ -211,7 +211,7 @@ int main(int argc, char **argv) { - + }; separator="\n"> = &_measurement;}; separator="\n"> @@ -248,14 +248,14 @@ int main(int argc, char **argv) { - + motion_spec::runtime::sleep_until_next(_next_tick, ); } - + }; separator="\n"> @@ -310,7 +310,7 @@ fsm-dispatch(motions, fsm_namespace) ::= << \}; >> -ref_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, shared_data, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, fsm_namespace, fsm_step_event, fsm_step_event_idx, needs_clock_time, control_period_ns, has_ros, ros_publishers, ros_node_name) ::= << +ref_main(motions, wrench_outputs, has_arm, has_mobile_base, shared_schedule, closures, views, shared_data, arm_solvers, base_velocity_solvers, base_force_solvers, backend, scene, trace, fsm_namespace, fsm_step_event, fsm_step_event_idx, needs_clock_time, control_period_ns, has_ros, ros_publishers, ros_node_name) ::= << #include "runtime.hpp" #include "shared_state.hpp" @@ -318,7 +318,7 @@ ref_main(motions, wrench_outputs, has_wrench_outputs, has_arm, has_mobile_base, }; separator="\n"> - + #include \ @@ -338,7 +338,7 @@ int main(int argc, char **argv) { - + }; separator="\n"> = &_measurement;}; separator="\n"> @@ -358,7 +358,7 @@ int main(int argc, char **argv) { - + @@ -452,7 +452,7 @@ int main(int argc, char **argv) { - + fsm_step_nbx(fsm); @@ -478,7 +478,7 @@ int main(int argc, char **argv) { - + }; separator="\n"> diff --git a/code-generator/mj_kdl_backend.stg b/code-generator/mj_kdl_backend.stg index d5725e8..be6c7fc 100644 --- a/code-generator/mj_kdl_backend.stg +++ b/code-generator/mj_kdl_backend.stg @@ -56,7 +56,7 @@ robot-shutdown-mj_kdl-KinovaGen3(solver) ::= << >> -app-arm-includes-mj_kdl(has_arm, wrench_outputs, has_wrench_outputs) ::= << +app-arm-includes-mj_kdl(has_arm, wrench_outputs) ::= << #ifdef MOTION_SPEC_ENABLE_INTROSPECTION #include "introspect_model.hpp" @@ -71,7 +71,7 @@ app-arm-includes-mj_kdl(has_arm, wrench_outputs, has_wrench_outputs) ::= << >> -app-arm-setup-mj_kdl(arm_solvers, scene, wrench_outputs, has_wrench_outputs) ::= << +app-arm-setup-mj_kdl(arm_solvers, scene, wrench_outputs) ::= << bool headless = false; int headless_steps = 2000; for (int i = 1; i \< argc; ++i) { @@ -258,7 +258,7 @@ app-arm-setup-mj_kdl(arm_solvers, scene, wrench_outputs, has_wrench_outputs) ::= } >> -app-arm-post-setup-mj_kdl(arm_solvers, motions, wrench_outputs, has_wrench_outputs) ::= << +app-arm-post-setup-mj_kdl(arm_solvers, motions, wrench_outputs) ::= << mj_env.on_reset = [&](mj_kdl::ResetContext *) { const double _q_home[7] = {0.0, 0.2618, 3.1416, -2.2689, 0.0, 0.9599, 1.5708}; { @@ -290,7 +290,7 @@ app-loop-condition-mj_kdl(has_arm) ::= << (!headless || headless_steps-- > 0) >> -app-arm-cleanup-mj_kdl(has_arm, wrench_outputs, has_wrench_outputs) ::= << +app-arm-cleanup-mj_kdl(has_arm, wrench_outputs) ::= << if (!headless) { mj_kdl::cleanup(&mj_viewer); } @@ -300,7 +300,7 @@ app-arm-cleanup-mj_kdl(has_arm, wrench_outputs, has_wrench_outputs) ::= << app-arm-start-mj_kdl(arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event_idx) ::= << >> -app-arm-runtime-step-mj_kdl(arm_solvers, wrench_outputs, has_wrench_outputs, has_wrench_data) ::= << +app-arm-runtime-step-mj_kdl(arm_solvers, wrench_outputs) ::= << mj_kdl::update(&mj_); }; separator=""> if (!mj_kdl::step(&mj_)) { diff --git a/code-generator/robif2b_backend.stg b/code-generator/robif2b_backend.stg index 6fa43de..3424f25 100644 --- a/code-generator/robif2b_backend.stg +++ b/code-generator/robif2b_backend.stg @@ -127,7 +127,7 @@ robot-shutdown-robif2b-KinovaGen3(solver) ::= << >> -app-arm-includes-robif2b(has_arm, wrench_outputs, has_wrench_outputs) ::= << +app-arm-includes-robif2b(has_arm, wrench_outputs) ::= << #ifdef MOTION_SPEC_ENABLE_INTROSPECTION #include "introspect_model.hpp" @@ -140,7 +140,7 @@ app-arm-includes-robif2b(has_arm, wrench_outputs, has_wrench_outputs) ::= << #include \ #include \ #include \ - + #include \ #include \ #include \ @@ -149,7 +149,7 @@ app-arm-includes-robif2b(has_arm, wrench_outputs, has_wrench_outputs) ::= << >> -app-arm-setup-robif2b(arm_solvers, scene, wrench_outputs, has_wrench_outputs) ::= << +app-arm-setup-robif2b(arm_solvers, scene, wrench_outputs) ::= << urdf::ModelInterfaceSharedPtr kinova_model = urdf::parseURDFFile(""); if (!kinova_model) { std::cerr \<\< "Failed to parse Kinova URDF: \n"; @@ -171,7 +171,7 @@ app-arm-setup-robif2b(arm_solvers, scene, wrench_outputs, has_wrench_outputs) :: }; separator="\n"> - struct robif2b_ft_runtime { + struct robif2b_ft_runtime { robif2b_robotiq_ft_sensor_nbx sensor{\}; std::string port; float force_msr[3] = {0.0f, 0.0f, 0.0f\}; @@ -218,7 +218,7 @@ app-loop-condition-robif2b(has_arm) ::= << true >> -app-arm-runtime-step-robif2b(arm_solvers, wrench_outputs, has_wrench_outputs, has_wrench_data) ::= << +app-arm-runtime-step-robif2b(arm_solvers, wrench_outputs) ::= << bool _torque_finite = true; for (int i = 0; i \< KINOVA_NUM_JOINTS; ++i) { if (!std::isfinite(kinova__state.eff_cmd[i])) { @@ -235,17 +235,15 @@ app-arm-runtime-step-robif2b(arm_solvers, wrench_outputs, has_wrench_outputs, ha break; \} }; separator=""> - if (robif2b_ft.error.load() || robif2b_gripper.error.load()) { + if (robif2b_ft.error.load() || robif2b_gripper.error.load()) { std::cerr \<\< "robif2b peripheral update failed\n"; break; \} - if (robot.ext_force != nullptr) { std::lock_guard\ lock(robif2b_ft.mutex); *robot.ext_force = robif2b_ft.wrench; \} - >> app-arm-trace-update-robif2b(arm_solvers, trace) ::= << @@ -257,8 +255,8 @@ robif2b-stop-arm(arm_solvers) ::= << }; separator=""> >> -robif2b-hardware-post-setup(arm_solvers, wrench_outputs, has_wrench_outputs) ::= << - +robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << + if (_use_ft) { const char *ft_port = std::getenv("MOTION_SPEC_ROBOTIQ_FT_PORT"); robif2b_ft.port = (ft_port && *ft_port) ? ft_port : "ttyUSB0"; @@ -361,8 +359,8 @@ robif2b-hardware-post-setup(arm_solvers, wrench_outputs, has_wrench_outputs) ::= >> -app-arm-cleanup-robif2b(has_arm, wrench_outputs, has_wrench_outputs) ::= << - +app-arm-cleanup-robif2b(has_arm, wrench_outputs) ::= << + if (robif2b_gripper.active) { robif2b_gripper.stop.store(true); if (robif2b_gripper.worker.joinable()) robif2b_gripper.worker.join(); @@ -410,7 +408,7 @@ app-arm-start-robif2b(arm_solvers, fsm_namespace, fsm_step_event, fsm_step_event }; separator="\n"> >> -cmake_robif2b(motions, fsm_namespace, wrench_outputs, has_wrench_outputs) ::= << +cmake_robif2b(motions, fsm_namespace, wrench_outputs) ::= << cmake_minimum_required(VERSION 3.16) project(motion_spec_robif2b_target LANGUAGES CXX) @@ -425,7 +423,7 @@ find_package(urdfdom REQUIRED) find_package(kdl_parser REQUIRED) find_package(robif2b REQUIRED) find_package(Threads REQUIRED) - + find_package(serial REQUIRED) find_package(robotiq_driver_noros REQUIRED) find_package(robotiq_ft REQUIRED) @@ -453,7 +451,7 @@ target_include_directories(main PRIVATE ) target_link_libraries(main PRIVATE robif2b::kinova_gen3 - + robif2b::robotiq_ft_sensor robif2b::robotiq_gripper serial::serial diff --git a/src/motion_spec/codegen.py b/src/motion_spec/codegen.py index dc1276a..8a0de92 100644 --- a/src/motion_spec/codegen.py +++ b/src/motion_spec/codegen.py @@ -251,7 +251,6 @@ def generate_code(ir_path: Path, output_dir: Path, stst_bin: str): "closures": ir["closures"], "views": ir["views"], "wrench_outputs": ir["wrench_outputs"], - "has_wrench_outputs": ir.get("has_wrench_outputs", bool(ir["wrench_outputs"])), "base_velocity_solvers": ir["base_velocity_solvers"], "base_force_solvers": ir["base_force_solvers"], "has_mobile_base": ir["has_mobile_base"], diff --git a/src/motion_spec/ir_gen.py b/src/motion_spec/ir_gen.py index 749446f..5f1fb33 100644 --- a/src/motion_spec/ir_gen.py +++ b/src/motion_spec/ir_gen.py @@ -5809,10 +5809,6 @@ def generate_ir(manifest_path): if item.type == "Wrench" and item.id not in closure_output_map ] ) - # A declared FT sensor must initialize the real peripheral backend even when - # its wrench is monitoring-only and is not consumed by a solver constraint. - has_ft_sensor = any(g.triples((None, RDF.type, SENSORS.ForceTorqueSensor)) ) - motions, fsm_meta = build_motion_units( g, p, @@ -5912,8 +5908,6 @@ def generate_ir(manifest_path): data_structures, pose_components ), "wrench_outputs": wrench_outputs, - "has_wrench_data": bool(wrench_outputs), - "has_wrench_outputs": bool(wrench_outputs) or has_ft_sensor, "has_arm": bool(slv_arm), "has_mobile_base": bool(slv_base_vel or slv_base_frc), "has_ros": bool(ros_publishers), From 11f1c4ef053ebffda725407c6e03d9ed4676d304 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Thu, 23 Jul 2026 18:46:04 +0200 Subject: [PATCH 4/8] Guard peripheral generation on nonempty wrench data --- code-generator/robif2b_backend.stg | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/code-generator/robif2b_backend.stg b/code-generator/robif2b_backend.stg index 3424f25..6a827cf 100644 --- a/code-generator/robif2b_backend.stg +++ b/code-generator/robif2b_backend.stg @@ -140,7 +140,7 @@ app-arm-includes-robif2b(has_arm, wrench_outputs) ::= << #include \ #include \ #include \ - + #include \ #include \ #include \ @@ -171,7 +171,7 @@ app-arm-setup-robif2b(arm_solvers, scene, wrench_outputs) ::= << }; separator="\n"> - struct robif2b_ft_runtime { + struct robif2b_ft_runtime { robif2b_robotiq_ft_sensor_nbx sensor{\}; std::string port; float force_msr[3] = {0.0f, 0.0f, 0.0f\}; @@ -235,7 +235,7 @@ app-arm-runtime-step-robif2b(arm_solvers, wrench_outputs) ::= << break; \} }; separator=""> - if (robif2b_ft.error.load() || robif2b_gripper.error.load()) { + if (robif2b_ft.error.load() || robif2b_gripper.error.load()) { std::cerr \<\< "robif2b peripheral update failed\n"; break; \} @@ -256,7 +256,7 @@ robif2b-stop-arm(arm_solvers) ::= << >> robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << - + if (_use_ft) { const char *ft_port = std::getenv("MOTION_SPEC_ROBOTIQ_FT_PORT"); robif2b_ft.port = (ft_port && *ft_port) ? ft_port : "ttyUSB0"; @@ -360,7 +360,7 @@ robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << >> app-arm-cleanup-robif2b(has_arm, wrench_outputs) ::= << - + if (robif2b_gripper.active) { robif2b_gripper.stop.store(true); if (robif2b_gripper.worker.joinable()) robif2b_gripper.worker.join(); @@ -423,7 +423,7 @@ find_package(urdfdom REQUIRED) find_package(kdl_parser REQUIRED) find_package(robif2b REQUIRED) find_package(Threads REQUIRED) - + find_package(serial REQUIRED) find_package(robotiq_driver_noros REQUIRED) find_package(robotiq_ft REQUIRED) @@ -451,7 +451,7 @@ target_include_directories(main PRIVATE ) target_link_libraries(main PRIVATE robif2b::kinova_gen3 - + robif2b::robotiq_ft_sensor robif2b::robotiq_gripper serial::serial From 0bfb8e99c4a00b8298c3ec78d3cef3494e3c67b8 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Thu, 23 Jul 2026 18:50:26 +0200 Subject: [PATCH 5/8] Avoid optional peripheral code for empty wrench lists --- src/motion_spec/codegen.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/motion_spec/codegen.py b/src/motion_spec/codegen.py index 8a0de92..d50b4f8 100644 --- a/src/motion_spec/codegen.py +++ b/src/motion_spec/codegen.py @@ -209,6 +209,11 @@ def generate_code(ir_path: Path, output_dir: Path, stst_bin: str): ir["introspection_artifacts"] = write_introspection_artifacts( ir, ir_path=ir_path, output_dir=output_dir ) + # StringTemplate treats an empty list as present in conditionals. Preserve + # the automatically derived wrench list, but expose an empty list as null to + # templates so optional peripheral blocks are not emitted for arm-only models. + if not ir.get("wrench_outputs"): + ir["wrench_outputs"] = None headers_dir = output_dir / "headers" headers_dir.mkdir(parents=True, exist_ok=True) From 608f82e6e0bcc9b7488ac8b5d7e4b05c509eafa2 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Thu, 23 Jul 2026 18:58:53 +0200 Subject: [PATCH 6/8] Simplify wrench output template guards --- code-generator/robif2b_backend.stg | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/code-generator/robif2b_backend.stg b/code-generator/robif2b_backend.stg index 6a827cf..3424f25 100644 --- a/code-generator/robif2b_backend.stg +++ b/code-generator/robif2b_backend.stg @@ -140,7 +140,7 @@ app-arm-includes-robif2b(has_arm, wrench_outputs) ::= << #include \ #include \ #include \ - + #include \ #include \ #include \ @@ -171,7 +171,7 @@ app-arm-setup-robif2b(arm_solvers, scene, wrench_outputs) ::= << }; separator="\n"> - struct robif2b_ft_runtime { + struct robif2b_ft_runtime { robif2b_robotiq_ft_sensor_nbx sensor{\}; std::string port; float force_msr[3] = {0.0f, 0.0f, 0.0f\}; @@ -235,7 +235,7 @@ app-arm-runtime-step-robif2b(arm_solvers, wrench_outputs) ::= << break; \} }; separator=""> - if (robif2b_ft.error.load() || robif2b_gripper.error.load()) { + if (robif2b_ft.error.load() || robif2b_gripper.error.load()) { std::cerr \<\< "robif2b peripheral update failed\n"; break; \} @@ -256,7 +256,7 @@ robif2b-stop-arm(arm_solvers) ::= << >> robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << - + if (_use_ft) { const char *ft_port = std::getenv("MOTION_SPEC_ROBOTIQ_FT_PORT"); robif2b_ft.port = (ft_port && *ft_port) ? ft_port : "ttyUSB0"; @@ -360,7 +360,7 @@ robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << >> app-arm-cleanup-robif2b(has_arm, wrench_outputs) ::= << - + if (robif2b_gripper.active) { robif2b_gripper.stop.store(true); if (robif2b_gripper.worker.joinable()) robif2b_gripper.worker.join(); @@ -423,7 +423,7 @@ find_package(urdfdom REQUIRED) find_package(kdl_parser REQUIRED) find_package(robif2b REQUIRED) find_package(Threads REQUIRED) - + find_package(serial REQUIRED) find_package(robotiq_driver_noros REQUIRED) find_package(robotiq_ft REQUIRED) @@ -451,7 +451,7 @@ target_include_directories(main PRIVATE ) target_link_libraries(main PRIVATE robif2b::kinova_gen3 - + robif2b::robotiq_ft_sensor robif2b::robotiq_gripper serial::serial From f0b8f41d4181a78ee3047c36e4c0d07e2c784251 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Fri, 24 Jul 2026 09:08:35 +0200 Subject: [PATCH 7/8] Use model timestep for Kinova cycle time --- code-generator/robif2b_backend.stg | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/code-generator/robif2b_backend.stg b/code-generator/robif2b_backend.stg index 3424f25..5760863 100644 --- a/code-generator/robif2b_backend.stg +++ b/code-generator/robif2b_backend.stg @@ -15,7 +15,7 @@ robot-state-struct-robif2b-KinovaGen3(solver) ::= << struct kinova_state { bool success = false; enum robif2b_ctrl_mode ctrl_mode = ROBIF2B_CTRL_MODE_FORCE; - double cycle_time = 0.001; + double cycle_time = 0.0; double pos_msr[KINOVA_NUM_JOINTS] = {0.0}; double vel_msr[KINOVA_NUM_JOINTS] = {0.0}; double eff_msr[KINOVA_NUM_JOINTS] = {0.0}; @@ -163,6 +163,8 @@ app-arm-setup-robif2b(arm_solvers, scene, wrench_outputs) ::= << + // Keep robif2b's velocity integration period identical to the model loop period. + kinova__state.cycle_time = ; From 29f2b5cf4610097a40a87c487c4a72fbbc077aa4 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Fri, 24 Jul 2026 10:47:49 +0200 Subject: [PATCH 8/8] Finalize Kinova robif2b backend support --- code-generator/robif2b_backend.stg | 67 +++-- src/motion_spec/codegen.py | 5 +- src/motion_spec/entities.py | 2 + src/motion_spec/ir_gen.py | 27 +- tests/test_codegen_artifacts.py | 16 ++ tests/test_ir_defaults.py | 16 +- thirdparty/kinova/GEN3_URDF_V12.urdf | 63 ++--- .../kinova/GEN3_URDF_V12_ROBOTIQ_FT_2F85.urdf | 251 ++++++++++++++++++ 8 files changed, 366 insertions(+), 81 deletions(-) create mode 100644 thirdparty/kinova/GEN3_URDF_V12_ROBOTIQ_FT_2F85.urdf diff --git a/code-generator/robif2b_backend.stg b/code-generator/robif2b_backend.stg index 5760863..c835324 100644 --- a/code-generator/robif2b_backend.stg +++ b/code-generator/robif2b_backend.stg @@ -74,10 +74,8 @@ robot-chain-robif2b-KinovaGen3(solver) ::= << std::string kinova_chain_root = ""; std::string kinova_chain_end = ""; if (!kinova_tree.getChain(kinova_chain_root, kinova_chain_end, chain_)) { - // URDF link names are case-sensitive. The scene model uses the canonical - // lower-case Kinova names, while the supplied Gen3 URDF uses names such as - // Shoulder_Link and Bracelet_Link. Resolve that naming-only difference before - // extracting the chain; the physical model and joint order remain unchanged. + // Resolve harmless case or namespace differences between scene and URDF + // link names before extracting the same physical chain. const auto resolve_kinova_link = [&kinova_tree](std::string_view requested) { for (const auto &entry : kinova_tree.getSegments()) { if (motion_spec::runtime::runtime_name_matches(entry.first, requested)) { @@ -140,12 +138,12 @@ app-arm-includes-robif2b(has_arm, wrench_outputs) ::= << #include \ #include \ #include \ +#include \ #include \ -#include \ #include \ -#include \ +#include \ >> @@ -188,6 +186,7 @@ app-arm-setup-robif2b(arm_solvers, scene, wrench_outputs) ::= << std::thread worker; bool active = false; \} robif2b_ft; + struct robif2b_gripper_runtime { robif2b_robotiq_gripper_nbx driver{\}; @@ -207,13 +206,13 @@ app-arm-setup-robif2b(arm_solvers, scene, wrench_outputs) ::= << \} robif2b_gripper; const char *_devices_env = std::getenv("MOTION_SPEC_COMMUNICATION_DEVICES"); - const std::string _devices = (_devices_env && *_devices_env) ? _devices_env : "ft,gripper"; + const std::string _devices = (_devices_env && *_devices_env) ? _devices_env : "arm"; const auto _selected = [&](const char *name) { return _devices == "all" || _devices.find(std::string(name)) != std::string::npos; }; - const bool _use_ft = _selected("ft"); - const bool _use_gripper = _selected("gripper"); + const bool _use_ft = _selected("ft"); + const bool _use_gripper = _selected("gripper"); >> app-loop-condition-robif2b(has_arm) ::= << @@ -237,13 +236,20 @@ app-arm-runtime-step-robif2b(arm_solvers, wrench_outputs) ::= << break; \} }; separator=""> - if (robif2b_ft.error.load() || robif2b_gripper.error.load()) { - std::cerr \<\< "robif2b peripheral update failed\n"; + if (robif2b_ft.error.load()) { + std::cerr \<\< "robif2b FT update failed\n"; break; \} - if (robot.ext_force != nullptr) { + + if (robif2b_gripper.error.load()) { + std::cerr \<\< "robif2b gripper update failed\n"; + break; + \} + if (robif2b_ft.active) { std::lock_guard\ lock(robif2b_ft.mutex); - *robot.ext_force = robif2b_ft.wrench; + != nullptr) { + *robot. = robif2b_ft.wrench; + \}}; separator="\n"> \} >> @@ -280,9 +286,21 @@ robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << return 1; \} + int ft_bias_samples = 100; + if (const char *value = std::getenv("MOTION_SPEC_ROBOTIQ_FT_BIAS_SAMPLES")) { + char *end = nullptr; + const long parsed = std::strtol(value, &end, 10); + if (end == value || *end != '\0' || parsed \< 0 || parsed > 100000) { + std::cerr \<\< "MOTION_SPEC_ROBOTIQ_FT_BIAS_SAMPLES must be an integer from 0 to 100000\n"; + robif2b_robotiq_ft_shutdown(&robif2b_ft.sensor); + + return 1; + \} + ft_bias_samples = static_cast\(parsed); + \} float force_sum[3] = {0.0f, 0.0f, 0.0f\}; float moment_sum[3] = {0.0f, 0.0f, 0.0f\}; - for (int sample = 0; sample \< 100; ++sample) { + for (int sample = 0; sample \< ft_bias_samples; ++sample) { robif2b_robotiq_ft_update(&robif2b_ft.sensor); if (!robif2b_ft.success) { std::cerr \<\< "Robotiq FT sensor zeroing failed\n"; @@ -296,9 +314,11 @@ robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << \} std::this_thread::sleep_for(std::chrono::milliseconds(10)); \} - for (int axis = 0; axis \< 3; ++axis) { - robif2b_ft.force_offset[axis] = force_sum[axis] / 100.0f; - robif2b_ft.moment_offset[axis] = moment_sum[axis] / 100.0f; + if (ft_bias_samples > 0) { + for (int axis = 0; axis \< 3; ++axis) { + robif2b_ft.force_offset[axis] = force_sum[axis] / ft_bias_samples; + robif2b_ft.moment_offset[axis] = moment_sum[axis] / ft_bias_samples; + \} \} robif2b_ft.worker = std::thread([&robif2b_ft]() { while (!robif2b_ft.stop.load()) { @@ -318,6 +338,7 @@ robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << \}); robif2b_ft.active = true; } + if (_use_gripper) { const char *gripper_port = std::getenv("MOTION_SPEC_ROBOTIQ_GRIPPER_PORT"); @@ -337,11 +358,13 @@ robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << robif2b_robotiq_gripper_configure(&robif2b_gripper.driver); if (!robif2b_gripper.success) { std::cerr \<\< "Robotiq gripper configure failed\n"; + if (robif2b_ft.active) { robif2b_ft.stop.store(true); if (robif2b_ft.worker.joinable()) robif2b_ft.worker.join(); robif2b_robotiq_ft_shutdown(&robif2b_ft.sensor); } + return 1; \} @@ -358,18 +381,16 @@ robif2b-hardware-post-setup(arm_solvers, wrench_outputs) ::= << \}); robif2b_gripper.active = true; } - >> app-arm-cleanup-robif2b(has_arm, wrench_outputs) ::= << - if (robif2b_gripper.active) { robif2b_gripper.stop.store(true); if (robif2b_gripper.worker.joinable()) robif2b_gripper.worker.join(); robif2b_robotiq_gripper_shutdown(&robif2b_gripper.driver); robif2b_gripper.active = false; \} - if (robif2b_ft.active) { + if (robif2b_ft.active) { robif2b_ft.stop.store(true); if (robif2b_ft.worker.joinable()) robif2b_ft.worker.join(); robif2b_robotiq_ft_shutdown(&robif2b_ft.sensor); @@ -425,9 +446,9 @@ find_package(urdfdom REQUIRED) find_package(kdl_parser REQUIRED) find_package(robif2b REQUIRED) find_package(Threads REQUIRED) - find_package(serial REQUIRED) find_package(robotiq_driver_noros REQUIRED) + find_package(robotiq_ft REQUIRED) @@ -453,11 +474,11 @@ target_include_directories(main PRIVATE ) target_link_libraries(main PRIVATE robif2b::kinova_gen3 - - robif2b::robotiq_ft_sensor robif2b::robotiq_gripper serial::serial robotiq::robotiq_driver_noros + + robif2b::robotiq_ft_sensor robotiq::robotiq_ft_driver orocos-kdl diff --git a/src/motion_spec/codegen.py b/src/motion_spec/codegen.py index d50b4f8..b73bc55 100644 --- a/src/motion_spec/codegen.py +++ b/src/motion_spec/codegen.py @@ -209,9 +209,8 @@ def generate_code(ir_path: Path, output_dir: Path, stst_bin: str): ir["introspection_artifacts"] = write_introspection_artifacts( ir, ir_path=ir_path, output_dir=output_dir ) - # StringTemplate treats an empty list as present in conditionals. Preserve - # the automatically derived wrench list, but expose an empty list as null to - # templates so optional peripheral blocks are not emitted for arm-only models. + # ST4 treats an empty list as present in . Keep the automatically + # derived wrench list, but expose its empty case as null to template guards. if not ir.get("wrench_outputs"): ir["wrench_outputs"] = None diff --git a/src/motion_spec/entities.py b/src/motion_spec/entities.py index 819cd6f..fd49c83 100644 --- a/src/motion_spec/entities.py +++ b/src/motion_spec/entities.py @@ -749,6 +749,7 @@ class HandlerArmSolver: root_acc: list[float] | None = None chain_root: str = "" chain_end: str = "" + num_joints: int = 0 torque_saturation: Saturation | None = None commanded_torque_samples: list = field(default_factory=list) runtime_id: str = "" @@ -768,6 +769,7 @@ class SolverWithInputAndOutput: urdf: str = "" chain_root: str = "" chain_end: str = "" + num_joints: int = 0 commanded_torque_samples: list = field(default_factory=list) chain_tip: str = "" robot_model: str = "" diff --git a/src/motion_spec/ir_gen.py b/src/motion_spec/ir_gen.py index 5f1fb33..51f23fa 100644 --- a/src/motion_spec/ir_gen.py +++ b/src/motion_spec/ir_gen.py @@ -2446,6 +2446,7 @@ def _arm_solvers_for_handler(handler, slv_arm, solver_ids): root_acc=solver.root_acc, chain_root=solver.chain_root, chain_end=solver.chain_end, + num_joints=solver.num_joints, torque_saturation=solver.torque_saturation, commanded_torque_samples=solver.commanded_torque_samples, ) @@ -3664,6 +3665,10 @@ def binding_for(node): "trees": [binding["tree"] for binding in bindings], "root_body": root_body, "chain_root": runtime_root, + "num_joints": sum( + set(g.objects(joint, RDF.type)) != {KC.Joint} + for *_edge, joint in path + ), "chain_tip": f"{runtime_prefix}{_leaf(chain_tip_body)}", "tool_body": ( f"{runtime_prefix}{_leaf(tip_body)}" @@ -3835,7 +3840,7 @@ def _robot_setups_from_graph(g): Returns ``(setups_by_node, ordered)`` where ``setups_by_node`` maps each robot's abstract agent node (the target of a solver's ``agn:of-agent``) to its setup tuple ``(urdf, chain_root, chain_end, chain_tip, robot_model, tool_body, tcp_site, - ft_sensors, runtime_prefix, owned_trees)``. + ft_sensors, runtime_prefix, owned_trees, num_joints)``. Chain bodies come from a serial-composition ``geom:KinematicTree``'s ``kc-ext:root`` / ``kc-ext:tip`` frames. A scene-dsl frame URI is @@ -3865,6 +3870,7 @@ def _robot_model_from_path(path): assembly["ft_sensors"], assembly["prefix"], assembly["trees"], + assembly["num_joints"], ) setups_by_node[assembly["agent"]] = setup ordered.append(setup) @@ -3932,7 +3938,7 @@ def _build_introspection( closures, views, shared_data, - arm_solvers, + arm_solvers=(), ): """Build the introspection artifact (uris, motions, controllers, monitors, quantities, provenance) and fold in the controller-state and frame-log samples. @@ -4367,6 +4373,7 @@ def _solver_sections( solver.ft_sensors, solver.runtime_prefix, solver.owned_trees, + solver.num_joints, ) = setups_by_node.get(robot_node, default_setup) solver.output = _dedupe_by_id( [ @@ -5074,9 +5081,6 @@ def add_quantity(item_id: str, controller_id: str, state_name: str) -> None: closure["internal_state_samples"] = closure_samples -KINOVA_NUM_JOINTS = 7 - - def add_solver_command_torque_logging( arm_solvers: list, motions: list, shared_data: list, introspection: dict ) -> None: @@ -5087,9 +5091,10 @@ def add_solver_command_torque_logging( samples_by_solver = {} for solver in arm_solvers: + solver_id = _field(solver, "id") samples = [] - for joint_index in range(KINOVA_NUM_JOINTS): - sample_id = f"commanded_torque_{solver.id}_joint_{joint_index + 1}" + for joint_index in range(_field(solver, "num_joints", 0)): + sample_id = f"commanded_torque_{solver_id}_joint_{joint_index + 1}" samples.append({"id": sample_id, "joint_index": joint_index}) if sample_id not in shared_ids: shared_data.append( @@ -5097,7 +5102,7 @@ def add_solver_command_torque_logging( "id": sample_id, "type": "Quantity", "role": "commanded_joint_torque", - "solver": solver.id, + "solver": solver_id, "joint_index": joint_index, } ) @@ -5110,13 +5115,13 @@ def add_solver_command_torque_logging( "unit": ["N_M"], "quantity_kind": ["Torque"], "role": "commanded_joint_torque", - "solver": solver.id, + "solver": solver_id, "joint_index": joint_index, } ) quantity_ids.add(sample_id) _set_field(solver, "commanded_torque_samples", samples) - samples_by_solver[solver.id] = samples + samples_by_solver[solver_id] = samples for motion in motions: for solver in _field(motion, "arm_solvers", []) or []: @@ -5769,7 +5774,7 @@ def generate_ir(manifest_path): default_setup = ( ordered_setups[0] if ordered_setups - else ("", "", "", "", "", "", "", [], "", []) + else ("", "", "", "", "", "", "", [], "", [], 0) ) # Derive backend + FSM up front: both are pure functions of the graph and are inputs to # downstream construction (solver validation, runtime-robot annotation, motion FSM wiring). diff --git a/tests/test_codegen_artifacts.py b/tests/test_codegen_artifacts.py index d93909e..42c0b13 100644 --- a/tests/test_codegen_artifacts.py +++ b/tests/test_codegen_artifacts.py @@ -13,6 +13,7 @@ _annotate_controller_signals, add_controller_internal_state_logging, add_quantity_samples, + add_solver_command_torque_logging, add_spatial_samples, ) from motion_spec.codegen_artifacts import ( @@ -164,6 +165,21 @@ def _sample_ir() -> dict: } +def test_commanded_torque_logging_uses_declared_chain_joint_count() -> None: + solver = {"id": "arm", "num_joints": 3} + motion_solver = {"id": "arm"} + shared_data = [] + introspection = {} + + add_solver_command_torque_logging( + [solver], [{"arm_solvers": [motion_solver]}], shared_data, introspection + ) + + assert [sample["joint_index"] for sample in solver["commanded_torque_samples"]] == [0, 1, 2] + assert motion_solver["commanded_torque_samples"] == solver["commanded_torque_samples"] + assert len(shared_data) == 3 + + def _sample_fsm() -> dict: return { "namespace_uri": "https://example.test/fsm/", diff --git a/tests/test_ir_defaults.py b/tests/test_ir_defaults.py index 4a05a91..fabf4d1 100644 --- a/tests/test_ir_defaults.py +++ b/tests/test_ir_defaults.py @@ -116,10 +116,13 @@ def test_agent_model_may_bind_the_assembled_kinematic_tree() -> None: graph = Graph() base = URIRef("https://example.test/arm/base") root = URIRef(f"{base}/root") + elbow = URIRef("https://example.test/arm/elbow") + elbow_frame = URIRef(f"{elbow}/origin") tool = URIRef("https://example.test/gripper/tool") tcp = URIRef(f"{tool}/tcp") tree = URIRef("https://example.test/assembled") - joint = URIRef(f"{tree}/fixed") + moving_joint = URIRef(f"{tree}/moving") + fixed_joint = URIRef(f"{tree}/fixed") agent = URIRef("https://example.test/robot") modelled = URIRef("https://example.test/modelled-robot") model = URIRef("https://example.test/robot-model") @@ -133,13 +136,18 @@ def test_agent_model_may_bind_the_assembled_kinematic_tree() -> None: graph.add((tree, RDF.type, URI_KC_TYPE_SERIAL)) graph.add((tree, NS_MM_KC_EXT["root"], root)) graph.add((tree, NS_MM_KC_EXT["tip"], tcp)) - graph.add((joint, RDF.type, KC.Joint)) - graph.add((joint, KC["between-attachments"], root)) - graph.add((joint, KC["between-attachments"], tcp)) + graph.add((moving_joint, RDF.type, KC.Joint)) + graph.add((moving_joint, RDF.type, KC.RevoluteJoint)) + graph.add((moving_joint, KC["between-attachments"], root)) + graph.add((moving_joint, KC["between-attachments"], elbow_frame)) + graph.add((fixed_joint, RDF.type, KC.Joint)) + graph.add((fixed_joint, KC["between-attachments"], elbow_frame)) + graph.add((fixed_joint, KC["between-attachments"], tcp)) setups, _ordered = _robot_setups_from_graph(graph) assert setups[agent][1:7] == ("base", "tool", "tool", "KinovaGen3", "", "") + assert setups[agent][10] == 1 def test_uris_table_maps_each_id_to_full_uri() -> None: diff --git a/thirdparty/kinova/GEN3_URDF_V12.urdf b/thirdparty/kinova/GEN3_URDF_V12.urdf index c131e32..7b3f242 100644 --- a/thirdparty/kinova/GEN3_URDF_V12.urdf +++ b/thirdparty/kinova/GEN3_URDF_V12.urdf @@ -1,9 +1,4 @@ - - - - - @@ -26,7 +21,7 @@ - + @@ -51,11 +46,11 @@ - + - + @@ -79,12 +74,12 @@ - - + + - + @@ -108,12 +103,12 @@ - - + + - + @@ -137,12 +132,12 @@ - - + + - + @@ -166,12 +161,12 @@ - - + + - + @@ -195,12 +190,12 @@ - - + + - + @@ -224,28 +219,16 @@ - - + + - - - - - - - + - - + + - - - - - - diff --git a/thirdparty/kinova/GEN3_URDF_V12_ROBOTIQ_FT_2F85.urdf b/thirdparty/kinova/GEN3_URDF_V12_ROBOTIQ_FT_2F85.urdf new file mode 100644 index 0000000..c80ec42 --- /dev/null +++ b/thirdparty/kinova/GEN3_URDF_V12_ROBOTIQ_FT_2F85.urdf @@ -0,0 +1,251 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +