[munseng-flexiv]

Forward-ports the features released in #136 (jazzy-v1.9.4, RDK v1.9.4.1, Rizon/AICO) onto the v2 line for the Enlight and MICO series on RDK v2.2.

Included

• Cartesian motion-force control (Feature/Add Cartesian Motion Force control interface #132) — new cartesian_motion_force_controller package, flexiv_msgs/msg/CartesianMotionForce and one service per RDK call (~/set_cartesian_impedance, ~/set_max_contact_wrench, ~/set_null_space_posture, ~/set_null_space_objectives, ~/set_force_control_axis, ~/set_force_control_frame, ~/set_passive_force_control, ~/set_cartesian_motion_limits) under /<robot_sn>/flexiv_cartesian_motion_force_config_node. TCP interfaces per arm under <prefix>tcp, F/T sensor zeroed on every controller start, commands synced to the measured TCP pose so the first command holds. Settings are not held: every start begins from the robot's defaults.
  • New test_cartesian_motion_force_controller.launch.py and cartesian_motion_force_example.py (mode:=pure_motion / mode:=motion_force). Extended: the example also runs in RT mode — contact search is ramped, motion limits skipped, and the target stays on the TCP along the force axis so stopping does not fault the robot.
• RT / NRT control modes (Feature/Add option to start RDK in realtime (RT) control modes #134) — rdk_realtime_mode and withhold_on_timeliness_failure launch / xacro args. true streams RT_JOINT_POSITION / RT_JOINT_IMPEDANCE / RT_CARTESIAN_MOTION_FORCE; false sends the NRT_* modes through the robot's motion generator. Effort stays in RT_JOINT_TORQUE. A non-finite or unclaimed command holds the last streamed target instead of skipping the cycle.
  • Extended: while motion is withheld the driver warns every 5 s, and the trajectory controllers now have a 0.05 rad goal tolerance per joint, so goals fail instead of reporting success.
• Joint motion limits — NRT joint position/velocity commands are limited by the URDF velocity limit and the new max_joint_acceleration hardware parameter (default 5.0 rad/s², matching flexiv_moveit_config).
• ros2_control export API — hardware interface migrated to on_export_state_interfaces() / on_export_command_interfaces().
• MoveIt — configs consolidated (OMPL, joint limits, RViz), MoveIt Servo config fixed for Jazzy (start_servo:=true, single-arm).
• Gripper — move / grasp / gripper_action finish when the fingers stop instead of when the command is sent (width_tolerance, action_timeout), plus force limit fixes and a tool_name launch arg for tool selection. New flexiv_gripper/README.md.
• gpio_controller — fixed reactivation (publisher kept across deactivate) and the digital outputs buffer is now realtime-safe.
• Robot states broadcaster — tcp_wrench_local / raw_tcp_wrench_local are stamped in the TCP frame instead of the flange frame.
• Dependencies, CI and docs — flexiv_description bumped to jazzy-v3.1, package.xml dependencies completed so rosdep installs everything (install steps updated), cartesian_motion_force_controller added to CI, Lyrical added to the README compatibility table.

v2-only changes

• Per-arm controllers — dual-arm robots now run left_flexiv_arm_controller / right_flexiv_arm_controller (plus flexiv_torso_controller on MICO-Plus/Ultra) instead of one 14/16-joint controller. The hardware interface arbitrates per RDK joint group (ARM_1 / ARM_2 / EXT_AXIS): each group is claimed whole with one command interface type, and an unclaimed group holds its last position, so each arm accepts and preempts goals independently. test_joint_trajectory_controller.launch.py gains arm:=single|left|right|both.
• TCP TF frame — the broadcaster publishes the measured TCP as <robot_sn>_tcp (child of <robot_sn>_flange, publish_tcp_tf parameter, default on) so the tcp_wrench_local topics can be placed. It follows the active tool and is skipped until the poses are valid. Also fixes the single-arm Cartesian robot states.
• SRDF — home poses loaded from the flexiv_description initial positions (mirrored right arm on MICO). Collision checks disabled between the MICO neck, head and their rigidly attached links.

Excluded

• AICO2 external axis types (Feature/Support the new AICO2 external axis types #128) — v1.x only.
• DRDK dependency update and optional DRDK plugin — v2 dual-arm models use one RDK connection, no DRDK.
• Dual-robot pair behaviours from Feature/Add option to start RDK in realtime (RT) control modes #134 (one arm starting / stopping / restarting on a RobotPair) — replaced by the per-joint-group arbitration above.
• fastcdr / spdlog dependency removal and the RDK v1.9.4.1 bump — RDK v1.x only.

RDK v2.2 adaptations

v1.9.4	v2.2
SendCartesianMotionForce(pose, wrench, vel, ...)	SendCartesianMotionForce(std::map<JointGroup, NrtCartesianCmd>)
StreamCartesianMotionForce(pose, wrench, vel, ...)	StreamCartesianMotionForce(std::map<JointGroup, RtCartesianCmd>)
SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(), SetNullSpaceObjectives(), SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl()	same calls with a leading JointGroup, one call per arm
RobotInfo::K_x_nom / q_min / q_max	per-JointGroup maps
ExecutePrimitive("ZeroFTSensor", {})	ExecutePrimitive(std::map<JointGroup, PrimitiveArgs>) for every arm
DRDK RobotPair for a dual setup	one Robot, ARM_1 / ARM_2 joint groups

• rdk_realtime_mode defaults to true — v2.x has always streamed in the RT modes, so the default keeps current behaviour; false opts into NRT.
• withhold_on_timeliness_failure defaults to false — true trips within seconds on a non-PREEMPT_RT host, so by default the driver only warns and keeps streaming, as the RDK does.
• One Cartesian controller per robot — cartesian_motion_force_controller claims the TCP interfaces of every arm, with one topic per arm (/cartesian_motion_force_controller/left_<robot_sn>/cartesian_motion_force, .../right_<robot_sn>/...). Per-arm arrays in the service requests hold ARM_1 first. The MICO torso is not commanded while it runs, and no joint controller may run alongside it.