From aa980437c0f23e370ee54c7086918a58c7f43571 Mon Sep 17 00:00:00 2001 From: kawtharzaidan95-max Date: Mon, 13 Jul 2026 13:39:18 +0300 Subject: [PATCH] Add Isaac Sim integration for Unitree Go1 --- .gitignore | 16 + README.md | 5 + docs/isaac/README.md | 260 ++++ isaac_legged_hw/CMakeLists.txt | 61 + isaac_legged_hw/config/go1_reference.info | 46 + .../include/isaac_legged_hw/IsaacHW.h | 75 + isaac_legged_hw/launch/bringup_isaac.launch | 52 + isaac_legged_hw/launch/control_isaac.launch | 38 + isaac_legged_hw/package.xml | 32 + .../scripts/isaac_gazebo_like_cmd_filter.py | 426 ++++++ isaac_legged_hw/scripts/mode_to_contacts.py | 57 + isaac_legged_hw/scripts/ros1_socket_bridge.py | 420 ++++++ isaac_legged_hw/src/IsaacHW.cpp | 733 ++++++++++ isaac_legged_hw/src/isaac_hw_node.cpp | 30 + isaac_sim/assets/README.md | 9 + isaac_sim/assets/go1_comp.usd | Bin 0 -> 31044 bytes isaac_sim/run_isaac_sim.sh | 18 + isaac_sim/scripts/go1_isaac_bridge.py | 1251 +++++++++++++++++ isaac_sim/scripts/ros2_isaac_socket_bridge.py | 277 ++++ isaac_sim/start_isaac_legged_stack.sh | 137 ++ .../src/TargetTrajectoriesPublisher.cpp | 62 +- 21 files changed, 4003 insertions(+), 2 deletions(-) create mode 100644 docs/isaac/README.md create mode 100644 isaac_legged_hw/CMakeLists.txt create mode 100644 isaac_legged_hw/config/go1_reference.info create mode 100644 isaac_legged_hw/include/isaac_legged_hw/IsaacHW.h create mode 100644 isaac_legged_hw/launch/bringup_isaac.launch create mode 100644 isaac_legged_hw/launch/control_isaac.launch create mode 100644 isaac_legged_hw/package.xml create mode 100755 isaac_legged_hw/scripts/isaac_gazebo_like_cmd_filter.py create mode 100755 isaac_legged_hw/scripts/mode_to_contacts.py create mode 100755 isaac_legged_hw/scripts/ros1_socket_bridge.py create mode 100644 isaac_legged_hw/src/IsaacHW.cpp create mode 100644 isaac_legged_hw/src/isaac_hw_node.cpp create mode 100644 isaac_sim/assets/README.md create mode 100644 isaac_sim/assets/go1_comp.usd create mode 100755 isaac_sim/run_isaac_sim.sh create mode 100755 isaac_sim/scripts/go1_isaac_bridge.py create mode 100755 isaac_sim/scripts/ros2_isaac_socket_bridge.py create mode 100755 isaac_sim/start_isaac_legged_stack.sh diff --git a/.gitignore b/.gitignore index 35d74bb7..cb648521 100644 --- a/.gitignore +++ b/.gitignore @@ -44,6 +44,22 @@ qtcreator-* *~ +# OS/editor metadata and local simulation output +*Zone.Identifier* +.DS_Store +.idea/ +.vscode/ +*.bag +*.db3 +*.mcap +*.log +__pycache__/ + +# Isaac Sim local state and generated USD layers +isaac_sim/.cache/ +isaac_sim/output/ +isaac_sim/assets/*.autosave.usd + # Emacs .#* diff --git a/README.md b/README.md index 877cfc47..2e107da7 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,11 @@ > [!NOTE] > You might be interested in this pipeline with perception, check [legged_perceptive](https://github.com/qiayuanl/legged_perceptive). +> [!NOTE] +> The `isaac-sim-integration` branch adds a tested Go1 backend for Isaac Sim +> 5.1 using ROS 2 Humble on the host and ROS 1 Noetic for this controller. +> See [the Isaac Sim setup guide](docs/isaac/README.md). + ## Publications If you use this work in an academic context, please consider citing the following publications: diff --git a/docs/isaac/README.md b/docs/isaac/README.md new file mode 100644 index 00000000..4ff9f929 --- /dev/null +++ b/docs/isaac/README.md @@ -0,0 +1,260 @@ +# Isaac Sim 5.1 integration (Go1) + +This branch connects the ROS 1 `legged_control` stack to a Go1 running in +Isaac Sim through a small TCP bridge. It was developed with Isaac Sim 5.1, +ROS 2 Humble on the host, and ROS 1 Noetic in Docker. + +## Data flow + +```text +Isaac Sim + go1_isaac_bridge.py (ROS 2) + | joint state, IMU, odometry, clock + v +ros2_isaac_socket_bridge.py (host TCP server, port 50055) + | newline-delimited JSON over TCP + v +ros1_socket_bridge.py (ROS 1 container) + | /isaac/* topics + v +IsaacHW -> controller_manager -> NMPC/WBC + | 12 x [q_des, dq_des, kp, kd, feed-forward] + +-------------------------------------------------> Isaac Sim +``` + +The TCP link is intentionally local and unauthenticated. Bind it only to a +trusted host/network and do not expose port 50055 to the Internet. + +## Included files + +- `isaac_legged_hw/`: catkin hardware-interface package, ROS 1 TCP client, + planned-contact publisher, optional velocity-command filter, and launch files. +- `isaac_sim/scripts/go1_isaac_bridge.py`: run inside Isaac Sim after opening + the stage. It applies torque and publishes simulated sensors on ROS 2. +- `isaac_sim/scripts/ros2_isaac_socket_bridge.py`: ROS 2 TCP server on the host. +- `isaac_sim/start_isaac_legged_stack.sh`: starts the host bridge and the ROS 1 + stack in Docker without opening machine-specific terminal windows. +- `isaac_sim/assets/go1_comp.usd`: tested stage override that composes the + NVIDIA Go1 asset. See the asset README for the dependency/licensing note. + +## Prerequisites + +1. A working upstream `legged_control` catkin workspace and the OCS2 + dependencies listed in the main README. +2. ROS 1 Noetic in the controller environment (a Docker container is fine). +3. Isaac Sim 5.1 with its ROS 2 Humble bridge installed and enabled. +4. The NVIDIA Go1 asset available to Isaac Sim. +5. Host/container TCP connectivity. On Linux, create the container with + `--add-host=host.docker.internal:host-gateway`, or pass the Docker bridge + gateway explicitly as `socket_host:=172.17.0.1`. + +The controller and Isaac Sim must run on the same little-endian machine; this +setup was tested locally, not across an untrusted or high-latency network. + +## Build the ROS 1 side + +Clone this branch into the catkin workspace's `src` directory, then build: + +```bash +cd ~/legged_ws +catkin config -DCMAKE_BUILD_TYPE=RelWithDebInfo +catkin build isaac_legged_hw legged_controllers legged_unitree_description +source devel/setup.bash +``` + +No file under `build/`, `devel/`, `logs/`, any bag file, or any local Isaac +cache belongs in Git. + +## Run + +The safe startup order is controller first, Isaac timeline last. Starting the +timeline before the controller is running leaves the robot at zero torque and +it will fall until control becomes active. + +### Recommended: portable stack launcher + +After building the branch in the container workspace, run this on the host: + +```bash +./isaac_sim/start_isaac_legged_stack.sh +``` + +Its defaults match the development setup. Override them when needed: + +```bash +CONTAINER_NAME=my_container \ +ROS1_WS=/root/legged_ws \ +ROS2_SOCKET_HOST=host.docker.internal \ +./isaac_sim/start_isaac_legged_stack.sh +``` + +The launcher starts the container, host ROS 2 socket server, `roscore`, Isaac +hardware bringup, loads the cheater controller, and switches it to `running`. +It intentionally does not start Isaac or publish velocity. Continue with step +1 below while leaving the Isaac timeline stopped, run the bridge script, and +only then press **Play**. The remaining manual steps document the same services +for users who do not use the launcher. + +Use separate terminals so failures remain visible. + +### 1. Start Isaac Sim + +Activate the Isaac Sim Python environment, then: + +```bash +./isaac_sim/run_isaac_sim.sh +``` + +Open `isaac_sim/assets/go1_comp.usd` while the timeline is stopped. Open Isaac +Sim's Script Editor, load `isaac_sim/scripts/go1_isaac_bridge.py`, and run it. +After the controller is `running`, press **Play**. The startup +summary must report these prims: + +```text +Robot root prim: /World/go1 +Articulation root: /World/go1/trunk +``` + +If your stage uses other paths, edit the configuration block at the top of +`go1_isaac_bridge.py` before running it. + +### 2. Start the ROS 2 socket server on the host + +Source only ROS 2 in this terminal: + +```bash +source /opt/ros/humble/setup.bash +export ROS_DOMAIN_ID=0 +export ROS_LOCALHOST_ONLY=0 +python3 isaac_sim/scripts/ros2_isaac_socket_bridge.py +``` + +`ISAAC_SOCKET_BIND` and `ISAAC_SOCKET_PORT` override the default bind address +(`0.0.0.0`) and port (`50055`). + +### 3. Start ROS 1 hardware and bridges in the container + +Start `roscore`, then in a sourced catkin terminal run: + +```bash +export ROBOT_TYPE=go1 +roslaunch isaac_legged_hw bringup_isaac.launch \ + socket_host:=host.docker.internal +``` + +If the container was not created with the `host.docker.internal` mapping, use: + +```bash +roslaunch isaac_legged_hw bringup_isaac.launch socket_host:=172.17.0.1 +``` + +The launch file starts `IsaacHW`, the ROS 1 TCP client, and the MPC-mode to +contact-state publisher. Its defaults are a 500 Hz hardware loop, 0.002 s +cycle error threshold, and real-time priority 95. + +### 4. Load and start the controller + +In another sourced ROS 1 terminal: + +```bash +roslaunch isaac_legged_hw control_isaac.launch cheater:=true +``` + +After the controller has loaded: + +```bash +rosservice call /controller_manager/switch_controller \ + "start_controllers: ['controllers/legged_cheater_controller'] +stop_controllers: [] +strictness: 2 +start_asap: false +timeout: 0.0" +``` + +The cheater controller consumes `/ground_truth/state`; do not use it on real +hardware. This launch also selects the Isaac-specific neutral-abduction posture +from `isaac_legged_hw/config/go1_reference.info` and enables the persistent +velocity-reference converter. The ordinary upstream controller launch keeps +the original Go1 posture and target converter. + +### 5. Send a command + +First select a gait from an interactive ROS 1 container terminal: + +```bash +rosrun ocs2_legged_robot_ros legged_robot_gait_command +``` + +For example, select `trot`, then publish a conservative command. Begin at +`0.05` m/s and increase it only after checking stable motion: + +Direct command, matching upstream behavior: + +```bash +rostopic pub -r 10 /cmd_vel geometry_msgs/Twist \ + '{linear: {x: 0.05, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}' +``` + +Stop the publisher and send an explicit zero command before ending the test. + +For the optional path/yaw stabilizer, start bringup with +`start_cmd_filter:=true` and publish commands to `/cmd_vel_user` instead. A +fresh zero command means stop; when commands time out, the filter publishes +nothing. Setting `linear.z` above `0.5` requests the filter's hold mode. + +## Interface contract + +| Direction | Topic | Message | Notes | +|---|---|---|---| +| Isaac to ROS 1 | `/isaac/joint_states` | `sensor_msgs/JointState` | 12 named joints | +| Isaac to ROS 1 | `/isaac/imu` | `sensor_msgs/Imu` | angular velocity in body frame | +| Isaac to ROS 1 | `/isaac/odom` | `nav_msgs/Odometry` | twist in world frame | +| ROS 1 local | `/isaac/contacts` | `std_msgs/Float64MultiArray` | order LF, LH, RF, RH | +| ROS 1 to Isaac | `/isaac/joint_cmd` | `std_msgs/Float64MultiArray` | 60 values; five per joint | +| ROS 1 public | `/ground_truth/state` | `nav_msgs/Odometry` | used by cheater estimation | + +Joint order is LF, LH, RF, RH, with HAA, HFE, KFE for each leg. Each joint +command is `[q_des, dq_des, kp, kd, feed_forward]`. The Isaac script converts +that hybrid command to torque, clips it to its configured limit, and applies +it with `ArticulationAction`. + +Contacts deliberately come from the MPC mode schedule. Isaac's geometric foot +contact estimate is published only as `/isaac/contacts_debug`; the ROS 1 TCP +client drops it to prevent two publishers from fighting over `/isaac/contacts`. + +## Checks and troubleshooting + +The complete GUI path was exercised with Isaac Sim 5.1: all 12 joints were +mapped, the controller remained `running`, ROS 1 joint state arrived at about +231 Hz, and a 3-second `0.05` m/s trot moved the base about 0.12 m forward. The +robot returned to a stable, upright stance at approximately 0.30 m base height +after the zero command. Rates depend on rendering and host load. + +Run these before enabling motion: + +```bash +rostopic hz /isaac/joint_states +rostopic hz /isaac/imu +rostopic hz /isaac/odom +rostopic echo -n 1 /isaac/contacts +rostopic echo -n 1 /clock +rostopic list | grep -E 'isaac|ground_truth|controller_manager' +``` + +- No TCP connection: verify the host address from inside the container with + `getent hosts host.docker.internal` and ensure port 50055 is not firewalled. +- No Isaac topics: confirm Simulation is playing, the ROS 2 bridge is enabled, + and the Isaac script completed without a missing-prim error. +- No `/clock`: the ROS 1 bridge derives it from the Isaac odometry header to + avoid competing clock publishers. +- Robot receives no torque: confirm `/isaac/joint_cmd` contains 60 values and + the controller is in `running` state using `controller_manager/list_controllers`. +- Wrong or unstable motion: stop the controller first. Verify the USD prim and + joint names, spawn height, joint order, and physics frequency before tuning + gains or torque limits. + +## Safety + +Start with no velocity command and keep the robot clear of obstacles. This is +research software: the bridge clips values but is not a certified safety +controller. Never connect the cheater controller or these simulation torque +settings directly to physical hardware. diff --git a/isaac_legged_hw/CMakeLists.txt b/isaac_legged_hw/CMakeLists.txt new file mode 100644 index 00000000..ef6ef397 --- /dev/null +++ b/isaac_legged_hw/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.10) +project(isaac_legged_hw) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +find_package(catkin REQUIRED COMPONENTS + hardware_interface + legged_common + legged_hw + nav_msgs + roscpp + sensor_msgs + std_msgs + urdf +) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS + hardware_interface + legged_common + legged_hw + nav_msgs + roscpp + sensor_msgs + std_msgs + urdf +) + +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_executable(isaac_hw_node + src/isaac_hw_node.cpp + src/IsaacHW.cpp +) +add_dependencies(isaac_hw_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(isaac_hw_node ${catkin_LIBRARIES}) + +catkin_install_python(PROGRAMS + scripts/isaac_gazebo_like_cmd_filter.py + scripts/mode_to_contacts.py + scripts/ros1_socket_bridge.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS isaac_hw_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) + +install(DIRECTORY config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config +) diff --git a/isaac_legged_hw/config/go1_reference.info b/isaac_legged_hw/config/go1_reference.info new file mode 100644 index 00000000..9e5a08c9 --- /dev/null +++ b/isaac_legged_hw/config/go1_reference.info @@ -0,0 +1,46 @@ +targetDisplacementVelocity 0.5; +targetRotationVelocity 1.57; + +comHeight 0.3 + +defaultJointState +{ + (0,0) -0.00 ; LF_HAA + (1,0) 0.72 ; LF_HFE + (2,0) -1.44 ; LF_KFE + (3,0) -0.00 ; LH_HAA + (4,0) 0.72 ; LH_HFE + (5,0) -1.44 ; LH_KFE + (6,0) 0.00 ; RF_HAA + (7,0) 0.72 ; RF_HFE + (8,0) -1.44 ; RF_KFE + (9,0) 0.00 ; RH_HAA + (10,0) 0.72 ; RH_HFE + (11,0) -1.44 ; RH_KFE +} + +initialModeSchedule +{ + modeSequence + { + [0] STANCE + [1] STANCE + } + eventTimes + { + [0] 0.5 + } +} + +defaultModeSequenceTemplate +{ + modeSequence + { + [0] STANCE + } + switchingTimes + { + [0] 0.0 + [1] 1.0 + } +} diff --git a/isaac_legged_hw/include/isaac_legged_hw/IsaacHW.h b/isaac_legged_hw/include/isaac_legged_hw/IsaacHW.h new file mode 100644 index 00000000..5ac09674 --- /dev/null +++ b/isaac_legged_hw/include/isaac_legged_hw/IsaacHW.h @@ -0,0 +1,75 @@ +#pragma once + +#include + +#include +#include +#include +#include + +#include +#include + +namespace legged { + +const std::vector ISAAC_CONTACT_SENSOR_NAMES = { + "LF_FOOT", + "LH_FOOT", + "RF_FOOT", + "RH_FOOT" +}; +struct IsaacMotorData { + double pos_{0.0}, vel_{0.0}, tau_{0.0}; // measured state + double posDes_{0.0}, velDes_{0.0}, kp_{0.0}, kd_{0.0}, ff_{0.0}; // command +}; + +struct IsaacImuData { + double ori_[4] = {0.0, 0.0, 0.0, 1.0}; + double oriCov_[9] = {0.0}; + double angularVel_[3] = {0.0}; + double angularVelCov_[9] = {0.0}; + double linearAcc_[3] = {0.0}; + double linearAccCov_[9] = {0.0}; +}; + +class IsaacHW : public LeggedHW { + public: + IsaacHW() = default; + + bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override; + void read(const ros::Time& time, const ros::Duration& period) override; + void write(const ros::Time& time, const ros::Duration& period) override; + + private: + bool setupJoints(); + bool setupImu(); + bool setupContactSensor(ros::NodeHandle& nh); + + void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg); + void imuCallback(const sensor_msgs::Imu::ConstPtr& msg); + void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); + void contactCallback(const std_msgs::Float64MultiArray::ConstPtr& msg); + + IsaacMotorData jointData_[12]{}; + IsaacImuData imuData_{}; + bool contactState_[4]{}; + + sensor_msgs::JointState latestJointState_; + sensor_msgs::Imu latestImu_; + nav_msgs::Odometry latestOdom_; + + bool jointStateReceived_ = false; + bool imuReceived_ = false; + bool odomReceived_ = false; + bool contactReceived_ = false; + + ros::Subscriber jointStateSub_; + ros::Subscriber imuSub_; + ros::Subscriber odomSub_; + ros::Subscriber contactSub_; + + ros::Publisher jointCmdPub_; + ros::Publisher groundTruthPub_; +}; + +} // namespace legged \ No newline at end of file diff --git a/isaac_legged_hw/launch/bringup_isaac.launch b/isaac_legged_hw/launch/bringup_isaac.launch new file mode 100644 index 00000000..76819241 --- /dev/null +++ b/isaac_legged_hw/launch/bringup_isaac.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/isaac_legged_hw/launch/control_isaac.launch b/isaac_legged_hw/launch/control_isaac.launch new file mode 100644 index 00000000..afcb2da6 --- /dev/null +++ b/isaac_legged_hw/launch/control_isaac.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/isaac_legged_hw/package.xml b/isaac_legged_hw/package.xml new file mode 100644 index 00000000..09448def --- /dev/null +++ b/isaac_legged_hw/package.xml @@ -0,0 +1,32 @@ + + + isaac_legged_hw + 0.1.0 + ROS 1 hardware interface and socket client for running legged_control with Isaac Sim. + + Kawthar + BSD-3-Clause + + catkin + + geometry_msgs + hardware_interface + legged_common + legged_hw + nav_msgs + ocs2_msgs + roscpp + rosgraph_msgs + sensor_msgs + std_msgs + tf + tf2_ros + urdf + + controller_manager + legged_controllers + legged_unitree_description + xacro + + + diff --git a/isaac_legged_hw/scripts/isaac_gazebo_like_cmd_filter.py b/isaac_legged_hw/scripts/isaac_gazebo_like_cmd_filter.py new file mode 100755 index 00000000..ff11eed0 --- /dev/null +++ b/isaac_legged_hw/scripts/isaac_gazebo_like_cmd_filter.py @@ -0,0 +1,426 @@ +#!/usr/bin/env python3 + +import math +import time +import rospy +from geometry_msgs.msg import Twist +from ocs2_msgs.msg import mpc_observation + + +# ============================================================ +# Isaac Gazebo-like command filter v6 +# ============================================================ +# +# IMPORTANT: +# This version DOES NOT publish /cmd_vel by itself. +# +# It publishes only when /cmd_vel_user is fresh. +# +# Behavior: +# - No /cmd_vel_user command: +# publish nothing +# +# - linear.z > STAND_Z_THRESHOLD: +# stand / hold current x,y,yaw +# +# - linear.x or linear.y: +# walking with yaw + cross-track correction +# +# - angular.z: +# pass-through yaw rotation +# +# This prevents the robot from standing by itself. +# ============================================================ + + +# Walking yaw correction +K_YAW = 1.20 +MAX_WZ_CORR = 0.55 + +# Walking cross-track correction +USE_CROSSTRACK_CORRECTION = True +K_CROSSTRACK = 0.45 +MAX_V_CORR = 0.15 + +# Explicit stand/hold correction +USE_STAND_HOLD = True +K_HOLD_POS = 0.65 +MAX_HOLD_V = 0.12 + +K_HOLD_YAW = 1.20 +MAX_HOLD_WZ = 0.45 + +# Thresholds +MIN_SPEED_TO_LOCK_PATH = 0.02 +TURN_CMD_THRESHOLD = 0.03 + +# If command publisher stops, this filter stops publishing /cmd_vel. +COMMAND_TIMEOUT_SEC = 0.35 + +# linear.z > 0.5 means explicit stand/hold request +STAND_Z_THRESHOLD = 0.5 + +# Relock path if command direction changes +RELOCK_DIR_ANGLE_DEG = 20.0 +RELOCK_DIR_DOT = math.cos(math.radians(RELOCK_DIR_ANGLE_DEG)) + + +def wrap_pi(a): + while a > math.pi: + a -= 2.0 * math.pi + while a < -math.pi: + a += 2.0 * math.pi + return a + + +def clamp(x, lo, hi): + return max(lo, min(hi, x)) + + +def rot2d(yaw, vx, vy): + c = math.cos(yaw) + s = math.sin(yaw) + return c * vx - s * vy, s * vx + c * vy + + +def limit_vec2(x, y, max_norm): + n = math.sqrt(x * x + y * y) + if n > max_norm and n > 1e-9: + scale = max_norm / n + return x * scale, y * scale + return x, y + + +class IsaacGazeboLikeCmdFilter: + def __init__(self): + rospy.init_node("isaac_gazebo_like_cmd_filter", anonymous=False) + + self.cmd_user = None + self.have_cmd = False + self.last_cmd_time = None + + self.x = 0.0 + self.y = 0.0 + self.yaw = 0.0 + self.have_obs = False + + self.have_yaw_ref = False + self.yaw_ref = 0.0 + + self.path_locked = False + self.x_ref = 0.0 + self.y_ref = 0.0 + + self.dir_world_x = 1.0 + self.dir_world_y = 0.0 + self.normal_world_x = 0.0 + self.normal_world_y = 1.0 + + self.last_body_dir_x = 1.0 + self.last_body_dir_y = 0.0 + self.have_last_body_dir = False + + self.hold_locked = False + self.hold_x = 0.0 + self.hold_y = 0.0 + self.hold_yaw = 0.0 + + self.was_turning = False + self.timeout_printed = False + + self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) + + rospy.Subscriber("/cmd_vel_user", Twist, self.cmd_cb, queue_size=10) + rospy.Subscriber("/legged_robot_mpc_observation", mpc_observation, self.obs_cb, queue_size=10) + + self.timer = rospy.Timer(rospy.Duration(0.01), self.timer_cb) + + rospy.loginfo("================================================") + rospy.loginfo("[isaac_gazebo_like_cmd_filter v6] started") + rospy.loginfo("[v6] subscribe: /cmd_vel_user") + rospy.loginfo("[v6] publish: /cmd_vel ONLY when cmd_vel_user is fresh") + rospy.loginfo("[v6] NO AUTO ZERO PUBLISH, so robot will not stand by itself") + rospy.loginfo("[v6] STAND/HOLD only when linear.z > %.2f", STAND_Z_THRESHOLD) + rospy.loginfo("================================================") + + def cmd_cb(self, msg): + self.cmd_user = msg + self.have_cmd = True + self.last_cmd_time = time.time() + self.timeout_printed = False + + def obs_cb(self, msg): + s = msg.state.value + if len(s) < 12: + return + + self.x = float(s[6]) + self.y = float(s[7]) + self.yaw = float(s[9]) + self.have_obs = True + + if not self.have_yaw_ref: + self.yaw_ref = self.yaw + self.have_yaw_ref = True + rospy.loginfo( + "[v6] yaw_ref locked from first obs: %.4f rad %.2f deg", + self.yaw_ref, + math.degrees(self.yaw_ref), + ) + + def command_is_fresh(self): + if not self.have_cmd or self.last_cmd_time is None: + return False + return (time.time() - self.last_cmd_time) <= COMMAND_TIMEOUT_SEC + + def lock_hold_here(self): + self.hold_x = self.x + self.hold_y = self.y + self.hold_yaw = self.yaw + self.hold_locked = True + + rospy.loginfo( + "[v6] HOLD locked: x=%.3f y=%.3f yaw=%.2f deg", + self.hold_x, + self.hold_y, + math.degrees(self.hold_yaw), + ) + + def unlock_hold(self): + if self.hold_locked: + rospy.loginfo("[v6] HOLD unlocked") + self.hold_locked = False + + def lock_path_here(self, vx_user, vy_user): + speed_xy = math.sqrt(vx_user * vx_user + vy_user * vy_user) + if speed_xy < 1e-9: + return + + self.path_locked = True + self.x_ref = self.x + self.y_ref = self.y + + bx = vx_user / speed_xy + by = vy_user / speed_xy + + self.last_body_dir_x = bx + self.last_body_dir_y = by + self.have_last_body_dir = True + + self.dir_world_x, self.dir_world_y = rot2d(self.yaw_ref, bx, by) + self.normal_world_x = -self.dir_world_y + self.normal_world_y = self.dir_world_x + + rospy.loginfo( + "[v6] PATH locked: x_ref=%.3f y_ref=%.3f yaw_ref=%.2f deg body_dir=[%.2f %.2f] world_dir=[%.3f %.3f]", + self.x_ref, + self.y_ref, + math.degrees(self.yaw_ref), + bx, + by, + self.dir_world_x, + self.dir_world_y, + ) + + def unlock_path(self): + if self.path_locked: + rospy.loginfo("[v6] PATH unlocked") + self.path_locked = False + self.have_last_body_dir = False + + def command_direction_changed(self, vx_user, vy_user): + speed_xy = math.sqrt(vx_user * vx_user + vy_user * vy_user) + if speed_xy < MIN_SPEED_TO_LOCK_PATH: + return False + + bx = vx_user / speed_xy + by = vy_user / speed_xy + + if not self.have_last_body_dir: + return True + + dot = bx * self.last_body_dir_x + by * self.last_body_dir_y + return dot < RELOCK_DIR_DOT + + def timer_cb(self, _event): + # ============================================================ + # CRITICAL FIX: + # If no fresh /cmd_vel_user exists, DO NOT publish /cmd_vel. + # ============================================================ + if not self.command_is_fresh(): + self.unlock_path() + self.unlock_hold() + + if not self.timeout_printed: + rospy.logwarn("[v6] no fresh /cmd_vel_user -> publishing nothing") + self.timeout_printed = True + + return + + if self.cmd_user is None: + return + + cmd = self.cmd_user + + out = Twist() + out.linear.x = cmd.linear.x + out.linear.y = cmd.linear.y + out.linear.z = cmd.linear.z + + out.angular.x = cmd.angular.x + out.angular.y = cmd.angular.y + out.angular.z = cmd.angular.z + + if not self.have_obs or not self.have_yaw_ref: + self.pub.publish(out) + return + + vx_user = cmd.linear.x + vy_user = cmd.linear.y + wz_user = cmd.angular.z + z_user = cmd.linear.z + + speed_xy = math.sqrt(vx_user * vx_user + vy_user * vy_user) + user_is_turning = abs(wz_user) > TURN_CMD_THRESHOLD + stand_hold_requested = z_user > STAND_Z_THRESHOLD + + # ============================================================ + # Case A: yaw rotation command + # ============================================================ + if user_is_turning: + self.unlock_path() + self.unlock_hold() + self.was_turning = True + + rospy.loginfo_throttle( + 0.5, + "[v6 TURN] pass-through wz=%.3f yaw=%.2f deg", + out.angular.z, + math.degrees(self.yaw), + ) + + self.pub.publish(out) + return + + if self.was_turning and not user_is_turning: + self.yaw_ref = self.yaw + self.was_turning = False + self.unlock_path() + rospy.loginfo("[v6] finished turning, new yaw_ref=%.2f deg", math.degrees(self.yaw_ref)) + + # ============================================================ + # Case B: explicit stand/hold command by linear.z + # ============================================================ + if stand_hold_requested: + self.unlock_path() + + if USE_STAND_HOLD: + if not self.hold_locked: + self.lock_hold_here() + + ex = self.hold_x - self.x + ey = self.hold_y - self.y + + v_world_x = K_HOLD_POS * ex + v_world_y = K_HOLD_POS * ey + v_world_x, v_world_y = limit_vec2(v_world_x, v_world_y, MAX_HOLD_V) + + v_body_x, v_body_y = rot2d(-self.yaw, v_world_x, v_world_y) + + yaw_err = wrap_pi(self.hold_yaw - self.yaw) + wz_corr = clamp(K_HOLD_YAW * yaw_err, -MAX_HOLD_WZ, MAX_HOLD_WZ) + + out.linear.x = v_body_x + out.linear.y = v_body_y + out.linear.z = 0.0 + out.angular.z = wz_corr + + rospy.loginfo_throttle( + 0.5, + "[v6 HOLD] err_xy=[%.3f %.3f] v_body=[%.3f %.3f] yaw=%.2f hold=%.2f wz=%.3f", + ex, + ey, + v_body_x, + v_body_y, + math.degrees(self.yaw), + math.degrees(self.hold_yaw), + wz_corr, + ) + + self.pub.publish(out) + return + + # ============================================================ + # Case C: zero command but fresh + # Publish zero only because user explicitly sent zero. + # No hold. + # ============================================================ + if speed_xy < MIN_SPEED_TO_LOCK_PATH: + self.unlock_path() + self.unlock_hold() + + out.linear.x = 0.0 + out.linear.y = 0.0 + out.linear.z = 0.0 + out.angular.z = 0.0 + + rospy.loginfo_throttle(0.5, "[v6 ZERO] explicit zero command -> publishing zero, no hold") + self.pub.publish(out) + return + + # ============================================================ + # Case D: linear walking command + # ============================================================ + self.unlock_hold() + + if (not self.path_locked) or self.command_direction_changed(vx_user, vy_user): + self.lock_path_here(vx_user, vy_user) + + yaw_err = wrap_pi(self.yaw_ref - self.yaw) + wz_corr = clamp(K_YAW * yaw_err, -MAX_WZ_CORR, MAX_WZ_CORR) + out.angular.z += wz_corr + + cte = 0.0 + v_corr_body_x = 0.0 + v_corr_body_y = 0.0 + + if USE_CROSSTRACK_CORRECTION and self.path_locked: + dx = self.x - self.x_ref + dy = self.y - self.y_ref + + cte = dx * self.normal_world_x + dy * self.normal_world_y + + v_corr_world_x = -K_CROSSTRACK * cte * self.normal_world_x + v_corr_world_y = -K_CROSSTRACK * cte * self.normal_world_y + v_corr_world_x, v_corr_world_y = limit_vec2(v_corr_world_x, v_corr_world_y, MAX_V_CORR) + + v_corr_body_x, v_corr_body_y = rot2d(-self.yaw, v_corr_world_x, v_corr_world_y) + + out.linear.x += v_corr_body_x + out.linear.y += v_corr_body_y + out.linear.z = 0.0 + + rospy.loginfo_throttle( + 0.5, + "[v6 WALK] cmd=[%.3f %.3f %.3f] yaw=%.2f ref=%.2f err=%.2f wz_corr=%.3f | cte=%.3f v_corr_body=[%.3f %.3f] out=[%.3f %.3f %.3f]", + vx_user, + vy_user, + wz_user, + math.degrees(self.yaw), + math.degrees(self.yaw_ref), + math.degrees(yaw_err), + wz_corr, + cte, + v_corr_body_x, + v_corr_body_y, + out.linear.x, + out.linear.y, + out.angular.z, + ) + + self.pub.publish(out) + + +if __name__ == "__main__": + IsaacGazeboLikeCmdFilter() + rospy.spin() diff --git a/isaac_legged_hw/scripts/mode_to_contacts.py b/isaac_legged_hw/scripts/mode_to_contacts.py new file mode 100755 index 00000000..7b8f4415 --- /dev/null +++ b/isaac_legged_hw/scripts/mode_to_contacts.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +import rospy +from std_msgs.msg import Float64MultiArray +from ocs2_msgs.msg import mpc_observation + + +def mode_to_contacts(mode): + mode = int(mode) + + # Contact order: [LF, LH, RF, RH] + return [ + 1.0 if (mode & 1) else 0.0, # LF + 1.0 if (mode & 2) else 0.0, # LH + 1.0 if (mode & 4) else 0.0, # RF + 1.0 if (mode & 8) else 0.0, # RH + ] + + +class ModeToContacts: + def __init__(self): + rospy.init_node("mode_to_contacts", anonymous=False) + + self.pub = rospy.Publisher( + "/isaac/contacts", + Float64MultiArray, + queue_size=10, + ) + + self.sub = rospy.Subscriber( + "/legged_robot_mpc_observation", + mpc_observation, + self.observation_cb, + queue_size=10, + ) + + self.last_mode = None + + rospy.loginfo("[mode_to_contacts] Started") + rospy.loginfo("[mode_to_contacts] Publishing /isaac/contacts from MPC mode") + + def observation_cb(self, msg): + mode = int(msg.mode) + contacts = mode_to_contacts(mode) + + out = Float64MultiArray() + out.data = contacts + self.pub.publish(out) + + if mode != self.last_mode: + rospy.loginfo("[mode_to_contacts] mode=%d -> contacts=%s", mode, contacts) + self.last_mode = mode + + +if __name__ == "__main__": + ModeToContacts() + rospy.spin() diff --git a/isaac_legged_hw/scripts/ros1_socket_bridge.py b/isaac_legged_hw/scripts/ros1_socket_bridge.py new file mode 100755 index 00000000..c9528a2b --- /dev/null +++ b/isaac_legged_hw/scripts/ros1_socket_bridge.py @@ -0,0 +1,420 @@ +#!/usr/bin/env python3 + +import os +import json +import socket +import threading +import queue +import time + +import rospy + +from std_msgs.msg import Float64MultiArray +from sensor_msgs.msg import JointState, Imu +from nav_msgs.msg import Odometry +from rosgraph_msgs.msg import Clock + + +HOST = os.environ.get("ROS2_SOCKET_HOST", "host.docker.internal") +PORT = int(os.environ.get("ROS2_SOCKET_PORT", "50055")) + +RECONNECT_DELAY_SEC = 1.0 +SOCKET_TIMEOUT_SEC = 1.0 +OUT_QUEUE_SIZE = 2000 +PRINT_STATUS_EVERY_SEC = 2.0 + +FORWARD_CONTACTS_FROM_SOCKET = False +FORWARD_CLOCK_PACKET_FROM_SOCKET = False +PUBLISH_CLOCK_FROM_ODOM_HEADER = True + + +class Ros1SocketBridge: + def __init__(self): + rospy.init_node("ros1_socket_bridge", anonymous=False) + + self.out_queue = queue.Queue(maxsize=OUT_QUEUE_SIZE) + + self.sock = None + self.sock_lock = threading.Lock() + self.connected = False + + self.last_cmd_time = None + self.last_feedback_time = None + self.last_clock_time = None + + self.cmd_count = 0 + self.rx_count = 0 + self.bad_packet_count = 0 + self.dropped_contact_count = 0 + self.dropped_clock_count = 0 + self.clock_from_odom_count = 0 + + self.joint_state_pub = rospy.Publisher( + "/isaac/joint_states", + JointState, + queue_size=10, + ) + + self.imu_pub = rospy.Publisher( + "/isaac/imu", + Imu, + queue_size=10, + ) + + self.odom_pub = rospy.Publisher( + "/isaac/odom", + Odometry, + queue_size=10, + ) + + self.contacts_pub = None + if FORWARD_CONTACTS_FROM_SOCKET: + self.contacts_pub = rospy.Publisher( + "/isaac/contacts", + Float64MultiArray, + queue_size=10, + ) + + self.clock_pub = rospy.Publisher( + "/clock", + Clock, + queue_size=10, + ) + + self.joint_cmd_sub = rospy.Subscriber( + "/isaac/joint_cmd", + Float64MultiArray, + self.joint_cmd_cb, + queue_size=20, + ) + + threading.Thread( + target=self.connection_loop, + daemon=True, + ).start() + + rospy.loginfo("================================================") + rospy.loginfo("[ROS1 SOCKET] ROS1 socket bridge started") + rospy.loginfo("[ROS1 SOCKET] Connecting to ROS2 socket bridge at %s:%d", HOST, PORT) + rospy.loginfo("[ROS1 SOCKET] ROS1 sub: /isaac/joint_cmd") + rospy.loginfo("[ROS1 SOCKET] ROS1 pub: /isaac/joint_states /isaac/imu /isaac/odom /clock") + rospy.loginfo("[ROS1 SOCKET] Contacts from socket forwarded: %s", FORWARD_CONTACTS_FROM_SOCKET) + rospy.loginfo("[ROS1 SOCKET] ROS2 /clock packet forwarded: %s", FORWARD_CLOCK_PACKET_FROM_SOCKET) + rospy.loginfo("[ROS1 SOCKET] ROS1 /clock from /isaac/odom header: %s", PUBLISH_CLOCK_FROM_ODOM_HEADER) + rospy.loginfo("================================================") + + def stamp_from_dict(self, d): + sec = int(d.get("sec", 0)) + nanosec = int(d.get("nanosec", 0)) + return rospy.Time(sec, nanosec) + + def fill_header(self, header, d): + stamp = d.get("stamp", {}) + header.stamp = self.stamp_from_dict(stamp) + header.frame_id = d.get("frame_id", "") + + def fill_vec3(self, target, d): + target.x = float(d.get("x", 0.0)) + target.y = float(d.get("y", 0.0)) + target.z = float(d.get("z", 0.0)) + + def fill_quat(self, target, d): + target.x = float(d.get("x", 0.0)) + target.y = float(d.get("y", 0.0)) + target.z = float(d.get("z", 0.0)) + target.w = float(d.get("w", 1.0)) + + def publish_clock_from_header_dict(self, header_dict): + if not PUBLISH_CLOCK_FROM_ODOM_HEADER: + return + + stamp_dict = header_dict.get("stamp", {}) + msg = Clock() + msg.clock = self.stamp_from_dict(stamp_dict) + self.clock_pub.publish(msg) + + self.clock_from_odom_count += 1 + self.last_clock_time = time.time() + + def joint_cmd_cb(self, msg): + data = [float(x) for x in msg.data] + + if len(data) != 60: + rospy.logwarn_throttle( + 1.0, + "[ROS1 SOCKET] /isaac/joint_cmd has wrong length: %d, expected 60", + len(data), + ) + + packet = { + "topic": "/isaac/joint_cmd", + "type": "std_msgs/Float64MultiArray", + "data": data, + } + + self.cmd_count += 1 + self.last_cmd_time = time.time() + + try: + self.out_queue.put_nowait(packet) + except queue.Full: + try: + self.out_queue.get_nowait() + except queue.Empty: + pass + + try: + self.out_queue.put_nowait(packet) + except queue.Full: + pass + + def connection_loop(self): + while not rospy.is_shutdown(): + s = None + + try: + rospy.loginfo("[ROS1 SOCKET] Connecting to %s:%d ...", HOST, PORT) + + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + s.settimeout(SOCKET_TIMEOUT_SEC) + s.connect((HOST, PORT)) + s.settimeout(None) + + with self.sock_lock: + self.sock = s + self.connected = True + + rospy.loginfo("[ROS1 SOCKET] Connected to ROS2 socket bridge at %s:%d", HOST, PORT) + + send_thread = threading.Thread( + target=self.send_loop, + args=(s,), + daemon=True, + ) + + recv_thread = threading.Thread( + target=self.recv_loop, + args=(s,), + daemon=True, + ) + + status_thread = threading.Thread( + target=self.status_loop, + daemon=True, + ) + + send_thread.start() + recv_thread.start() + status_thread.start() + + recv_thread.join() + + except Exception as e: + rospy.logwarn("[ROS1 SOCKET] Connection failed: %s", str(e)) + + finally: + with self.sock_lock: + if self.sock is s: + self.sock = None + self.connected = False + + if s is not None: + try: + s.shutdown(socket.SHUT_RDWR) + except Exception: + pass + + try: + s.close() + except Exception: + pass + + rospy.logwarn("[ROS1 SOCKET] Disconnected. Retrying in %.1f sec...", RECONNECT_DELAY_SEC) + time.sleep(RECONNECT_DELAY_SEC) + + def send_loop(self, s): + while not rospy.is_shutdown(): + try: + packet = self.out_queue.get(timeout=0.2) + line = json.dumps(packet, separators=(",", ":")) + "\n" + s.sendall(line.encode("utf-8")) + + except queue.Empty: + continue + + except Exception as e: + rospy.logwarn("[ROS1 SOCKET] Send loop stopped: %s", str(e)) + break + + def recv_loop(self, s): + try: + file_obj = s.makefile("r") + except Exception as e: + rospy.logwarn("[ROS1 SOCKET] Could not create socket file object: %s", str(e)) + return + + while not rospy.is_shutdown(): + try: + line = file_obj.readline() + + if not line: + rospy.logwarn("[ROS1 SOCKET] Socket closed by ROS2 bridge") + break + + packet = json.loads(line) + self.rx_count += 1 + self.last_feedback_time = time.time() + + self.handle_packet(packet) + + except Exception as e: + self.bad_packet_count += 1 + rospy.logwarn_throttle( + 1.0, + "[ROS1 SOCKET] Bad packet or receive error: %s", + str(e), + ) + + def handle_packet(self, packet): + topic = packet.get("topic", "") + + if topic == "/isaac/joint_states": + msg = JointState() + + self.fill_header(msg.header, packet.get("header", {})) + + msg.name = list(packet.get("name", [])) + msg.position = [float(x) for x in packet.get("position", [])] + msg.velocity = [float(x) for x in packet.get("velocity", [])] + msg.effort = [float(x) for x in packet.get("effort", [])] + + self.joint_state_pub.publish(msg) + + elif topic == "/isaac/imu": + msg = Imu() + + self.fill_header(msg.header, packet.get("header", {})) + + self.fill_quat(msg.orientation, packet.get("orientation", {})) + self.fill_vec3(msg.angular_velocity, packet.get("angular_velocity", {})) + self.fill_vec3(msg.linear_acceleration, packet.get("linear_acceleration", {})) + + msg.orientation_covariance = [ + float(x) for x in packet.get("orientation_covariance", [0.0] * 9) + ] + + msg.angular_velocity_covariance = [ + float(x) for x in packet.get("angular_velocity_covariance", [0.0] * 9) + ] + + msg.linear_acceleration_covariance = [ + float(x) for x in packet.get("linear_acceleration_covariance", [0.0] * 9) + ] + + self.imu_pub.publish(msg) + + elif topic == "/isaac/odom": + msg = Odometry() + + self.fill_header(msg.header, packet.get("header", {})) + msg.child_frame_id = packet.get("child_frame_id", "base") + + self.fill_vec3(msg.pose.pose.position, packet.get("position", {})) + self.fill_quat(msg.pose.pose.orientation, packet.get("orientation", {})) + + self.fill_vec3(msg.twist.twist.linear, packet.get("linear", {})) + self.fill_vec3(msg.twist.twist.angular, packet.get("angular", {})) + + msg.pose.covariance = [ + float(x) for x in packet.get("pose_covariance", [0.0] * 36) + ] + + msg.twist.covariance = [ + float(x) for x in packet.get("twist_covariance", [0.0] * 36) + ] + + self.odom_pub.publish(msg) + + # Critical fix: + # ROS1 /clock follows Isaac odom header stamp. + self.publish_clock_from_header_dict(packet.get("header", {})) + + elif topic == "/isaac/contacts": + if FORWARD_CONTACTS_FROM_SOCKET and self.contacts_pub is not None: + msg = Float64MultiArray() + msg.data = [float(x) for x in packet.get("data", [])] + self.contacts_pub.publish(msg) + else: + self.dropped_contact_count += 1 + rospy.logwarn_throttle( + 2.0, + "[ROS1 SOCKET] Dropping /isaac/contacts from socket. Real contacts come from mode_to_contacts.py. dropped=%d", + self.dropped_contact_count, + ) + return + + elif topic == "/isaac/contacts_debug": + self.dropped_contact_count += 1 + return + + elif topic == "/clock": + if FORWARD_CLOCK_PACKET_FROM_SOCKET: + msg = Clock() + msg.clock = self.stamp_from_dict(packet.get("clock", {})) + self.clock_pub.publish(msg) + else: + self.dropped_clock_count += 1 + rospy.logwarn_throttle( + 2.0, + "[ROS1 SOCKET] Dropping forwarded ROS2 /clock. ROS1 /clock comes from /isaac/odom header. dropped=%d", + self.dropped_clock_count, + ) + return + + else: + rospy.logwarn_throttle( + 2.0, + "[ROS1 SOCKET] Unknown packet topic from ROS2 bridge: %s", + topic, + ) + + def status_loop(self): + while not rospy.is_shutdown(): + time.sleep(PRINT_STATUS_EVERY_SEC) + + with self.sock_lock: + connected = self.connected + + if not connected: + break + + now = time.time() + + cmd_age = None if self.last_cmd_time is None else now - self.last_cmd_time + feedback_age = None if self.last_feedback_time is None else now - self.last_feedback_time + clock_age = None if self.last_clock_time is None else now - self.last_clock_time + + rospy.loginfo( + "[ROS1 SOCKET] status | connected=%s | queued=%d | cmd_count=%d | rx_count=%d | bad_packets=%d | dropped_contacts=%d | dropped_clock=%d | clock_from_odom=%d | last_cmd_age=%s | last_feedback_age=%s | last_clock_age=%s", + connected, + self.out_queue.qsize(), + self.cmd_count, + self.rx_count, + self.bad_packet_count, + self.dropped_contact_count, + self.dropped_clock_count, + self.clock_from_odom_count, + "None" if cmd_age is None else "%.3f sec" % cmd_age, + "None" if feedback_age is None else "%.3f sec" % feedback_age, + "None" if clock_age is None else "%.3f sec" % clock_age, + ) + + +def main(): + Ros1SocketBridge() + rospy.spin() + + +if __name__ == "__main__": + main() diff --git a/isaac_legged_hw/src/IsaacHW.cpp b/isaac_legged_hw/src/IsaacHW.cpp new file mode 100644 index 00000000..544bed83 --- /dev/null +++ b/isaac_legged_hw/src/IsaacHW.cpp @@ -0,0 +1,733 @@ +// +// Isaac backend modeled after Gazebo LeggedHWSim behavior +// FIXED VERSION FOR PYTHON GAZEBO-STYLE TORQUE BRIDGE +// +// Responsibility split: +// +// Python bridge: +// - Publishes backend Isaac topics: +// /isaac/joint_states +// /isaac/imu +// /isaac/odom +// /isaac/contacts +// - Receives /isaac/joint_cmd. +// - Applies command to Isaac. +// +// C++ IsaacHW: +// - Subscribes to backend Isaac topics. +// - Exposes HybridJointInterface to the controller. +// - Publishes [q_des, dq_des, kp, kd, ff] x 12 to /isaac/joint_cmd. +// - Republishes public Gazebo-style ground-truth odom only: +// /ground_truth/state frame_id = world, child_frame_id = base +// - Does NOT publish /odom here. +// - Does NOT publish odom -> base TF here. +// Reason: the base legged stack/controller already interacts with odom/TF internally. +// Publishing /odom/TF again from this backend can create mixed frames or duplicate odom data. +// - Does NOT compute final torque. +// - Does NOT apply stand correction. +// - Does NOT force high gains. +// - Does NOT command a fallback stand pose when controller is inactive. +// + +#include "isaac_legged_hw/IsaacHW.h" + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace { + +constexpr int NUM_JOINTS = 12; +constexpr int NUM_LEGS = 4; + +// File-scope publisher, so IsaacHW.h does NOT need odomPub_. +// /odom is intentionally not published by this backend. + +// Controller / OCS2 order: +// [LF_HAA, LF_HFE, LF_KFE, +// LH_HAA, LH_HFE, LH_KFE, +// RF_HAA, RF_HFE, RF_KFE, +// RH_HAA, RH_HFE, RH_KFE] + +// Used only as safe internal initialization before real /isaac/joint_states arrives. +// This is NOT published as an automatic stand command. +const std::array GAZEBO_STAND_Q = { + 0.0794857511, 0.7901606123, -1.6145155326, + 0.0721311389, 0.7799202724, -1.6157185682, + -0.0761236063, 0.7883236768, -1.6167676846, + -0.0692159051, 0.7817057874, -1.6177661831, +}; + +// IMPORTANT: +// Keep false. We do not want IsaacHW to generate a stand command by itself. +constexpr bool USE_SAFE_STAND_FALLBACK_WHEN_NO_COMMAND = false; + +// Do not force gains here. +// The controller should decide kp/kd. +// Python bridge can decide how to apply/convert them. +constexpr double MIN_ISAAC_KP = 0.0; +constexpr double MIN_ISAAC_KD = 0.0; + +// Safety clipping before sending to Python. +constexpr double MAX_ISAAC_KP = 45.0; +constexpr double MAX_ISAAC_KD = 10.0; +constexpr double MAX_ISAAC_FF = 45.0; + +// No q_des smoothing here. +constexpr double Q_DES_ALPHA = 1.0; + +// Mild ff smoothing. +constexpr double FF_ALPHA = 0.35; +constexpr double MAX_FF_STEP = 2.0; + +constexpr double COMMAND_EPS = 1e-6; + +// Critical: +// Disable old q_des correction. Python/Isaac should receive controller targets. +constexpr bool ENABLE_ISAAC_STAND_CONVENTION_CORRECTION = false; + +inline double safeValue(double value, double fallback = 0.0) { + return std::isfinite(value) ? value : fallback; +} + +inline double clampValue(double value, double lo, double hi) { + return std::max(lo, std::min(value, hi)); +} + +inline double rateLimit(double target, double previous, double maxStep) { + const double diff = target - previous; + + if (diff > maxStep) { + return previous + maxStep; + } + + if (diff < -maxStep) { + return previous - maxStep; + } + + return target; +} + +inline int isaacJointNameToIndex(const std::string& name) { + static const std::unordered_map kMap = { + // OCS2 / Gazebo names + {"LF_HAA", 0}, {"LF_HFE", 1}, {"LF_KFE", 2}, + {"LH_HAA", 3}, {"LH_HFE", 4}, {"LH_KFE", 5}, + {"RF_HAA", 6}, {"RF_HFE", 7}, {"RF_KFE", 8}, + {"RH_HAA", 9}, {"RH_HFE", 10}, {"RH_KFE", 11}, + + // Isaac / Unitree USD names + {"FL_hip_joint", 0}, {"FL_thigh_joint", 1}, {"FL_calf_joint", 2}, + {"RL_hip_joint", 3}, {"RL_thigh_joint", 4}, {"RL_calf_joint", 5}, + {"FR_hip_joint", 6}, {"FR_thigh_joint", 7}, {"FR_calf_joint", 8}, + {"RR_hip_joint", 9}, {"RR_thigh_joint", 10}, {"RR_calf_joint", 11}, + }; + + const auto it = kMap.find(name); + return (it == kMap.end()) ? -1 : it->second; +} + +inline int urdfJointNameToIndex(const std::string& name) { + int legIndex = -1; + int jointIndex = -1; + + if (name.find("LF") != std::string::npos || + name.find("FL") != std::string::npos) { + legIndex = 0; + } else if (name.find("LH") != std::string::npos || + name.find("RL") != std::string::npos) { + legIndex = 1; + } else if (name.find("RF") != std::string::npos || + name.find("FR") != std::string::npos) { + legIndex = 2; + } else if (name.find("RH") != std::string::npos || + name.find("RR") != std::string::npos) { + legIndex = 3; + } else { + return -1; + } + + if (name.find("HAA") != std::string::npos || + name.find("hip") != std::string::npos) { + jointIndex = 0; + } else if (name.find("HFE") != std::string::npos || + name.find("thigh") != std::string::npos) { + jointIndex = 1; + } else if (name.find("KFE") != std::string::npos || + name.find("calf") != std::string::npos) { + jointIndex = 2; + } else { + return -1; + } + + return legIndex * 3 + jointIndex; +} + +inline bool hasRealControllerCommand( + const std::array& qDes, + const std::array& qNow, + const std::array& kp, + const std::array& kd, + const std::array& ff) { + for (int i = 0; i < NUM_JOINTS; ++i) { + if (std::abs(kp[i]) > COMMAND_EPS || + std::abs(kd[i]) > COMMAND_EPS || + std::abs(ff[i]) > COMMAND_EPS) { + return true; + } + + if (std::abs(qDes[i] - qNow[i]) > 1e-4) { + return true; + } + } + + return false; +} + +} // namespace + + +namespace legged { + +bool IsaacHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { + if (!LeggedHW::init(root_nh, robot_hw_nh)) { + ROS_ERROR("Failed to initialize base LeggedHW."); + return false; + } + + setupJoints(); + setupImu(); + setupContactSensor(robot_hw_nh); + + jointStateSub_ = root_nh.subscribe( + "/isaac/joint_states", + 1, + &IsaacHW::jointStateCallback, + this); + + imuSub_ = root_nh.subscribe( + "/isaac/imu", + 1, + &IsaacHW::imuCallback, + this); + + // Backend odom from Python bridge. + // Python publishes /isaac/odom only. + odomSub_ = root_nh.subscribe( + "/isaac/odom", + 1, + &IsaacHW::odomCallback, + this); + + contactSub_ = root_nh.subscribe( + "/isaac/contacts", + 1, + &IsaacHW::contactCallback, + this); + + jointCmdPub_ = root_nh.advertise( + "/isaac/joint_cmd", + 10); + + // Public Gazebo-style odom topics are published by IsaacHW only. + groundTruthPub_ = root_nh.advertise( + "/ground_truth/state", + 10); + + // Do NOT advertise /odom here. + // In this Isaac backend we publish only /ground_truth/state. + // Publishing /odom from here caused mixed frame_id observations in /odom + // when the base legged stack also handled odom internally. + + // Internal initialization only. + // This does not command Isaac to stand. + for (int i = 0; i < NUM_JOINTS; ++i) { + jointData_[i].pos_ = GAZEBO_STAND_Q[i]; + jointData_[i].vel_ = 0.0; + jointData_[i].tau_ = 0.0; + + jointData_[i].posDes_ = jointData_[i].pos_; + jointData_[i].velDes_ = 0.0; + jointData_[i].kp_ = 0.0; + jointData_[i].kd_ = 0.0; + jointData_[i].ff_ = 0.0; + } + + for (int i = 0; i < NUM_LEGS; ++i) { + contactState_[i] = true; + } + + ROS_INFO("================================================"); + ROS_INFO("IsaacHW FIXED VERSION initialized."); + ROS_INFO("C++ publishes q_des/dq_des/kp/kd/ff only."); + ROS_INFO("Python publishes backend /isaac/odom."); + ROS_INFO("C++ republishes public /ground_truth/state as world->base."); + ROS_INFO("C++ does NOT publish public /odom."); + ROS_INFO("C++ does NOT publish odom->base TF."); + ROS_INFO("No automatic stand fallback when controller is inactive."); + ROS_INFO("No Isaac stand convention correction in C++."); + ROS_INFO("No forced minimum high gains in C++."); + ROS_INFO("Q_DES_ALPHA=%.2f, FF_ALPHA=%.2f, MAX_FF_STEP=%.2f", + Q_DES_ALPHA, FF_ALPHA, MAX_FF_STEP); + ROS_INFO("Gains: MIN_KP=%.2f MIN_KD=%.2f MAX_KP=%.2f MAX_KD=%.2f MAX_FF=%.2f", + MIN_ISAAC_KP, + MIN_ISAAC_KD, + MAX_ISAAC_KP, + MAX_ISAAC_KD, + MAX_ISAAC_FF); + ROS_INFO("Expected order: LF_HAA LF_HFE LF_KFE LH_HAA LH_HFE LH_KFE RF_HAA RF_HFE RF_KFE RH_HAA RH_HFE RH_KFE"); + ROS_INFO("Subscribed: /isaac/joint_states, /isaac/imu, /isaac/odom, /isaac/contacts"); + ROS_INFO("Publishing: /isaac/joint_cmd, /ground_truth/state only from IsaacHW backend"); + ROS_INFO("USE_SAFE_STAND_FALLBACK_WHEN_NO_COMMAND = %s", + USE_SAFE_STAND_FALLBACK_WHEN_NO_COMMAND ? "true" : "false"); + ROS_INFO("ENABLE_ISAAC_STAND_CONVENTION_CORRECTION = %s", + ENABLE_ISAAC_STAND_CONVENTION_CORRECTION ? "true" : "false"); + ROS_INFO("================================================"); + + return true; +} + + +void IsaacHW::read(const ros::Time& /*time*/, const ros::Duration& /*period*/) { + if (jointStateReceived_) { + std::array seen{}; + seen.fill(false); + + for (size_t j = 0; j < latestJointState_.name.size(); ++j) { + const int index = isaacJointNameToIndex(latestJointState_.name[j]); + + if (index < 0 || index >= NUM_JOINTS) { + ROS_WARN_THROTTLE( + 2.0, + "Unknown Isaac joint name received: %s", + latestJointState_.name[j].c_str()); + continue; + } + + seen[index] = true; + + if (j < latestJointState_.position.size()) { + jointData_[index].pos_ = + safeValue(latestJointState_.position[j], jointData_[index].pos_); + } + + if (j < latestJointState_.velocity.size()) { + jointData_[index].vel_ = + safeValue(latestJointState_.velocity[j], 0.0); + } + + if (j < latestJointState_.effort.size()) { + jointData_[index].tau_ = + safeValue(latestJointState_.effort[j], 0.0); + } + } + + ROS_INFO_THROTTLE( + 1.0, + "Isaac q: LF[%.3f %.3f %.3f] LH[%.3f %.3f %.3f] RF[%.3f %.3f %.3f] RH[%.3f %.3f %.3f]", + jointData_[0].pos_, jointData_[1].pos_, jointData_[2].pos_, + jointData_[3].pos_, jointData_[4].pos_, jointData_[5].pos_, + jointData_[6].pos_, jointData_[7].pos_, jointData_[8].pos_, + jointData_[9].pos_, jointData_[10].pos_, jointData_[11].pos_); + + ROS_INFO_THROTTLE( + 2.0, + "Seen joints: LF[%d %d %d] LH[%d %d %d] RF[%d %d %d] RH[%d %d %d]", + seen[0], seen[1], seen[2], + seen[3], seen[4], seen[5], + seen[6], seen[7], seen[8], + seen[9], seen[10], seen[11]); + + } else { + ROS_WARN_THROTTLE( + 1.0, + "No /isaac/joint_states received yet. Internal state uses Gazebo stand q only as estimate."); + } + + if (imuReceived_) { + imuData_.ori_[0] = safeValue(latestImu_.orientation.x, 0.0); + imuData_.ori_[1] = safeValue(latestImu_.orientation.y, 0.0); + imuData_.ori_[2] = safeValue(latestImu_.orientation.z, 0.0); + imuData_.ori_[3] = safeValue(latestImu_.orientation.w, 1.0); + + imuData_.angularVel_[0] = safeValue(latestImu_.angular_velocity.x, 0.0); + imuData_.angularVel_[1] = safeValue(latestImu_.angular_velocity.y, 0.0); + imuData_.angularVel_[2] = safeValue(latestImu_.angular_velocity.z, 0.0); + + imuData_.linearAcc_[0] = safeValue(latestImu_.linear_acceleration.x, 0.0); + imuData_.linearAcc_[1] = safeValue(latestImu_.linear_acceleration.y, 0.0); + imuData_.linearAcc_[2] = safeValue(latestImu_.linear_acceleration.z, 9.81); + } else { + ROS_WARN_THROTTLE(1.0, "No /isaac/imu received yet."); + } + + if (odomReceived_) { + // /isaac/odom comes from Python backend. + // Publish only /ground_truth/state from this hardware backend. + // /odom and odom->base TF are intentionally disabled here to avoid + // mixed frame_id observations and duplicate odom paths. + + nav_msgs::Odometry gt = latestOdom_; + gt.header.frame_id = "world"; + gt.child_frame_id = "base"; + + // Publish only Gazebo-style ground truth. + // Do NOT publish /odom here and do NOT broadcast odom->base TF here. + // This avoids mixed /odom frames from two odom paths inside the same /isaac_hw process. + groundTruthPub_.publish(gt); + + ROS_INFO_THROTTLE( + 1.0, + "IsaacHW published /ground_truth/state world->base only | pos[%.3f %.3f %.3f] lin[%.4f %.4f %.4f] ang[%.4f %.4f %.4f]", + gt.pose.pose.position.x, + gt.pose.pose.position.y, + gt.pose.pose.position.z, + gt.twist.twist.linear.x, + gt.twist.twist.linear.y, + gt.twist.twist.linear.z, + gt.twist.twist.angular.x, + gt.twist.twist.angular.y, + gt.twist.twist.angular.z); + } else { + ROS_WARN_THROTTLE(2.0, "No /isaac/odom received yet."); + } + + if (!contactReceived_) { + ROS_WARN_THROTTLE( + 2.0, + "No /isaac/contacts received yet. Using default contact state."); + } + + // Gazebo LeggedHWSim-like reset. + // Controller update should overwrite these values if active. + // If controller does not overwrite, write() sends zero gains/zero torque. + for (int i = 0; i < NUM_JOINTS; ++i) { + jointData_[i].posDes_ = jointData_[i].pos_; + jointData_[i].velDes_ = 0.0; + jointData_[i].kp_ = 0.0; + jointData_[i].kd_ = 0.0; + jointData_[i].ff_ = 0.0; + } +} + + +void IsaacHW::write(const ros::Time& /*time*/, const ros::Duration& /*period*/) { + std_msgs::Float64MultiArray cmdMsg; + cmdMsg.data.resize(NUM_JOINTS * 5); + + std::array qNow{}; + std::array qDesRaw{}; + std::array velDesRaw{}; + std::array kpRaw{}; + std::array kdRaw{}; + std::array ffRaw{}; + + for (int i = 0; i < NUM_JOINTS; ++i) { + qNow[i] = safeValue(jointData_[i].pos_, GAZEBO_STAND_Q[i]); + qDesRaw[i] = safeValue(jointData_[i].posDes_, qNow[i]); + velDesRaw[i] = safeValue(jointData_[i].velDes_, 0.0); + kpRaw[i] = safeValue(jointData_[i].kp_, 0.0); + kdRaw[i] = safeValue(jointData_[i].kd_, 0.0); + ffRaw[i] = safeValue(jointData_[i].ff_, 0.0); + } + + const bool realCommand = + hasRealControllerCommand(qDesRaw, qNow, kpRaw, kdRaw, ffRaw); + + std::array qOut{}; + std::array velOut{}; + std::array kpOut{}; + std::array kdOut{}; + std::array ffOut{}; + + if (!realCommand) { + if (USE_SAFE_STAND_FALLBACK_WHEN_NO_COMMAND) { + for (int i = 0; i < NUM_JOINTS; ++i) { + qOut[i] = GAZEBO_STAND_Q[i]; + velOut[i] = 0.0; + kpOut[i] = 20.0; + kdOut[i] = 2.0; + ffOut[i] = 0.0; + } + } else { + // No command means no torque. + for (int i = 0; i < NUM_JOINTS; ++i) { + qOut[i] = qNow[i]; + velOut[i] = 0.0; + kpOut[i] = 0.0; + kdOut[i] = 0.0; + ffOut[i] = 0.0; + } + } + } else { + for (int i = 0; i < NUM_JOINTS; ++i) { + qOut[i] = qDesRaw[i]; + velOut[i] = velDesRaw[i]; + + kpOut[i] = clampValue(kpRaw[i], 0.0, MAX_ISAAC_KP); + kdOut[i] = clampValue(kdRaw[i], 0.0, MAX_ISAAC_KD); + ffOut[i] = clampValue(ffRaw[i], -MAX_ISAAC_FF, MAX_ISAAC_FF); + + kpOut[i] = std::max(kpOut[i], MIN_ISAAC_KP); + kdOut[i] = std::max(kdOut[i], MIN_ISAAC_KD); + } + } + + static bool smoothingInitialized = false; + static std::array qSmooth{}; + static std::array ffSmooth{}; + + if (!realCommand) { + smoothingInitialized = false; + } else { + if (!smoothingInitialized) { + qSmooth = qOut; + ffSmooth = ffOut; + smoothingInitialized = true; + } else { + for (int i = 0; i < NUM_JOINTS; ++i) { + qSmooth[i] = + Q_DES_ALPHA * qOut[i] + + (1.0 - Q_DES_ALPHA) * qSmooth[i]; + + const double ffFiltered = + FF_ALPHA * ffOut[i] + + (1.0 - FF_ALPHA) * ffSmooth[i]; + + ffSmooth[i] = + rateLimit(ffFiltered, ffSmooth[i], MAX_FF_STEP); + } + + qOut = qSmooth; + ffOut = ffSmooth; + } + } + + for (int i = 0; i < NUM_JOINTS; ++i) { + cmdMsg.data[5 * i + 0] = qOut[i]; + cmdMsg.data[5 * i + 1] = velOut[i]; + cmdMsg.data[5 * i + 2] = kpOut[i]; + cmdMsg.data[5 * i + 3] = kdOut[i]; + cmdMsg.data[5 * i + 4] = ffOut[i]; + } + + jointCmdPub_.publish(cmdMsg); + + ROS_INFO_THROTTLE( + 1.0, + "Isaac command mode: %s", + realCommand ? "CONTROLLER ACTIVE - COMMAND PACKAGED" : "NO COMMAND - ZERO TORQUE"); + + ROS_INFO_THROTTLE( + 1.0, + "Isaac raw q_des: LF[%.3f %.3f %.3f] LH[%.3f %.3f %.3f] RF[%.3f %.3f %.3f] RH[%.3f %.3f %.3f]", + qDesRaw[0], qDesRaw[1], qDesRaw[2], + qDesRaw[3], qDesRaw[4], qDesRaw[5], + qDesRaw[6], qDesRaw[7], qDesRaw[8], + qDesRaw[9], qDesRaw[10], qDesRaw[11]); + + ROS_INFO_THROTTLE( + 1.0, + "Isaac sent q_des: LF[%.3f %.3f %.3f] LH[%.3f %.3f %.3f] RF[%.3f %.3f %.3f] RH[%.3f %.3f %.3f]", + cmdMsg.data[0], cmdMsg.data[5], cmdMsg.data[10], + cmdMsg.data[15], cmdMsg.data[20], cmdMsg.data[25], + cmdMsg.data[30], cmdMsg.data[35], cmdMsg.data[40], + cmdMsg.data[45], cmdMsg.data[50], cmdMsg.data[55]); + + ROS_INFO_THROTTLE( + 1.0, + "Isaac sent kp/kd/ff: kp LF[%.2f %.2f %.2f] LH[%.2f %.2f %.2f] RF[%.2f %.2f %.2f] RH[%.2f %.2f %.2f] | kd LF[%.2f %.2f %.2f] LH[%.2f %.2f %.2f] RF[%.2f %.2f %.2f] RH[%.2f %.2f %.2f] | ff LF[%.2f %.2f %.2f] LH[%.2f %.2f %.2f] RF[%.2f %.2f %.2f] RH[%.2f %.2f %.2f]", + cmdMsg.data[2], cmdMsg.data[7], cmdMsg.data[12], + cmdMsg.data[17], cmdMsg.data[22], cmdMsg.data[27], + cmdMsg.data[32], cmdMsg.data[37], cmdMsg.data[42], + cmdMsg.data[47], cmdMsg.data[52], cmdMsg.data[57], + + cmdMsg.data[3], cmdMsg.data[8], cmdMsg.data[13], + cmdMsg.data[18], cmdMsg.data[23], cmdMsg.data[28], + cmdMsg.data[33], cmdMsg.data[38], cmdMsg.data[43], + cmdMsg.data[48], cmdMsg.data[53], cmdMsg.data[58], + + cmdMsg.data[4], cmdMsg.data[9], cmdMsg.data[14], + cmdMsg.data[19], cmdMsg.data[24], cmdMsg.data[29], + cmdMsg.data[34], cmdMsg.data[39], cmdMsg.data[44], + cmdMsg.data[49], cmdMsg.data[54], cmdMsg.data[59]); +} + + +bool IsaacHW::setupJoints() { + size_t registeredJoints = 0; + + for (const auto& joint : urdfModel_->joints_) { + const int index = urdfJointNameToIndex(joint.first); + + if (index < 0 || index >= NUM_JOINTS) { + continue; + } + + hardware_interface::JointStateHandle stateHandle( + joint.first, + &jointData_[index].pos_, + &jointData_[index].vel_, + &jointData_[index].tau_); + + jointStateInterface_.registerHandle(stateHandle); + + hybridJointInterface_.registerHandle( + HybridJointHandle( + stateHandle, + &jointData_[index].posDes_, + &jointData_[index].velDes_, + &jointData_[index].kp_, + &jointData_[index].kd_, + &jointData_[index].ff_)); + + ROS_INFO( + "Registered Isaac joint: %s -> controller index %d", + joint.first.c_str(), + index); + + registeredJoints++; + } + + ROS_INFO("Total registered IsaacHW joints: %zu", registeredJoints); + + if (registeredJoints != NUM_JOINTS) { + ROS_WARN( + "Expected 12 registered joints, got %zu. Check URDF joint names.", + registeredJoints); + } + + return true; +} + + +bool IsaacHW::setupImu() { + imuSensorInterface_.registerHandle( + hardware_interface::ImuSensorHandle( + "base_imu", + "base_imu", + imuData_.ori_, + imuData_.oriCov_, + imuData_.angularVel_, + imuData_.angularVelCov_, + imuData_.linearAcc_, + imuData_.linearAccCov_)); + + imuData_.ori_[0] = 0.0; + imuData_.ori_[1] = 0.0; + imuData_.ori_[2] = 0.0; + imuData_.ori_[3] = 1.0; + + imuData_.angularVel_[0] = 0.0; + imuData_.angularVel_[1] = 0.0; + imuData_.angularVel_[2] = 0.0; + + imuData_.linearAcc_[0] = 0.0; + imuData_.linearAcc_[1] = 0.0; + imuData_.linearAcc_[2] = 9.81; + + for (int i = 0; i < 9; ++i) { + imuData_.oriCov_[i] = 0.0; + imuData_.angularVelCov_[i] = 0.0; + imuData_.linearAccCov_[i] = 0.0; + } + + imuData_.oriCov_[0] = 0.0012; + imuData_.oriCov_[4] = 0.0012; + imuData_.oriCov_[8] = 0.0012; + + imuData_.angularVelCov_[0] = 0.0004; + imuData_.angularVelCov_[4] = 0.0004; + imuData_.angularVelCov_[8] = 0.0004; + + imuData_.linearAccCov_[0] = 0.01; + imuData_.linearAccCov_[4] = 0.01; + imuData_.linearAccCov_[8] = 0.01; + + return true; +} + + +bool IsaacHW::setupContactSensor(ros::NodeHandle& /*nh*/) { + for (size_t i = 0; i < ISAAC_CONTACT_SENSOR_NAMES.size(); ++i) { + contactSensorInterface_.registerHandle( + ContactSensorHandle( + ISAAC_CONTACT_SENSOR_NAMES[i], + &contactState_[i])); + } + + for (int i = 0; i < NUM_LEGS; ++i) { + contactState_[i] = true; + } + + return true; +} + + +void IsaacHW::jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) { + latestJointState_ = *msg; + jointStateReceived_ = true; + + ROS_INFO_THROTTLE( + 2.0, + "Received /isaac/joint_states with %zu joints.", + latestJointState_.name.size()); + + if (!latestJointState_.name.empty()) { + std::string names; + + for (const auto& n : latestJointState_.name) { + names += n + " "; + } + + ROS_INFO_THROTTLE( + 5.0, + "Isaac joint names: %s", + names.c_str()); + } +} + + +void IsaacHW::imuCallback(const sensor_msgs::Imu::ConstPtr& msg) { + latestImu_ = *msg; + imuReceived_ = true; +} + + +void IsaacHW::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) { + latestOdom_ = *msg; + odomReceived_ = true; +} + + +void IsaacHW::contactCallback(const std_msgs::Float64MultiArray::ConstPtr& msg) { + if (msg->data.size() < NUM_LEGS) { + ROS_WARN_THROTTLE( + 1.0, + "Received /isaac/contacts with size %zu, expected at least 4.", + msg->data.size()); + return; + } + + for (int i = 0; i < NUM_LEGS; ++i) { + contactState_[i] = msg->data[i] > 0.5; + } + + contactReceived_ = true; + + ROS_INFO_THROTTLE( + 1.0, + "Isaac contacts: LF=%d LH=%d RF=%d RH=%d", + contactState_[0], + contactState_[1], + contactState_[2], + contactState_[3]); +} + +} // namespace legged diff --git a/isaac_legged_hw/src/isaac_hw_node.cpp b/isaac_legged_hw/src/isaac_hw_node.cpp new file mode 100644 index 00000000..5f0138ad --- /dev/null +++ b/isaac_legged_hw/src/isaac_hw_node.cpp @@ -0,0 +1,30 @@ +#include "isaac_legged_hw/IsaacHW.h" +#include + +int main(int argc, char** argv) { + ros::init(argc, argv, "isaac_legged_hw"); + ros::NodeHandle nh; + ros::NodeHandle robotHwNh("~"); + + ros::AsyncSpinner spinner(3); + spinner.start(); + + try { + std::shared_ptr isaacHw = std::make_shared(); + + if (!isaacHw->init(nh, robotHwNh)) { + ROS_FATAL("Failed to initialize the Isaac hardware interface."); + return 1; + } + + legged::LeggedHWLoop controlLoop(nh, isaacHw); + + ros::waitForShutdown(); + } catch (const ros::Exception& e) { + ROS_FATAL_STREAM("Error in the Isaac hardware interface:\n" + << "\t" << e.what()); + return 1; + } + + return 0; +} diff --git a/isaac_sim/assets/README.md b/isaac_sim/assets/README.md new file mode 100644 index 00000000..85d5f83f --- /dev/null +++ b/isaac_sim/assets/README.md @@ -0,0 +1,9 @@ +# Go1 stage + +`go1_comp.usd` is the tested Isaac Sim 5.1 stage used by this integration. It +composes NVIDIA's Isaac Sim Go1 sensor asset and adds the simulation settings +expected by `../scripts/go1_isaac_bridge.py`. + +The referenced NVIDIA asset is not redistributed here. It must be available +through the Isaac Sim asset browser/cache. NVIDIA's terms apply to that asset. +The expected robot paths are `/World/go1` and `/World/go1/trunk`. diff --git a/isaac_sim/assets/go1_comp.usd b/isaac_sim/assets/go1_comp.usd new file mode 100644 index 0000000000000000000000000000000000000000..b79a33c5d55a1eb01e817491788c6f9b2d607909 GIT binary patch literal 31044 zcmeI534B!5`S+i5@14oaWShw(B!G|!n*k&XLB+UaLKX-FGD&8J#U+`{+>n9E%rHwx z5NZUpxS*gw(b|8}f>moRwAQ-Ry0sOp7P0A4X1 zPK?V!pQ|n|_r%Mz@;uZqq77E$Ze0_LlT8w?Eisv>ZyuI-Sm%w$55IYQf64XntKqtN zsegt4YgTa!)`))mh``84-< ze{+`&EFcFA1F{F(2w2podm%g* z0kx2e`^db-YWO9PEh+fm?ox0WxEx#oN`MuVf=QqZl!FRT2_}OnzzDdj0#m^>Pz|Po z8K4H(KrN^P^}r4szzJr8SwOAS>^}09kk#(I3i8$98qfgL_>GV)s;3DawQJmvJwWao zfwz`Fv{PLdZ*0t&;@1{jy!!2%EhVGsdP&;}NQcA!>jSRc8gkGu$S zZ_TfThmUyf7K0_=JK#Fd39biAftu;EKJxND@(q3D8~e!L?IW+~Bi{sB?c$pu-vVw0 zEBl0JL*|XJfE*wI?rsCO1J%D8@*SYJQFp?_RRMQv!8-6gupZn6?gqV)?}10{fgeEr zAMiu40o)7j13vU(nF zpI_v=;W^Y4aJQlKOEUbia2_Wnvz`EIZr*)`N^kzLPS6m&ruJ(*@>efiCF>^s?b=1P z+k3Aky2@8SvS(L`dObBwZ=>Vga*C?qQPmRp6Sn|09xXsP*PkS2bH|o@cUBX^78V6_^U9 z0p1tW!3^+Cjm!&(`&*!v`R^DTwGD3M;{h`0>HD5_mKXkcU_S7HR&cJ(Mr=qR-~ZPw z#XmC+YCFHNJ?K@UeDuT0`K`iiC! z8hcjO-EmFzN3N+t@wnq-*Rxw$-yhH8Y&6w}@jST3P_+i{YuEiCIivV>8{eFz4&r5H(zbY+ykE{#?=u}EmBsQpyRSXI z8_$DVPt}5QyTdnU3*&XU;E$kxb4K(r`r#AsDd^vv^>5DRVI0f{KF|vMAOM130SJLGh=3?)0}DYrh^z2Z zj)^<^gfD`u+J$e+=1kRp-1c!BXaTCN{IA$VwG;oZY}lUQ%|kmk#jm|vmGcp7QHwqY zd$j-5w(MWniSd?yz3VE8IOR~-8&C&?Iv8@0Mh)Z1&*eg80v~nXV4-qiX->`6-uFsg zx%Q{`U%cUPN#Rg7lYMZo9~mw7e|m5<|j@X36}#m*VN?=}?-|W2 z^Un_izRu1bjHN%loqhaEcGfb6mz7C7<7J+WowdjEI=cgb_3=DTXJ=Q$>v*p1?9q6c z-$pz85}pn}6zD$?_@WQCqd(MdYrky=0qSO~Zyo!;WyJ%7a{~{MfeCQe0=!@z=-TC$?x4bxJMXdU$XJ-c_GFIayY zOMj|8em@z? z-EViky>I-gx??*YmAZ%Q-=Fbu+rK~S`=XJmlfNx?r{@WM-Dg*cTHpVKUAgQEUXOo% zK3Q_B{Ja zDlv)=&X{!N){p3Q#dU_6hru`3fxdfI+EyOJBv|6JRZ_3<+M?MlC0;iHc) zU(~Y{zRXc&zWhi7+*N_8U>e{r&rJt2Kn<{gT2Ke-fgL!26U+p&zzg8FK+Su0ANfki zsvW%w^3~uP&;ZoZF;&zAPc4uNd?$LOeXJdDKvAoXi z9Bp$vkJH(m4e>gjYrA8t?CI~**&PkP1;=eEA1%D>--9tw01~d;^<9hb@viEI5s(vk z1h`x<8hknK7=(=l7l3gf4_MR;cw}CwQ{5N8iJ>ZpNo8QTR;xD00pRVSHph?;L4M`v-;}`wc`J>p_0jU zok3Lzef5mmd|tJ4dG2-~nX~*BYZ}y)`cE;mwh?jXbb}F4O6jAT9*&oe{ z=W#kaH6>n03rdL{&*7>L6+U!$nlD_ol@BX5tmo;A*2m-db9)mLaaEtKu}^#xWd27a ziG9LiL0|LRA}8Yc{;RgGC(kdYQs42#>#vU5f7j-z>A8(nBAK(7>FN!eM)p5TZ^;YE@`p7r-k-ytV{yKBV=cPJN zdH$*?`?49FfO{6Tz~M-Ax;3Y2+tdtm5RVsdF%nJ$YWiXbR@KA*JUjL!1R`Dzng3lt z3E&Y*!6Z-y_`3}ipb|_5Q$Q7%3Z?=6%Y)Ow3{V4Xpcd4DdSC|*-~=n zz}4Uy&;S~N3p4>Y;8sxv&0sEQ0bVc<%m+Tu3j81dcqj122O;O+Dgf@j6^n+5M@we) zarM;etDkC((btpEKaPAZhU9RYajl+ z#M+dYv;UHf&pbZ#hL36=f25=-zZblhl~L(yPqyEjY`Z>wAu@W^4C9IBVy0l$aB5mzW;&%$rG%6?B0 z*;?g@92!IWf)2koLNfy)pNBkhvn%S0*h1b`s_?gYLjixQ?2k|#lE@(nBD}1{>+^(U zKe=74a>!LLMM*^+YW2wtj?uxkZ=cy>l+w4V(SchetD0^(*8q_oC1I$#l?M-{K0Qm8G^Sf>7^n zky~A1^7@0(NVwSLpX-y8X^q#9>Y@1^CQ1umN^^?Q?ebXC52Dg=pgE#pqJyL%RMQ-A zM>9yQw(|sZbRY^WZua_QYVmsH2Cu&XrOuTb7JB`jz(Vo|8a%QO&x+PFZ89}Y_63?; zzDz-LBP(;dLgBH31}}k@(1f~3yOG3+^lqfo)8_KKWe=*`P&kv4N77*1NZOwe3!vY zDiQUW3_mUO_es7$pw-sW5%!|vhtkv2U6F`f(=-o*2p zJd-IkpXmIKpoKPLUPOvR@?4ClUs1w3<|_93QK1TU(R5evdP*HnDa;`>l?6f}**%s? zduU5AQrzlluXnWueTmw6)vl=D-D07Fxxwg2ve~@tvQMMkT!$X^~@ zf%LB1g#lU|4x;y6zOrPpc`!BW9%7ws&BI{6;WhT=#I8rl-@d2S%qMgBxNh_H+gv2|UltPAL5?66=?7cc- zPcbo>guP5kCfy)}6@C3nE*^Wv|h{j)_uORIv>5XN-RdEnD5y=*jgQsNQqbTpHi zU2YkFQXP@e2~mF}Y$9uw-{bv>U<@j(OAM1IfaSDYkJ;#feham36RFMcd>gb`vBw(@ zLg||%v6X}qBKC= zeNpq3S7TTOWmjanHxvql&@@k=RUVmOq;@g(o_u9uEww?-l~HK=BpcZ$*WbffIWb!T zGmv#|)a!ZKu#Y0+VW~qfHnbq>itV$SUu0=TcTrl=+ti6dT+v8NU>u2mPBi8HS}MgH z2tFy%SPgq1!QA=>%^Ss@rec4<@AQVFF5eTHqOyADCYCUs*tmpM9$z?IEVs+^@GUxrXfoy&Y_!x|;e1du0L{rv?OLy+bmEjbyAz#jp%@B$KhspTs(tFp+HBSXHhR z_KWd9ocEL5=JKJofza**SFCy+PUM^~?3nM5%!9v;5yjw&kL+qe%Cx z-Rqu@)rHGb?`^$1c`5OImN(q8jOaqmFsNvO-9mTFz(NMQ$p#z4|~Dh!|xlGuE$ zvE3}NR9b_ccENCZJ({G_Xjm>!CtFoH7BJ2PshH|uKO?ToQZm9B`x1fOMU4vrVOuEB$fWb>h6PbMic!00ICl1A z#~5o34ui%B1ikL5q*+b1VQ*{HcMW4@OuULTx&+FgV7crMw0ix&VA2%QTgWmadSi-y zD6N6ks`1w&@ftQt4qYTjn7Fry&+?U*y)w2M8$V4bfSf?4@sD5>eWEYG`X`N;M-X?h zk#Z=Fq!~w?8vZgR3`xa@1LmGCG2?sdLp#9xN6DW!{ zg~%JO@RD)i+X9W#4!>x^PK|b;XtCJWilxPteDBW;>=ixL8mU2J%AZN4`H^NCLYwhG z^Fo2}_`IgjmQ;HlX2|%x5`LK8l=nl*$#XqMgY(8{QYVpCoW_KUHJVXmb7Sh~bx1kk zyeWyISEC;%(H2TwLrknA-2qV=OGz^+X{V^^q@=wh+DUUnOdUz=msUqQ;bW1qjOXmE=>7yFt z_H;HtXKjP3UZ`hBMFs`xdpg|;%@!V++}R4f8>0M~KNMceuP$tuP~K4O^`Lhy($ieA zFdS_fl>Vcd=4NhEX|lXRgR$W*hOL%snvGdxr3>@&@l#5Q2h;p%{5 z^jwn8^wKhUR&f++Ic!PAb7jtS!(iwt1AAS|en-ZJ?aAz^8<6|h&4y3 zBY2IrqZo6-)f$G4E+Iw4Cd6MHE+{B0hM}*M!_$Mglu|Ou>mNbMT|&qmwxdh^bISs9 zbFRg`Ll#ws{ z9$-csda_!osV|q?mQhk6a@d%WJsoA)nlR!kek7!JLlcjs{L)2ZDJ_w))Xikfj=U$P zY8_&}Yf&CE)}TY%HcMJ9z94e2>^i5ALZe8yUP`sGq}!2W>7r8q+!|z4PuX?TVt2Eh zqbNmT9yx;6V2{~~PO0*=Z=$3PvLCTaeO?!aTIy>WX0Whqr-h~ESNI;Flm_Hi;Ty`* zg2BK-IaKLVjH$2U7kY;=+`o@ftW92jIEUre2TQdQwk%q+#GVimXXpxM&!No;8I}ii zECu_ufn5d*Hj@^<_qAZ}GCvx`Ft9XX31N9l_pBu>yF%_3*m`$V(HdP-wEGjNtfszd zMnz?5b#+Z;CC#XrQ98NWr%ey8#ocdvF8G&&u$aTscg%5U9Bh-rVRzUw92#q_!?AA6AnF>Ao4HdB zO%>kJO}WeUo%fN|&^eD89?H$dJcSd|_Kjj6bqkukX8nosjb>amd$T0At2--+{S9Gg zZ91`RYRuYu-if_g0v!>LYosHZ+huEA_cuBk=}}bT&LSi2@>A+Ye(uTDc%?hBSsq%twg~|&^^`^Vmc}u z5vkHq0sf5<9T^icktWePVH$Q6*>r(jN7m8QJ{@-oHCRL79Utou9Tg>7r&!3`s3Y^p zEV48TdMdJz58|#$BNYv&4Ms9QjJuB`;UBW`ukj>WUqR4b1pO~!kpl@v(|Pb-23o)h zumNBA=JB@@nf0}v*n*CYU{)PjEY4gi7)=u}RFT&&A1CvKEHdMcxf&GblKDe03t@siWJ2bs<5447%4b@5MQn(36-x0+S-c@FNO~)R)`N{; zGx#;w3f=%a!N-U$&6=e);{r5vRc5SmQ;_c!8WR;*XiZObd9}WeJm;fY?T|NSkq~6& z3)-1A(K?`hAQcRw2NOtxx-jcQMtYX06VFHok|yB+ExSD#&w*%FsgVkor{);Rdff-x zo`>h}Pi)V^=W{PZ-u~iP3l)x!z6H5$)`j@IcFTwO+;!kcBUx{1yW?3a6~11GW?Ls( zx0T>d;K#QjwRrS_SyVWCa4td~>fVjpM{@^1mrI2|oihSA9zRlq8@A78E^gKikjnTXY)jvDcW zN9Uy#K{7sdek?&2I>}&NhUO2WRfw5QSuK1@1u>E^-OG^dVG|#$8Zra|&|k+}Q}d(RnoX4O(JnrGR?E?TcUOS!hmTlx{1na5yCDN_XID zcfi>-pxlv>;mAlYbyPrYI%>-`85!yJ;gl*6>r#|W8IIbFbZA(OvqhdgV2;yU;qW?~ z>o(1C%#vN39Ghf$);f9iy8A}sv+{oLy4mZTu1%X}ty{f%Q|`LeYu9aRx%m@H1|L(FEu25bmdCvN5=`)jlQ}%TEQ-({QFkJDNq4ZHh*%sR)wZc#B zC#9b_A9M;2IK>}3rTd+lO-}7bXTp!1y8E2^d!2?2&cq)&lm5q<`~zpoJ!_f%{F|cZN#m%k+-zn)F!Ox zxY0TChQ&)43)e3ZPb^=oxlXyBjxS%VU92qDii?z`q&bw-t~iSpDvMLvKvcPz_NPXb z+Dp4kVJPWAudJpab&CDrr8KTqsco>Kw0S6O zHyXBtH|$z8>=@d$g=Q$utEVf@YpNBSXPRP@rz*C&Rm$TuS$UEwl-l{_ip^K1bnIxI zget!1FI8+otMVLO0ffu(Azr5Zj*68RX(A9VR$itHA!sH*5Q~&Q(z5Rq^3UrE_-7~O zD{u1g4!0wI-5tNuehrsgGQLN!SFLrf#j;_o1JsGEZcWE@^U)n z$&PgFCVoMpC{`%J%W>IG!X=U``hZ zsnGoDC*7~Fp;l$=l*%W~ehkEbvRGWYg>1@_<-?NTCML~Sma^-*bCT%}pVEHQb!=(( z&{P^9P=@HKE~tcW8>g;?^GYfG;#HQ}ecrCYyN13uWDM>46Ek%We>>-$oOcRdU}=d| z@ITCJCG8@*^8+@h`^`~rpBQ;DbzQoWxs}}SwIr~`%JJJ4u;Zfcu#g}MZ1JdSrOAnD zB|J~pDJzK$R^B6%MnUK9A$Rv<>_j)jkBMa}yQ#7JSL`?p<42TvOh>D_h?$iSDdY2G zvVKMxM-g<8SgP_VnU5go0GSW#X!8docbgeI`7j+yr2-v0m_S|mTiC< zoxF-IKe<(e}1G}JaJM;Ir_Quxl#L%A)oIV%7o9-n3%G?h6yJhp#zhs zz`(E>>Pr0^wRI;Ua0|WD&9d2FyTy~j$-|VOoDjR&Np7TLZh!1$iVPdxwUNA}v11cs zV|F}#vTAG$yBZtIu<|(Pkr6M+7a(i|f8)Jt0Bj`g+AF3y$gZ(EaLh+`{LgF-X46>l z{a6P6yPPxvS*-ZjZ?+*LtCIGa!n*ZGS@$2HLf_IJ>(+nXopiXHvF`K#mO_@~Ln-8% z^FYe}Ze2E6(m#c6G40F529iNaJ&;2C(uBc#PGo+_x|zbX$5{7os9X_FBpznS23ZTj z2`s_!ZJ>}viHWf4On0=~aJ0MhIRzVqohT=*OFYTT!~fR!EciuFXHd@~;QAaaX)Lil zpFl>mJ^|IwC^x3p4zQ2TC^brYi`|wkVVi=}Jt#bTn%%J)Iau`h)YaJ8v~;QdAzI0V zLPb9s!^I~E!p*R7og`=>>b%O`Bx#}D;3?(mThL0PQ2vxoCn%Lq&KILX++=Xf&vps6qXtF3EHx z8D~YxFpXB!B#5Y|M$2&byz^?N;dDUFiRhkLJ@Se8_x<(ux~dtIzlg9t z*5x?bjVGzBs;Dmi;x<)#dv1)3%`vu~e#@zn_lrpsY literal 0 HcmV?d00001 diff --git a/isaac_sim/run_isaac_sim.sh b/isaac_sim/run_isaac_sim.sh new file mode 100755 index 00000000..955ab12f --- /dev/null +++ b/isaac_sim/run_isaac_sim.sh @@ -0,0 +1,18 @@ +#!/usr/bin/env bash +set -euo pipefail + +if ! command -v isaacsim >/dev/null 2>&1; then + echo "isaacsim is not on PATH. Activate the Isaac Sim Python environment first." >&2 + exit 1 +fi + +# Avoid mixing a sourced ROS 1 environment with Isaac Sim's ROS 2 bridge. +unset ROS_VERSION ROS_PYTHON_VERSION ROS_PACKAGE_PATH AMENT_PREFIX_PATH +unset CMAKE_PREFIX_PATH COLCON_PREFIX_PATH PYTHONPATH + +export ROS_DISTRO="${ROS_DISTRO:-humble}" +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-0}" +export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}" + +exec isaacsim --enable isaacsim.ros2.bridge "$@" diff --git a/isaac_sim/scripts/go1_isaac_bridge.py b/isaac_sim/scripts/go1_isaac_bridge.py new file mode 100755 index 00000000..6ed4338e --- /dev/null +++ b/isaac_sim/scripts/go1_isaac_bridge.py @@ -0,0 +1,1251 @@ +#!/usr/bin/env python3 +""" +GO1 Isaac-ROS2 Bridge v9 +===================== +Purpose of this version: + - Keep Isaac/Gazebo-style ground-truth odom path clean. + - Keep quaternion-derived yaw-rate for odom angular.z. + - Use stable Isaac torque/FF smoothing tuned after the contact-phase and + command-filter fixes. + - Do NOT modify /cmd_vel or target generation here; use the separate + isaac_gazebo_like_cmd_filter.py in ROS1 Docker for Gazebo-like path behavior. + +Fixes applied vs previous versions: + 1. Robot trunk is reset to ZERO YAW and origin XY at initialization. + The MPC initializes its reference from the first observation. Any + nonzero yaw at startup becomes a permanent heading offset causing + the robot to walk obliquely in the world frame. + 2. Odom twist.linear and twist.angular published in WORLD frame, + matching legged_control rbd_state_ convention (linear_vel_x/y/z_global, + angular_vel_x/y/z_global). + 3. IMU angular_velocity published in BODY frame (physical gyro convention). + 4. Quaternion finite-difference NOT used as angular velocity fallback. + Roll/pitch oscillations during trot alias into fake yaw rate in the + world frame and corrupt the Kalman filter with gait-locked yaw drift. + Zero is published instead when the direct API is unavailable. + 5. IMU linear acceleration smoothed (ACC_ALPHA) to suppress foot-strike + velocity spikes. Odom velocity is NOT smoothed. + 6. Direct API velocity fallback threshold changed from > 1e-10 to + np.isfinite, so near-zero valid velocities during stance are not + incorrectly replaced by finite-difference. + 7. TROT_PHASE_OFFSET = 0.0 (removed empirical 0.3 s offset that was + run-specific and could swap the diagonal contact pair incorrectly). +""" + +import builtins +import time +import math +import numpy as np +import omni +import omni.usd +import omni.kit.app +import rclpy + +from std_msgs.msg import Float64MultiArray +from sensor_msgs.msg import Imu, JointState +from nav_msgs.msg import Odometry +from pxr import UsdGeom, UsdPhysics, Sdf + +try: + import omni.physx +except Exception: + omni.physx = None + +try: + import carb +except Exception: + carb = None + +try: + from pxr import PhysxSchema +except Exception: + PhysxSchema = None + +try: + from pxr import Gf +except Exception: + Gf = None + +try: + from isaacsim.core.prims import SingleArticulation + from isaacsim.core.utils.types import ArticulationAction +except Exception: + from omni.isaac.core.articulations import Articulation as SingleArticulation + from omni.isaac.core.utils.types import ArticulationAction + + +# ================================================== +# CONFIG +# ================================================== + +ARTICULATION_ROOT = "/World/go1/trunk" +BASE_PRIM_PATH = "/World/go1/trunk" + +# The go1 top-level prim (parent of trunk). The yaw reset is applied here +# so the entire robot moves, not just the trunk link in isolation. +ROBOT_ROOT_PRIM_PATH = "/World/go1" + +CMD_TOPIC = "/isaac/joint_cmd" +IMU_TOPIC = "/isaac/imu" +CONTACT_TOPIC = "/isaac/contacts_debug" # debug only — real contacts from mode_to_contacts.py +ODOM_TOPIC = "/isaac/odom" +JOINT_STATE_TOPIC = "/isaac/joint_states" + +PHYSICS_HZ = 250.0 +PHYSICS_DT = 1.0 / PHYSICS_HZ + +FOOT_PRIMS = { + "LF": "/World/go1/FL_foot", + "LH": "/World/go1/RL_foot", + "RF": "/World/go1/FR_foot", + "RH": "/World/go1/RR_foot", +} + +GROUND_Z = 0.0 +CONTACT_ON_Z = 0.030 +CONTACT_OFF_Z = 0.060 + +# ================================================== +# SPAWN POSE +# ================================================== +# Reset the robot to this position and zero yaw at bridge init. +# Adjust XY if your ground plane is not at the origin. +# Z is the nominal trunk height above ground for the GO1 standing pose. +SPAWN_X = 0.0 +SPAWN_Y = 0.0 +SPAWN_Z = 0.35 # metres above ground — adjust to your USD ground plane + +# ================================================== +# SCHEDULED TROT CONTACTS +# ================================================== +# Published to CONTACT_TOPIC (/isaac/contacts_debug) only. +# Real /isaac/contacts must come from ROS1 mode_to_contacts.py. +USE_SCHEDULED_TROT_CONTACTS = True +TROT_PERIOD = 0.6 +TROT_SWITCH_TIME = 0.3 +TROT_PHASE_OFFSET = 0.0 # no empirical offset — anchored to sim time + +# ================================================== +# JOINT ORDER MAPPING +# ================================================== +# OCS2/ROS order → Isaac USD joint name +ROS_ORDER_TO_ISAAC_NAME = { + 0: "FL_hip_joint", + 1: "FL_thigh_joint", + 2: "FL_calf_joint", + 3: "RL_hip_joint", + 4: "RL_thigh_joint", + 5: "RL_calf_joint", + 6: "FR_hip_joint", + 7: "FR_thigh_joint", + 8: "FR_calf_joint", + 9: "RR_hip_joint", + 10: "RR_thigh_joint", + 11: "RR_calf_joint", +} + +# Initial joint positions in ROS/OCS2 order (set once at init, not enforced). +GAZEBO_REST_Q_ROS_ORDER = np.array([ + 0.33545, 1.19854, -2.81800, # LF + 0.33541, 1.19855, -2.81800, # LH + -0.33546, 1.19852, -2.81800, # RF + -0.33541, 1.19855, -2.81800, # RH +], dtype=np.float64) + +SET_REST_POSE_ON_INIT = True + +# ================================================== +# COMMAND / ACTUATION LIMITS +# ================================================== +MAX_ABS_KP = 60.0 +MAX_ABS_KD = 8.0 +MAX_ABS_FF = 35.0 +MIN_ACTIVE_KP = 0.0 +MIN_ACTIVE_KD = 0.0 + +# ================================================== +# TORQUE CALCULATION +# ================================================== +# tau = kp*(q_des - q) + kd*(dq_des - dq) + ff +# USD DriveAPI PD is disabled; Isaac receives final torque via ArticulationAction. +MAX_JOINT_TORQUE = 22.0 + +# Stable Isaac torque smoothing. +# Old 0.45/3.0 was too laggy and accumulated yaw. +# Aggressive 1.0/100.0 vibrated in stance/trot. +# This setting is the current stable compromise. +TAU_ALPHA = 0.55 +MAX_TAU_STEP = 2.5 + +# ================================================== +# REAR SUPPORT BOOST (disabled by default) +# ================================================== +REAR_SUPPORT_GAIN_BOOST = False +REAR_SUPPORT_JOINTS = [4, 5, 10, 11] +REAR_HFE_KP_BOOST = 1.25 +REAR_KFE_KP_BOOST = 1.35 +REAR_HFE_KD_BOOST = 1.15 +REAR_KFE_KD_BOOST = 1.20 +REAR_DRIVE_FORCE_BOOST = 1.30 + +# ================================================== +# COMMAND SMOOTHING +# ================================================== +Q_DES_ALPHA = 1.0 # 1.0 = pass-through (no position smoothing) +DQ_DES_ALPHA = 1.0 +FF_ALPHA = 0.30 + +MAX_Q_STEP = 0.15 +MAX_DQ_STEP = 4.0 +MAX_FF_STEP = 1.5 + +COMMAND_TIMEOUT_SEC = 0.60 + +# ================================================== +# PUBLISH RATES +# ================================================== +JOINT_STATE_EVERY_N = 1 +IMU_ODOM_EVERY_N = 1 +CONTACT_EVERY_N = 1 + +# IMU accelerometer smoothing — applied to finite-diff acc only. +# Odom velocity is NOT smoothed. +ACC_ALPHA = 0.30 + +# Yaw drift test fix: +# If Isaac reports tiny angular velocity while the robot is physically still, +# zero it before publishing to prevent gyro integration drift. +OMEGA_DEADBAND = 0.01 # rad/s + +# Most impactful yaw fix: +# Use quaternion-derived yaw-rate for omega_world.z, then low-pass it. +YAW_RATE_ALPHA = 0.15 + +# ================================================== +# DRIVE FORCE LIMITS (kept for reference, drives are disabled) +# ================================================== +MIN_DRIVE_FORCE = 110.0 +EXTRA_DRIVE_FORCE = 55.0 +MAX_DRIVE_FORCE = 170.0 + + +# ================================================== +# CLEANUP OLD BRIDGE INSTANCES +# ================================================== + +BRIDGE_NAME = "GO1_ISAAC_BRIDGE_V9_GAZEBO_STYLE_STABLE_TORQUE" + +OLD_BRIDGE_NAMES = [ + "GO1_ISAAC_BRIDGE_STABLE_HYBRID_USD_PD_FF", + "GO1_ISAAC_BRIDGE_STABLE_HYBRID_TROT_REAR_SUPPORT", + "GO1_ISAAC_BRIDGE_PHYSX_HYBRID_USD_PD_FF", + "GO1_ISAAC_BRIDGE_FORCE_PHYSX_250_HYBRID", + "GO1_ISAAC_BRIDGE_FORCE_PHYSX_250_HYBRID_FIXED", + "GO1_ISAAC_BRIDGE_FORCE_PHYSX_250_HYBRID_ODOM_BODY_TWIST", + "GO1_ISAAC_BRIDGE_FORCE_PHYSX_250_HYBRID_GAZEBO_MATCH", + "GO1_ISAAC_BRIDGE_FORCE_PHYSX_250_HYBRID_LEGGED_GLOBAL_ODOM", + "GO1_ISAAC_BRIDGE_V5_ZERO_YAW_SPAWN", + "GO1_ISAAC_BRIDGE_V6_OMEGA_DEADBAND_COV", + "GO1_ISAAC_BRIDGE_V8_GAZEBO_STYLE_TORQUE_TEST", + "GO1_ISAAC_BRIDGE_V9_GAZEBO_STYLE_STABLE_TORQUE", +] + +for _name in OLD_BRIDGE_NAMES: + if hasattr(builtins, _name): + _old = getattr(builtins, _name) + for _key in ["update_sub", "physics_sub"]: + try: + _sub = _old.get(_key) + if _sub is not None: + if hasattr(_sub, "unsubscribe"): + _sub.unsubscribe() + elif hasattr(_sub, "release"): + _sub.release() + except Exception: + pass + try: + if _old.get("node") is not None: + _old["node"].destroy_node() + except Exception: + pass + try: + delattr(builtins, _name) + except Exception: + pass + print(f"[GO1 BRIDGE] Removed old bridge: {_name}") + + +setattr(builtins, BRIDGE_NAME, { + "node": None, + "cmd_sub": None, + "imu_pub": None, + "contact_pub": None, + "odom_pub": None, + "joint_state_pub": None, + "update_sub": None, + "physics_sub": None, + + "latest_cmd": None, + "command_received": False, + "last_cmd_wall_time": None, + + "robot": None, + "initialized": False, + "joint_indices": None, + "dof_names": None, + "joint_prims": None, + + "last_time": None, + "last_pos": None, + "last_vel": None, + "last_quat": None, + + # State for quaternion-derived yaw-rate fix + "yaw_prev": None, + "yaw_rate_smooth": 0.0, + + "acc_world_smooth": np.zeros(3, dtype=np.float64), + "acc_initialized": False, + + "contact_state": np.ones(4, dtype=np.float64), + + "smooth_initialized": False, + "q_des_smooth": np.zeros(12, dtype=np.float64), + "dq_des_smooth": np.zeros(12, dtype=np.float64), + "ff_smooth": np.zeros(12, dtype=np.float64), + + "last_q": np.zeros(12, dtype=np.float64), + "last_dq": np.zeros(12, dtype=np.float64), + "last_ff_applied": np.zeros(12, dtype=np.float64), + "last_tau_applied": np.zeros(12, dtype=np.float64), + "last_kp_used": np.zeros(12, dtype=np.float64), + "last_kd_used": np.zeros(12, dtype=np.float64), + "last_drive_force": np.zeros(12, dtype=np.float64), + + "tau_smooth": np.zeros(12, dtype=np.float64), + "tau_smooth_initialized": False, + "drives_disabled": False, + + "update_count": 0, + "last_print_time": 0.0, + "callback_mode": "unknown", + "physics_forced": False, +}) + +bridge = getattr(builtins, BRIDGE_NAME) + + +# ================================================== +# PHYSICS RATE +# ================================================== + +def find_or_create_physics_scene(stage): + for p in ["/World/physicsScene", "/World/PhysicsScene", + "/physicsScene", "/PhysicsScene"]: + prim = stage.GetPrimAtPath(p) + if prim.IsValid(): + return prim + for prim in stage.Traverse(): + try: + if prim.GetTypeName() == "PhysicsScene": + return prim + except Exception: + pass + scene = UsdPhysics.Scene.Define(stage, Sdf.Path("/World/physicsScene")) + print("[GO1 BRIDGE] Created PhysicsScene at /World/physicsScene") + return scene.GetPrim() + + +def force_physics_rate(): + stage = omni.usd.get_context().get_stage() + if stage is None: + print("[GO1 BRIDGE] No USD stage; cannot set physics rate yet.") + return False + try: + scene_prim = find_or_create_physics_scene(stage) + scene = UsdPhysics.Scene(scene_prim) + try: + scene.CreateTimeStepsPerSecondAttr().Set(float(PHYSICS_HZ)) + print(f"[GO1 BRIDGE] UsdPhysics timeStepsPerSecond = {PHYSICS_HZ}") + except Exception as e: + print("[GO1 BRIDGE] Could not set timeStepsPerSecond:", e) + try: + scene.CreateGravityDirectionAttr().Set((0.0, 0.0, -1.0)) + scene.CreateGravityMagnitudeAttr().Set(9.81) + except Exception: + pass + if PhysxSchema is not None: + try: + physx_api = PhysxSchema.PhysxSceneAPI.Apply(scene_prim) + if hasattr(physx_api, "CreateTimeStepsPerSecondAttr"): + physx_api.CreateTimeStepsPerSecondAttr().Set(float(PHYSICS_HZ)) + if hasattr(physx_api, "CreateEnableCCDAttr"): + physx_api.CreateEnableCCDAttr().Set(True) + print("[GO1 BRIDGE] PhysxSceneAPI settings applied.") + except Exception as e: + print("[GO1 BRIDGE] PhysxSceneAPI error:", e) + if carb is not None: + try: + settings = carb.settings.get_settings() + settings.set("/physics/timeStepsPerSecond", float(PHYSICS_HZ)) + settings.set("/physics/minFrameRate", float(PHYSICS_HZ)) + print(f"[GO1 BRIDGE] carb physics settings = {PHYSICS_HZ} Hz") + except Exception as e: + print("[GO1 BRIDGE] carb settings error:", e) + bridge["physics_forced"] = True + return True + except Exception as e: + print("[GO1 BRIDGE] force_physics_rate failed:", e) + return False + + +# ================================================== +# ROS 2 SETUP +# ================================================== + +if not rclpy.ok(): + rclpy.init(args=None) + +node = rclpy.create_node("go1_isaac_bridge_v9_gazebo_style_stable_torque") + +imu_pub = node.create_publisher(Imu, IMU_TOPIC, 10) +contact_pub = node.create_publisher(Float64MultiArray, CONTACT_TOPIC, 10) +odom_pub = node.create_publisher(Odometry, ODOM_TOPIC, 10) +joint_state_pub = node.create_publisher(JointState, JOINT_STATE_TOPIC, 10) + +bridge["node"] = node +bridge["imu_pub"] = imu_pub +bridge["contact_pub"] = contact_pub +bridge["odom_pub"] = odom_pub +bridge["joint_state_pub"] = joint_state_pub + + +# ================================================== +# QUATERNION HELPERS +# ================================================== + +def quat_normalize(q): + q = np.array(q, dtype=np.float64) + n = np.linalg.norm(q) + return q / n if n > 1e-12 else np.array([1., 0., 0., 0.]) + + +def quat_conjugate(q): + q = np.array(q, dtype=np.float64) + return np.array([q[0], -q[1], -q[2], -q[3]]) + + +def quat_multiply(q1, q2): + w1, x1, y1, z1 = q1 + w2, x2, y2, z2 = q2 + return np.array([ + w1*w2 - x1*x2 - y1*y2 - z1*z2, + w1*x2 + x1*w2 + y1*z2 - z1*y2, + w1*y2 - x1*z2 + y1*w2 + z1*x2, + w1*z2 + x1*y2 - y1*x2 + z1*w2, + ], dtype=np.float64) + + +def quat_rotate(q, v): + q = quat_normalize(q) + vq = np.array([0., v[0], v[1], v[2]]) + return quat_multiply(quat_multiply(q, vq), quat_conjugate(q))[1:] + + +def world_to_body(q_world_from_body, v_world): + return quat_rotate(quat_conjugate(quat_normalize(q_world_from_body)), v_world) + + +def yaw_from_quat_wxyz(q): + """ + Extract world yaw from quaternion [w, x, y, z]. + """ + q = quat_normalize(q) + w, x, y, z = q + siny = 2.0 * (w * z + x * y) + cosy = 1.0 - 2.0 * (y * y + z * z) + return math.atan2(siny, cosy) + + +def wrap_pi(a): + while a > math.pi: + a -= 2.0 * math.pi + while a < -math.pi: + a += 2.0 * math.pi + return a + + +# ================================================== +# USD HELPERS +# ================================================== + +def get_world_pose_from_cache(cache, stage, prim_path): + prim = stage.GetPrimAtPath(prim_path) + if not prim.IsValid(): + raise RuntimeError(f"Prim not found: {prim_path}") + mat = cache.GetLocalToWorldTransform(prim) + trans = mat.ExtractTranslation() + pos = np.array([trans[0], trans[1], trans[2]], dtype=np.float64) + try: + q_gf = mat.ExtractRotationQuat() + except Exception: + q_gf = mat.ExtractRotation().GetQuat() + w = q_gf.GetReal() + imag = q_gf.GetImaginary() + return pos, quat_normalize([w, imag[0], imag[1], imag[2]]) + + +def get_sim_time(): + try: + return float(omni.timeline.get_timeline_interface().get_current_time()) + except Exception: + return time.perf_counter() + + +def rate_limit_array(target, previous, max_step): + target = np.array(target, dtype=np.float64) + previous = np.array(previous, dtype=np.float64) + return np.clip(target, previous - max_step, previous + max_step) + + +# ================================================== +# SPAWN POSE RESET ← KEY FIX +# ================================================== + +def reset_robot_spawn_pose(stage): + """ + Move the entire GO1 robot to (SPAWN_X, SPAWN_Y, SPAWN_Z) with zero yaw. + + The MPC initializes its reference trajectory from the very first + observation. If the robot spawns with any yaw offset (e.g. +9.2° as + seen in the diagnostics), that offset becomes the MPC's idea of + "forward" and the robot walks obliquely in the world frame for the + entire run. + + We reset at the robot root prim (ROBOT_ROOT_PRIM_PATH = /World/go1) + so that all child links move together. Resetting only /World/go1/trunk + would leave the visual/collision meshes behind. + """ + if Gf is None: + print("[GO1 BRIDGE] pxr.Gf not available; cannot reset spawn pose.") + return False + + # Try robot root first, fall back to trunk prim. + for prim_path in [ROBOT_ROOT_PRIM_PATH, BASE_PRIM_PATH]: + prim = stage.GetPrimAtPath(prim_path) + if prim.IsValid(): + try: + xformable = UsdGeom.Xformable(prim) + + # Clear existing xform ops so we start clean. + xformable.ClearXformOpOrder() + + # Translation — preserve XY spawn position, set Z to stand height. + t_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + t_op.Set(Gf.Vec3d(float(SPAWN_X), float(SPAWN_Y), float(SPAWN_Z))) + + # Rotation — identity quaternion = zero yaw/pitch/roll. + r_op = xformable.AddOrientOp(UsdGeom.XformOp.PrecisionDouble) + r_op.Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) + + print(f"[GO1 BRIDGE] Spawn pose reset: prim={prim_path} " + f"pos=({SPAWN_X}, {SPAWN_Y}, {SPAWN_Z}) yaw=0") + return True + except Exception as e: + print(f"[GO1 BRIDGE] Could not reset spawn pose at {prim_path}: {e}") + + print("[GO1 BRIDGE] WARNING: Could not find any prim to reset spawn pose.") + return False + + +# ================================================== +# COMMAND CALLBACK +# ================================================== + +def cmd_callback(msg): + data = list(msg.data) + if len(data) != 60: + print(f"[GO1 BRIDGE] Ignored joint_cmd: expected 60 values, got {len(data)}") + return + + arr = np.array(data, dtype=np.float64).reshape((12, 5)) + q_des = arr[:, 0].copy() + dq_des = arr[:, 1].copy() + kp = np.clip(arr[:, 2], 0.0, MAX_ABS_KP) + kd = np.clip(arr[:, 3], 0.0, MAX_ABS_KD) + ff = np.clip(arr[:, 4], -MAX_ABS_FF, MAX_ABS_FF) + + active = bool( + np.any(np.abs(kp) > 1e-6) or + np.any(np.abs(kd) > 1e-6) or + np.any(np.abs(ff) > 1e-6) + ) + + if active and REAR_SUPPORT_GAIN_BOOST: + kp[4] *= REAR_HFE_KP_BOOST; kp[5] *= REAR_KFE_KP_BOOST + kp[10] *= REAR_HFE_KP_BOOST; kp[11] *= REAR_KFE_KP_BOOST + kd[4] *= REAR_HFE_KD_BOOST; kd[5] *= REAR_KFE_KD_BOOST + kd[10] *= REAR_HFE_KD_BOOST; kd[11] *= REAR_KFE_KD_BOOST + kp = np.clip(kp, 0.0, MAX_ABS_KP) + kd = np.clip(kd, 0.0, MAX_ABS_KD) + + bridge["latest_cmd"] = { + "q_des": q_des, "dq_des": dq_des, + "kp": kp, "kd": kd, "ff": ff, "active": active, + } + bridge["command_received"] = True + bridge["last_cmd_wall_time"] = time.perf_counter() + + +cmd_sub = node.create_subscription(Float64MultiArray, CMD_TOPIC, cmd_callback, 10) +bridge["cmd_sub"] = cmd_sub + + +# ================================================== +# INITIALIZE ARTICULATION +# ================================================== + +def initialize_robot_if_needed(): + if bridge["initialized"]: + return True + + stage = omni.usd.get_context().get_stage() + root_prim = stage.GetPrimAtPath(ARTICULATION_ROOT) + if not root_prim.IsValid(): + print(f"[GO1 BRIDGE] Articulation root not found: {ARTICULATION_ROOT}") + return False + + try: + force_physics_rate() + + # ── CRITICAL: reset spawn pose BEFORE initializing articulation ── + # This ensures the first observation the MPC receives has zero yaw + # so the controller's world frame matches Isaac's world frame. + reset_robot_spawn_pose(stage) + + robot = SingleArticulation( + prim_path=ARTICULATION_ROOT, + name="go1_bridge_v9_robot", + ) + robot.initialize() + + dof_names = list(robot.dof_names) + print("[GO1 BRIDGE] Isaac DOF names:") + for i, n in enumerate(dof_names): + print(f" {i}: {n}") + + joint_indices = [] + joint_prims = [] + + for ros_i in range(12): + isaac_name = ROS_ORDER_TO_ISAAC_NAME[ros_i] + if isaac_name not in dof_names: + raise RuntimeError(f"Joint '{isaac_name}' not found in Isaac DOFs") + joint_indices.append(dof_names.index(isaac_name)) + + found_prim = None + for prim in stage.Traverse(): + if prim.GetName() == isaac_name: + found_prim = prim + break + if found_prim is None: + raise RuntimeError(f"Could not find joint prim for '{isaac_name}'") + joint_prims.append(found_prim) + + bridge["robot"] = robot + bridge["joint_indices"] = np.array(joint_indices, dtype=np.int32) + bridge["dof_names"] = dof_names + bridge["joint_prims"] = joint_prims + + if SET_REST_POSE_ON_INIT: + q_all = np.array(robot.get_joint_positions(), dtype=np.float64) + dq_all = np.array(robot.get_joint_velocities(), dtype=np.float64) + for ros_i, isaac_i in enumerate(joint_indices): + q_all[isaac_i] = GAZEBO_REST_Q_ROS_ORDER[ros_i] + dq_all[isaac_i] = 0.0 + robot.set_joint_positions(q_all) + robot.set_joint_velocities(dq_all) + try: + robot.set_linear_velocity(np.zeros(3)) + robot.set_angular_velocity(np.zeros(3)) + except Exception: + pass + print("[GO1 BRIDGE] Initial rest pose set.") + + bridge["initialized"] = True + print("[GO1 BRIDGE] Robot initialized. DOF index map:", joint_indices) + print("[GO1 BRIDGE] Callback mode:", bridge["callback_mode"]) + print("[GO1 BRIDGE] Physics forced:", bridge["physics_forced"]) + return True + + except Exception as e: + print("[GO1 BRIDGE] Initialization failed:", e) + return False + + +# ================================================== +# READ JOINT STATE +# ================================================== + +def read_joint_state_ros_order(): + robot = bridge["robot"] + joint_indices = bridge["joint_indices"] + q_all = np.array(robot.get_joint_positions(), dtype=np.float64) + dq_all = np.array(robot.get_joint_velocities(), dtype=np.float64) + q = np.zeros(12, dtype=np.float64) + dq = np.zeros(12, dtype=np.float64) + for ros_i in range(12): + isaac_i = joint_indices[ros_i] + q[ros_i] = q_all[isaac_i] + dq[ros_i] = dq_all[isaac_i] + bridge["last_q"] = q.copy() + bridge["last_dq"] = dq.copy() + return q, dq + + +# ================================================== +# COMMAND SMOOTHING +# ================================================== + +def get_smoothed_command(cmd): + q_des = cmd["q_des"]; dq_des = cmd["dq_des"] + kp = cmd["kp"]; kd = cmd["kd"]; ff = cmd["ff"] + active = cmd["active"] + + if not active: + bridge["smooth_initialized"] = False + return q_des, dq_des, kp, kd, np.zeros(12, dtype=np.float64) + + if not bridge["smooth_initialized"]: + bridge["q_des_smooth"] = q_des.copy() + bridge["dq_des_smooth"] = dq_des.copy() + bridge["ff_smooth"] = ff.copy() + bridge["smooth_initialized"] = True + else: + q_t = Q_DES_ALPHA * q_des + (1 - Q_DES_ALPHA) * bridge["q_des_smooth"] + dq_t = DQ_DES_ALPHA * dq_des + (1 - DQ_DES_ALPHA) * bridge["dq_des_smooth"] + ff_t = FF_ALPHA * ff + (1 - FF_ALPHA) * bridge["ff_smooth"] + bridge["q_des_smooth"] = rate_limit_array(q_t, bridge["q_des_smooth"], MAX_Q_STEP) + bridge["dq_des_smooth"] = rate_limit_array(dq_t, bridge["dq_des_smooth"], MAX_DQ_STEP) + bridge["ff_smooth"] = rate_limit_array(ff_t, bridge["ff_smooth"], MAX_FF_STEP) + + return (bridge["q_des_smooth"].copy(), bridge["dq_des_smooth"].copy(), + kp.copy(), kd.copy(), bridge["ff_smooth"].copy()) + + +# ================================================== +# DISABLE USD DRIVES +# ================================================== + +def disable_usd_drives(): + """Zero out all USD DriveAPI gains so Isaac uses pure torque from ArticulationAction.""" + joint_prims = bridge["joint_prims"] + if joint_prims is None: + return + for ros_i in range(12): + prim = joint_prims[ros_i] + drive_api = UsdPhysics.DriveAPI.Apply(prim, "angular") + drive_api.CreateStiffnessAttr(0.0) + drive_api.CreateDampingAttr(0.0) + drive_api.CreateMaxForceAttr(0.0) + drive_api.CreateTargetPositionAttr(0.0) + drive_api.CreateTargetVelocityAttr(0.0) + bridge["last_drive_force"] = np.zeros(12, dtype=np.float64) + bridge["drives_disabled"] = True + + +# ================================================== +# TORQUE COMPUTATION +# ================================================== + +def command_is_timed_out(): + last = bridge.get("last_cmd_wall_time") + return last is None or (time.perf_counter() - last) > COMMAND_TIMEOUT_SEC + + +def compute_hybrid_torque(q_des, dq_des, kp, kd, ff, q_now, dq_now, active): + if not active: + bridge["tau_smooth_initialized"] = False + return np.zeros(12, dtype=np.float64) + + tau = kp * (q_des - q_now) + kd * (dq_des - dq_now) + ff + + if REAR_SUPPORT_GAIN_BOOST: + for idx in REAR_SUPPORT_JOINTS: + tau[idx] *= 1.08 + + tau = np.clip(tau, -MAX_JOINT_TORQUE, MAX_JOINT_TORQUE) + + if not bridge["tau_smooth_initialized"]: + bridge["tau_smooth"] = tau.copy() + bridge["tau_smooth_initialized"] = True + else: + tau_f = TAU_ALPHA * tau + (1.0 - TAU_ALPHA) * bridge["tau_smooth"] + tau_f = rate_limit_array(tau_f, bridge["tau_smooth"], MAX_TAU_STEP) + bridge["tau_smooth"] = tau_f.copy() + tau = tau_f + + return tau + + +def apply_latest_command(): + cmd = bridge["latest_cmd"] + if cmd is None: + return + if not initialize_robot_if_needed(): + return + + try: + q_now, dq_now = read_joint_state_ros_order() + + if not bridge.get("drives_disabled", False): + disable_usd_drives() + + zero = np.zeros(12, dtype=np.float64) + + if command_is_timed_out(): + bridge["tau_smooth_initialized"] = False + bridge["robot"].apply_action(ArticulationAction( + joint_efforts=zero, joint_indices=bridge["joint_indices"])) + bridge["last_ff_applied"] = zero.copy() + bridge["last_tau_applied"] = zero.copy() + bridge["last_kp_used"] = zero.copy() + bridge["last_kd_used"] = zero.copy() + return + + q_des, dq_des, kp, kd, ff = get_smoothed_command(cmd) + active = bool(cmd["active"]) + + tau = compute_hybrid_torque(q_des, dq_des, kp, kd, ff, q_now, dq_now, active) + + bridge["robot"].apply_action(ArticulationAction( + joint_efforts=tau, joint_indices=bridge["joint_indices"])) + + bridge["last_ff_applied"] = ff.copy() + bridge["last_tau_applied"] = tau.copy() + bridge["last_kp_used"] = kp.copy() + bridge["last_kd_used"] = kd.copy() + + except Exception as e: + print("[GO1 BRIDGE] apply_action failed:", e) + + +# ================================================== +# PUBLISH JOINT STATES +# ================================================== + +def publish_joint_states(): + if not bridge["initialized"]: + return + try: + q, dq = read_joint_state_ros_order() + except Exception as e: + print("[GO1 BRIDGE] Cannot read joint states:", e) + return + msg = JointState() + msg.header.stamp = node.get_clock().now().to_msg() + msg.name = [ROS_ORDER_TO_ISAAC_NAME[i] for i in range(12)] + msg.position = [float(x) for x in q] + msg.velocity = [float(x) for x in dq] + msg.effort = [float(x) for x in bridge["last_tau_applied"]] + joint_state_pub.publish(msg) + + +# ================================================== +# BASE VELOCITY HELPERS +# ================================================== + +def _as_vec3(value): + try: + arr = np.array(value, dtype=np.float64).reshape(-1) + if arr.size >= 3 and np.all(np.isfinite(arr[:3])): + return arr[:3].copy() + except Exception: + pass + return None + + +def get_robot_base_velocities_world(robot): + lin = None + ang = None + for m in ["get_linear_velocity", "get_world_linear_velocity", "get_default_linear_velocity"]: + try: + fn = getattr(robot, m, None) + if callable(fn): + lin = _as_vec3(fn()) + if lin is not None: + break + except Exception: + pass + for m in ["get_angular_velocity", "get_world_angular_velocity", "get_default_angular_velocity"]: + try: + fn = getattr(robot, m, None) + if callable(fn): + ang = _as_vec3(fn()) + if ang is not None: + break + except Exception: + pass + if lin is None or ang is None: + for m in ["get_velocities", "get_world_velocities"]: + try: + fn = getattr(robot, m, None) + if callable(fn): + v = np.array(fn(), dtype=np.float64).reshape(-1) + if v.size >= 6 and np.all(np.isfinite(v[:6])): + if lin is None: lin = v[:3].copy() + if ang is None: ang = v[3:6].copy() + break + except Exception: + pass + return lin, ang + + +# ================================================== +# PUBLISH IMU + ODOM +# ================================================== + +def publish_imu_and_odom(stage, cache, now): + try: + pos_world, q_world_from_body = get_world_pose_from_cache( + cache, stage, BASE_PRIM_PATH) + except Exception as e: + print("[GO1 BRIDGE] Cannot read base pose:", e) + return + + last_time = bridge["last_time"] + + if last_time is None: + bridge["last_time"] = now + bridge["last_pos"] = pos_world + bridge["last_vel"] = np.zeros(3, dtype=np.float64) + bridge["last_quat"] = q_world_from_body + return + + dt = now - last_time + if dt <= 1e-6 or dt > 1.0: + dt = PHYSICS_DT * IMU_ODOM_EVERY_N + + # ── Linear velocity ────────────────────────────────────────────────── + # Prefer direct Isaac API (world frame). Fall back to finite-diff only + # if the API returns non-finite values. Near-zero is a valid velocity + # during stance — do NOT threshold on magnitude. + vel_world_fd = (pos_world - bridge["last_pos"]) / dt + + robot = bridge.get("robot") + vel_world_direct = None + omega_world_direct = None + if robot is not None: + vel_world_direct, omega_world_direct = get_robot_base_velocities_world(robot) + + if vel_world_direct is not None and np.all(np.isfinite(vel_world_direct)): + vel_world = vel_world_direct + else: + vel_world = vel_world_fd + + # ── Angular velocity ───────────────────────────────────────────────── + # CRITICAL: Do NOT use quaternion finite-difference as fallback. + # During trot, roll/pitch oscillations alias into a fake world-frame + # yaw rate that creates gait-locked oscillatory yaw drift in the + # Kalman filter. Publish zero instead if the direct API is unavailable. + if omega_world_direct is not None and np.all(np.isfinite(omega_world_direct)): + omega_world = omega_world_direct.copy() + else: + omega_world = np.zeros(3, dtype=np.float64) + + # FIX A: deadband tiny numerical angular velocity at rest. + # This prevents small PhysX/API noise from being integrated as yaw. + if np.linalg.norm(omega_world) < OMEGA_DEADBAND: + omega_world = np.zeros(3, dtype=np.float64) + + # FIX D, most important: replace world yaw rate with quaternion-derived + # yaw finite difference only on Z, then low-pass it. This avoids using + # the articulation angular-velocity Z component that can contain + # foot-strike vibration and solver noise. + yaw_now = yaw_from_quat_wxyz(q_world_from_body) + yaw_prev = bridge.get("yaw_prev", None) + + if yaw_prev is None: + yaw_rate_raw = 0.0 + bridge["yaw_rate_smooth"] = 0.0 + else: + dyaw = wrap_pi(yaw_now - yaw_prev) + yaw_rate_raw = dyaw / dt + if abs(yaw_rate_raw) < OMEGA_DEADBAND: + yaw_rate_raw = 0.0 + bridge["yaw_rate_smooth"] = ( + YAW_RATE_ALPHA * yaw_rate_raw + + (1.0 - YAW_RATE_ALPHA) * bridge.get("yaw_rate_smooth", 0.0) + ) + + bridge["yaw_prev"] = yaw_now + + # Keep roll/pitch angular velocity from direct API; use clean yaw-rate. + omega_world[2] = bridge["yaw_rate_smooth"] + + # ── IMU acceleration ───────────────────────────────────────────────── + # Finite-diff of velocity amplifies foot-strike spikes. Smooth the + # acceleration for the IMU only; odom velocity stays unsmoothed. + acc_raw = (vel_world - bridge["last_vel"]) / dt + if not bridge["acc_initialized"]: + bridge["acc_world_smooth"] = acc_raw.copy() + bridge["acc_initialized"] = True + else: + bridge["acc_world_smooth"] = ( + ACC_ALPHA * acc_raw + (1.0 - ACC_ALPHA) * bridge["acc_world_smooth"]) + acc_world = bridge["acc_world_smooth"] + + # Specific force and body-frame quantities for IMU message + gravity_world = np.array([0., 0., -9.81]) + specific_force_world = acc_world - gravity_world + specific_force_body = world_to_body(q_world_from_body, specific_force_world) + omega_body = world_to_body(q_world_from_body, omega_world) + + # Apply the same deadband after rotating to IMU/body frame. + if np.linalg.norm(omega_body) < OMEGA_DEADBAND: + omega_body = np.zeros(3, dtype=np.float64) + + stamp = node.get_clock().now().to_msg() + + # ── IMU message ─────────────────────────────────────────────────────── + # orientation : world frame (quaternion) + # angular_velocity : BODY frame (physical gyro convention) + # linear_acceleration : BODY frame specific force (includes gravity removal) + imu_msg = Imu() + imu_msg.header.stamp = stamp + imu_msg.header.frame_id = "base_imu" + + imu_msg.orientation.w = float(q_world_from_body[0]) + imu_msg.orientation.x = float(q_world_from_body[1]) + imu_msg.orientation.y = float(q_world_from_body[2]) + imu_msg.orientation.z = float(q_world_from_body[3]) + + imu_msg.angular_velocity.x = float(omega_body[0]) # body frame + imu_msg.angular_velocity.y = float(omega_body[1]) + imu_msg.angular_velocity.z = float(omega_body[2]) + + imu_msg.linear_acceleration.x = float(specific_force_body[0]) # body frame + imu_msg.linear_acceleration.y = float(specific_force_body[1]) + imu_msg.linear_acceleration.z = float(specific_force_body[2]) + + imu_msg.orientation_covariance[0] = 0.0012 + imu_msg.orientation_covariance[4] = 0.0012 + imu_msg.orientation_covariance[8] = 0.0012 + # FIX B: do not over-trust simulated gyro yaw rate. + imu_msg.angular_velocity_covariance[0] = 0.002 + imu_msg.angular_velocity_covariance[4] = 0.002 + imu_msg.angular_velocity_covariance[8] = 0.002 + imu_msg.linear_acceleration_covariance[0] = 0.01 + imu_msg.linear_acceleration_covariance[4] = 0.01 + imu_msg.linear_acceleration_covariance[8] = 0.01 + + imu_pub.publish(imu_msg) + + # ── Odometry message ────────────────────────────────────────────────── + # legged_control rbd_state_ convention (confirmed from issue #10): + # linear_vel_x/y/z_global → twist.linear in WORLD frame + # angular_vel_x/y/z_global → twist.angular in WORLD frame + odom_msg = Odometry() + odom_msg.header.stamp = stamp + odom_msg.header.frame_id = "world" + odom_msg.child_frame_id = "base" + + odom_msg.pose.pose.position.x = float(pos_world[0]) + odom_msg.pose.pose.position.y = float(pos_world[1]) + odom_msg.pose.pose.position.z = float(pos_world[2]) + + odom_msg.pose.pose.orientation.w = float(q_world_from_body[0]) + odom_msg.pose.pose.orientation.x = float(q_world_from_body[1]) + odom_msg.pose.pose.orientation.y = float(q_world_from_body[2]) + odom_msg.pose.pose.orientation.z = float(q_world_from_body[3]) + + odom_msg.twist.twist.linear.x = float(vel_world[0]) # world frame + odom_msg.twist.twist.linear.y = float(vel_world[1]) + odom_msg.twist.twist.linear.z = float(vel_world[2]) + + odom_msg.twist.twist.angular.x = float(omega_world[0]) # world frame + odom_msg.twist.twist.angular.y = float(omega_world[1]) + odom_msg.twist.twist.angular.z = float(omega_world[2]) + + odom_msg.pose.covariance[0] = 0.00055 + odom_msg.pose.covariance[7] = 0.00055 + odom_msg.pose.covariance[14] = 0.000053 + odom_msg.pose.covariance[21] = 0.0012 + odom_msg.pose.covariance[28] = 0.0012 + odom_msg.pose.covariance[35] = 0.0012 + + odom_msg.twist.covariance[0] = 0.00049 + odom_msg.twist.covariance[7] = 0.00049 + odom_msg.twist.covariance[14] = 0.00049 + # FIX C: do not over-trust odom yaw-rate twist. + odom_msg.twist.covariance[21] = 0.002 + odom_msg.twist.covariance[28] = 0.002 + odom_msg.twist.covariance[35] = 0.002 + + odom_pub.publish(odom_msg) + + bridge["last_time"] = now + bridge["last_pos"] = pos_world + bridge["last_vel"] = vel_world + bridge["last_quat"] = q_world_from_body + + +# ================================================== +# PUBLISH CONTACTS (debug only) +# ================================================== + +def publish_contacts(stage, cache): + if USE_SCHEDULED_TROT_CONTACTS: + now = get_sim_time() + phase = (now + TROT_PHASE_OFFSET) % TROT_PERIOD + if phase < TROT_SWITCH_TIME: + contacts = np.array([1., 0., 0., 1.]) # mode 9: LF + RH + else: + contacts = np.array([0., 1., 1., 0.]) # mode 6: LH + RF + else: + contacts = bridge["contact_state"].copy() + for idx, leg in enumerate(["LF", "LH", "RF", "RH"]): + try: + foot_pos, _ = get_world_pose_from_cache(cache, stage, FOOT_PRIMS[leg]) + foot_z = foot_pos[2] + if foot_z <= (GROUND_Z + CONTACT_ON_Z): contacts[idx] = 1.0 + elif foot_z >= (GROUND_Z + CONTACT_OFF_Z): contacts[idx] = 0.0 + except Exception as e: + print(f"[GO1 BRIDGE] Cannot read foot {leg}: {e}") + + bridge["contact_state"] = contacts + msg = Float64MultiArray() + msg.data = [float(x) for x in contacts] + contact_pub.publish(msg) + + +# ================================================== +# MAIN STEP +# ================================================== + +def bridge_step(dt=None): + stage = omni.usd.get_context().get_stage() + cache = UsdGeom.XformCache() + now = get_sim_time() + + bridge["update_count"] += 1 + k = bridge["update_count"] + + try: + rclpy.spin_once(node, timeout_sec=0.0) + except Exception as e: + print("[GO1 BRIDGE] rclpy spin error:", e) + return + + if not initialize_robot_if_needed(): + return + + apply_latest_command() + + if k % JOINT_STATE_EVERY_N == 0: + publish_joint_states() + if k % IMU_ODOM_EVERY_N == 0: + publish_imu_and_odom(stage, cache, now) + if k % CONTACT_EVERY_N == 0: + publish_contacts(stage, cache) + + if now - bridge["last_print_time"] > 1.5: + bridge["last_print_time"] = now + has_cmd = bridge["latest_cmd"] is not None + timed_out = command_is_timed_out() + print("------------------------------------------------") + print("[GO1 BRIDGE] running | updates:", bridge["update_count"]) + print("callback_mode:", bridge["callback_mode"], + "| physics_forced:", bridge["physics_forced"]) + print("cmd:", has_cmd, "| timeout:", timed_out) + if has_cmd: + cmd = bridge["latest_cmd"] + print("active:", cmd["active"]) + print("kp :", np.round(bridge["last_kp_used"], 2)) + print("kd :", np.round(bridge["last_kd_used"], 2)) + print("ff :", np.round(bridge["last_ff_applied"], 2)) + print("tau :", np.round(bridge["last_tau_applied"], 2)) + print("q_now:", np.round(bridge["last_q"], 3)) + print("q_des:", np.round(bridge["q_des_smooth"], 3)) + print("contacts:", bridge["contact_state"]) + print("------------------------------------------------") + + +# ================================================== +# REGISTER PHYSICS CALLBACK +# ================================================== + +def register_callback(): + force_physics_rate() + + # 1. PhysX physics-step interface (preferred — runs at physics rate) + try: + if omni.physx is None: + raise RuntimeError("omni.physx unavailable") + physx_iface = omni.physx.get_physx_interface() + if hasattr(physx_iface, "subscribe_physics_step_events"): + sub = physx_iface.subscribe_physics_step_events(lambda dt: bridge_step(dt)) + bridge["physics_sub"] = sub + bridge["update_sub"] = sub + bridge["callback_mode"] = "physx_interface.subscribe_physics_step_events" + print("[GO1 BRIDGE] Registered PhysX physics-step callback.") + return + raise RuntimeError("physx_interface has no subscribe_physics_step_events") + except Exception as e: + print("[GO1 BRIDGE] PhysX interface callback failed:", e) + + # 2. PhysX simulation-step interface (fallback) + try: + if omni.physx is None: + raise RuntimeError("omni.physx unavailable") + sim_iface = omni.physx.get_physx_simulation_interface() + if hasattr(sim_iface, "subscribe_physics_step_events"): + sub = sim_iface.subscribe_physics_step_events(lambda dt: bridge_step(dt)) + bridge["physics_sub"] = sub + bridge["update_sub"] = sub + bridge["callback_mode"] = "physx_simulation_interface.subscribe_physics_step_events" + print("[GO1 BRIDGE] Registered PhysX simulation-step callback.") + return + raise RuntimeError("sim_interface has no subscribe_physics_step_events") + except Exception as e: + print("[GO1 BRIDGE] PhysX simulation callback failed:", e) + + # 3. App update loop (last resort — not physics-rate locked) + sub = omni.kit.app.get_app().get_update_event_stream().create_subscription_to_pop( + lambda event: bridge_step(None), + name="go1_isaac_bridge_v9_update", + ) + bridge["update_sub"] = sub + bridge["callback_mode"] = "app_update_fallback" + print("[GO1 BRIDGE] WARNING: app-update fallback — feedback rates may be low.") + + +register_callback() + + +# ================================================== +# STARTUP SUMMARY +# ================================================== +print("=" * 60) +print("GO1 ISAAC BRIDGE v9 — GAZEBO-STYLE STATE + STABLE TORQUE") +print("=" * 60) +print(f"Articulation root : {ARTICULATION_ROOT}") +print(f"Robot root prim : {ROBOT_ROOT_PRIM_PATH}") +print(f"Spawn pose : x={SPAWN_X} y={SPAWN_Y} z={SPAWN_Z} yaw=0") +print(f"Sub : {CMD_TOPIC}") +print(f"Pubs : {JOINT_STATE_TOPIC} {IMU_TOPIC} {ODOM_TOPIC} {CONTACT_TOPIC}") +print("NOTE : Real /isaac/contacts must come from ROS1 mode_to_contacts.py") +print("Odom twist frame : WORLD (legged_control rbd_state_ convention)") +print("IMU ang_vel frame: BODY (physical gyro convention)") +print("IMU lin_acc frame: BODY (specific force, gravity removed)") +print(f"Physics Hz : {PHYSICS_HZ}") +print(f"TAU_ALPHA : {TAU_ALPHA} MAX_TAU_STEP: {MAX_TAU_STEP}") +print(f"FF_ALPHA : {FF_ALPHA} ACC_ALPHA : {ACC_ALPHA}") +print(f"OMEGA_DEADBAND : {OMEGA_DEADBAND} rad/s") +print(f"YAW_RATE_ALPHA : {YAW_RATE_ALPHA}") +print(f"MAX_JOINT_TORQUE : {MAX_JOINT_TORQUE} Nm") +print(f"COMMAND_TIMEOUT : {COMMAND_TIMEOUT_SEC} s") +print(f"Trot contacts : period={TROT_PERIOD}s switch={TROT_SWITCH_TIME}s offset={TROT_PHASE_OFFSET}s") +print(f"Callback mode : {bridge['callback_mode']}") +print(f"Physics forced : {bridge['physics_forced']}") +print("=" * 60) diff --git a/isaac_sim/scripts/ros2_isaac_socket_bridge.py b/isaac_sim/scripts/ros2_isaac_socket_bridge.py new file mode 100755 index 00000000..f593a2f3 --- /dev/null +++ b/isaac_sim/scripts/ros2_isaac_socket_bridge.py @@ -0,0 +1,277 @@ +#!/usr/bin/env python3 + +import json +import os +import socket +import threading +import queue + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float64MultiArray +from sensor_msgs.msg import JointState, Imu +from nav_msgs.msg import Odometry +from rosgraph_msgs.msg import Clock + + +HOST = os.environ.get("ISAAC_SOCKET_BIND", "0.0.0.0") +PORT = int(os.environ.get("ISAAC_SOCKET_PORT", "50055")) + + +def stamp_to_dict(stamp): + return { + "sec": int(stamp.sec), + "nanosec": int(stamp.nanosec), + } + + +def header_to_dict(header): + return { + "stamp": stamp_to_dict(header.stamp), + "frame_id": header.frame_id, + } + + +def vec3_to_dict(v): + return { + "x": float(v.x), + "y": float(v.y), + "z": float(v.z), + } + + +def quat_to_dict(q): + return { + "x": float(q.x), + "y": float(q.y), + "z": float(q.z), + "w": float(q.w), + } + + +class Ros2IsaacSocketBridge(Node): + def __init__(self): + super().__init__("ros2_isaac_socket_bridge") + + self.out_queue = queue.Queue(maxsize=2000) + self.conn = None + self.conn_lock = threading.Lock() + + self.joint_cmd_pub = self.create_publisher( + Float64MultiArray, + "/isaac/joint_cmd", + 10, + ) + + self.create_subscription( + JointState, + "/isaac/joint_states", + self.joint_state_cb, + 10, + ) + + self.create_subscription( + Imu, + "/isaac/imu", + self.imu_cb, + 10, + ) + + self.create_subscription( + Odometry, + "/isaac/odom", + self.odom_cb, + 10, + ) + + self.create_subscription( + Float64MultiArray, + "/isaac/contacts", + self.contacts_cb, + 10, + ) + + self.create_subscription( + Clock, + "/clock", + self.clock_cb, + 10, + ) + + threading.Thread(target=self.server_loop, daemon=True).start() + + self.get_logger().info( + f"ROS 2 Isaac socket bridge listening on {HOST}:{PORT}" + ) + + def has_client(self): + with self.conn_lock: + return self.conn is not None + + def enqueue(self, packet): + if not self.has_client(): + return + + try: + self.out_queue.put_nowait(packet) + except queue.Full: + try: + self.out_queue.get_nowait() + except queue.Empty: + pass + try: + self.out_queue.put_nowait(packet) + except queue.Full: + pass + + def joint_state_cb(self, msg): + packet = { + "topic": "/isaac/joint_states", + "type": "sensor_msgs/JointState", + "header": header_to_dict(msg.header), + "name": list(msg.name), + "position": [float(x) for x in msg.position], + "velocity": [float(x) for x in msg.velocity], + "effort": [float(x) for x in msg.effort], + } + self.enqueue(packet) + + def imu_cb(self, msg): + packet = { + "topic": "/isaac/imu", + "type": "sensor_msgs/Imu", + "header": header_to_dict(msg.header), + "orientation": quat_to_dict(msg.orientation), + "angular_velocity": vec3_to_dict(msg.angular_velocity), + "linear_acceleration": vec3_to_dict(msg.linear_acceleration), + "orientation_covariance": [float(x) for x in msg.orientation_covariance], + "angular_velocity_covariance": [float(x) for x in msg.angular_velocity_covariance], + "linear_acceleration_covariance": [float(x) for x in msg.linear_acceleration_covariance], + } + self.enqueue(packet) + + def odom_cb(self, msg): + packet = { + "topic": "/isaac/odom", + "type": "nav_msgs/Odometry", + "header": header_to_dict(msg.header), + "child_frame_id": msg.child_frame_id, + "position": vec3_to_dict(msg.pose.pose.position), + "orientation": quat_to_dict(msg.pose.pose.orientation), + "linear": vec3_to_dict(msg.twist.twist.linear), + "angular": vec3_to_dict(msg.twist.twist.angular), + "pose_covariance": [float(x) for x in msg.pose.covariance], + "twist_covariance": [float(x) for x in msg.twist.covariance], + } + self.enqueue(packet) + + def contacts_cb(self, msg): + packet = { + "topic": "/isaac/contacts", + "type": "std_msgs/Float64MultiArray", + "data": [float(x) for x in msg.data], + } + self.enqueue(packet) + + def clock_cb(self, msg): + packet = { + "topic": "/clock", + "type": "rosgraph_msgs/Clock", + "clock": stamp_to_dict(msg.clock), + } + self.enqueue(packet) + + def publish_joint_cmd(self, data): + msg = Float64MultiArray() + msg.data = [float(x) for x in data] + self.joint_cmd_pub.publish(msg) + + def server_loop(self): + srv = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + srv.bind((HOST, PORT)) + srv.listen(1) + + self.get_logger().info(f"Waiting for ROS 1 Docker client on port {PORT}") + + while rclpy.ok(): + conn, addr = srv.accept() + + with self.conn_lock: + self.conn = conn + + self.get_logger().info(f"ROS 1 client connected from {addr}") + + send_thread = threading.Thread( + target=self.send_loop, + args=(conn,), + daemon=True, + ) + recv_thread = threading.Thread( + target=self.recv_loop, + args=(conn,), + daemon=True, + ) + + send_thread.start() + recv_thread.start() + + recv_thread.join() + + with self.conn_lock: + if self.conn is conn: + self.conn = None + + try: + conn.close() + except Exception: + pass + + self.get_logger().warn("ROS 1 client disconnected; waiting again") + + def send_loop(self, conn): + while rclpy.ok(): + try: + packet = self.out_queue.get(timeout=0.2) + line = json.dumps(packet) + "\n" + conn.sendall(line.encode("utf-8")) + except queue.Empty: + continue + except Exception: + break + + def recv_loop(self, conn): + file_obj = conn.makefile("r") + + while rclpy.ok(): + line = file_obj.readline() + + if not line: + break + + try: + packet = json.loads(line) + except Exception: + continue + + if packet.get("topic") == "/isaac/joint_cmd": + data = packet.get("data", []) + self.publish_joint_cmd(data) + + +def main(): + rclpy.init() + node = Ros2IsaacSocketBridge() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/isaac_sim/start_isaac_legged_stack.sh b/isaac_sim/start_isaac_legged_stack.sh new file mode 100755 index 00000000..b853fd48 --- /dev/null +++ b/isaac_sim/start_isaac_legged_stack.sh @@ -0,0 +1,137 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Portable, non-GUI equivalent of the development start_isaac_legged_stack.sh. +# The repository must already be built in the container's catkin workspace. +SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd -- "${SCRIPT_DIR}/.." && pwd)" + +CONTAINER_NAME="${CONTAINER_NAME:-ros1_noetic_legged}" +ROS1_WS="${ROS1_WS:-/root/legged_ws}" +ROS2_SOCKET_HOST="${ROS2_SOCKET_HOST:-172.17.0.1}" +ROS2_SOCKET_PORT="${ROS2_SOCKET_PORT:-50055}" +ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-0}" +LOG_DIR="${ISAAC_STACK_LOG_DIR:-/tmp/isaac_legged_stack}" +HOST_BRIDGE="${REPO_ROOT}/isaac_sim/scripts/ros2_isaac_socket_bridge.py" + +mkdir -p "${LOG_DIR}" + +container_ros() { + docker exec "${CONTAINER_NAME}" bash -lc \ + "source /opt/ros/noetic/setup.bash && cd '${ROS1_WS}' && source devel/setup.bash && $*" +} + +container_ros_detached() { + docker exec -d "${CONTAINER_NAME}" bash -lc \ + "source /opt/ros/noetic/setup.bash && cd '${ROS1_WS}' && source devel/setup.bash && $*" +} + +wait_for_container_ros() { + local description="$1" + local check="$2" + local attempts="${3:-60}" + printf 'Waiting for %s' "${description}" + for ((i = 0; i < attempts; i++)); do + if container_ros "${check}" >/dev/null 2>&1; then + printf ' ready.\n' + return 0 + fi + printf '.' + sleep 1 + done + printf '\nTimed out waiting for %s.\n' "${description}" >&2 + return 1 +} + +command -v docker >/dev/null 2>&1 || { + echo "docker is required." >&2 + exit 1 +} +[[ -f "${HOST_BRIDGE}" ]] || { + echo "Missing host bridge: ${HOST_BRIDGE}" >&2 + exit 1 +} + +echo "Starting container ${CONTAINER_NAME}..." +docker start "${CONTAINER_NAME}" >/dev/null +docker exec "${CONTAINER_NAME}" mkdir -p "${LOG_DIR}" + +existing_bridge_pid="" +if [[ -f "${LOG_DIR}/ros2_bridge.pid" ]] && \ + kill -0 "$(<"${LOG_DIR}/ros2_bridge.pid")" 2>/dev/null; then + existing_bridge_pid="$(<"${LOG_DIR}/ros2_bridge.pid")" +else + existing_bridge_pid="$(pgrep -f '[r]os2_isaac_socket_bridge.py' | head -n 1 || true)" +fi + +if [[ -n "${existing_bridge_pid}" ]]; then + echo "ROS 2 socket bridge is already running." + echo "${existing_bridge_pid}" >"${LOG_DIR}/ros2_bridge.pid" +else + echo "Starting ROS 2 socket bridge..." + ( + unset ROS_VERSION ROS_PYTHON_VERSION ROS_PACKAGE_PATH AMENT_PREFIX_PATH + unset CMAKE_PREFIX_PATH COLCON_PREFIX_PATH PYTHONPATH + set +u + source /opt/ros/humble/setup.bash + set -u + export ROS_DOMAIN_ID ROS_LOCALHOST_ONLY=0 + export ISAAC_SOCKET_PORT="${ROS2_SOCKET_PORT}" + exec python3 -u "${HOST_BRIDGE}" + ) >"${LOG_DIR}/ros2_bridge.log" 2>&1 & + echo "$!" >"${LOG_DIR}/ros2_bridge.pid" + sleep 1 + if ! kill -0 "$(<"${LOG_DIR}/ros2_bridge.pid")" 2>/dev/null; then + echo "ROS 2 socket bridge exited; see ${LOG_DIR}/ros2_bridge.log" >&2 + exit 1 + fi +fi + +if ! container_ros "rosparam list" >/dev/null 2>&1; then + echo "Starting roscore..." + container_ros_detached \ + "exec roscore >'${LOG_DIR}/roscore.log' 2>&1" +fi +wait_for_container_ros "roscore" "rosparam list" + +if ! container_ros "rosnode list | grep -qx /isaac_hw" >/dev/null 2>&1; then + echo "Starting the ROS 1 socket client and Isaac hardware interface..." + container_ros_detached \ + "export ROBOT_TYPE=go1; exec roslaunch isaac_legged_hw bringup_isaac.launch socket_host:='${ROS2_SOCKET_HOST}' socket_port:='${ROS2_SOCKET_PORT}' >'${LOG_DIR}/bringup.log' 2>&1" +fi +wait_for_container_ros "controller manager" \ + "rosservice list | grep -qx /controller_manager/list_controllers" 90 + +if ! container_ros \ + "rosservice call /controller_manager/list_controllers | grep -q controllers/legged_cheater_controller" \ + >/dev/null 2>&1; then + echo "Loading the Isaac cheater controller..." + container_ros_detached \ + "export ROBOT_TYPE=go1; exec roslaunch isaac_legged_hw control_isaac.launch cheater:=true start_gait_command:=false >'${LOG_DIR}/control.log' 2>&1" +fi +wait_for_container_ros "cheater controller" \ + "rosservice call /controller_manager/list_controllers | grep -q controllers/legged_cheater_controller" 90 + +echo "Switching the cheater controller to running..." +container_ros "rosservice call /controller_manager/switch_controller \"start_controllers: ['controllers/legged_cheater_controller'] +stop_controllers: [] +strictness: 2 +start_asap: false +timeout: 0.0\"" >/dev/null + +wait_for_container_ros "running controller" \ + "rosservice call /controller_manager/list_controllers | grep -A2 controllers/legged_cheater_controller | grep -q 'state:.*running'" 30 + +cat <(6); + const scalar_t currentX = currentPose(0); + const scalar_t currentY = currentPose(1); + const scalar_t currentYaw = currentPose(3); + const scalar_t vxBody = cmdVel(0); + const scalar_t vyBody = cmdVel(1); + const scalar_t wz = cmdVel(3); + + if (!referenceInitialized) { + xReference = currentX; + yReference = currentY; + yawReference = currentYaw; + lastObservationTime = observation.time; + referenceInitialized = true; + } + + scalar_t dt = observation.time - lastObservationTime; + dt = std::max(0.0, std::min(dt, 0.05)); + lastObservationTime = observation.time; + + if (std::abs(wz) > 0.05) { + yawReference = currentYaw; + } + + const scalar_t cy = std::cos(yawReference); + const scalar_t sy = std::sin(yawReference); + const scalar_t vxWorld = cy * vxBody - sy * vyBody; + const scalar_t vyWorld = sy * vxBody + cy * vyBody; + + xReference += vxWorld * dt; + yReference += vyWorld * dt; + yawReference += wz * dt; + + vector_t targetPose(6); + targetPose(0) = xReference; + targetPose(1) = yReference; + targetPose(2) = COM_HEIGHT; + targetPose(3) = yawReference; + targetPose(4) = 0.0; + targetPose(5) = 0.0; + + const scalar_t targetReachingTime = observation.time + TIME_TO_TARGET; + auto trajectories = targetPoseToTargetTrajectories(targetPose, observation, targetReachingTime); + trajectories.stateTrajectory[0].head(3) << vxWorld, vyWorld, 0.0; + trajectories.stateTrajectory[1].head(3) << vxWorld, vyWorld, 0.0; + + return trajectories; +} int main(int argc, char** argv) { const std::string robotName = "legged_robot"; @@ -107,7 +161,11 @@ int main(int argc, char** argv) { loadData::loadCppDataType(referenceFile, "targetDisplacementVelocity", TARGET_DISPLACEMENT_VELOCITY); loadData::loadCppDataType(taskFile, "mpc.timeHorizon", TIME_TO_TARGET); - TargetTrajectoriesPublisher target_pose_command(nodeHandle, robotName, &goalToTargetTrajectories, &cmdVelToTargetTrajectories); + bool usePersistentVelocityReference = false; + nodeHandle.param("~use_persistent_velocity_reference", usePersistentVelocityReference, false); + auto cmdVelConverter = usePersistentVelocityReference ? &cmdVelToPersistentTargetTrajectories : &cmdVelToTargetTrajectories; + + TargetTrajectoriesPublisher target_pose_command(nodeHandle, robotName, &goalToTargetTrajectories, cmdVelConverter); ros::spin(); // Successful exit