使用PX4，gazebo，mavros為旋翼添加下視的相機（仿真采集openrealm數據集-第一步）

一.方法一（沒成功）

1.運行PX4?

2.運行mavros通訊

3.啟動仿真世界和無人機

（1）單獨測試相機

（2）make px4_sitl gazebo啟動四旋翼iris無人機

二.方法二（成功）

1.通過?roslaunch?啟動

2.運行rostopic list和rosnode list有gazebo節點和相關話題

3.如何修改iris.sdf，我應該在iris的哪里添加相機呢？

一、添加相機的位置

在哪里添加？

二、修改后的 iris.sdf

三、代碼說明

四、總結

4.檢查相機是否符合要求

三.解釋和分析上述 iris.sdf 文件的內容，包括其結構、語法和每個部分的含義

一、SDF 文件總體結構與語法

1. 文件頭

2. 模型定義

二、主要組成部分分析

- 物理部件

示例：base_link（主體）

示例：rotor_0（旋翼）

示例：camera_link（相機）

2. - 關節

示例：rotor_0_joint

示例：camera_joint

3. - 插件

示例：motor_model（電機）

示例：mavlink_interface（MAVLink 接口）

4. - 外部模型

三、語法規則

四、整體功能分析

五、總結

四.sdf模型坐標系是什么，gazebo仿真世界坐標系是什么

一、SDF 模型坐標系

定義

在 iris.sdf 中的體現

特點

二、Gazebo 仿真世界坐標系

定義

在仿真中的體現

特點

三、兩者的關系

四、示例分析（基于 iris.sdf）

模型坐標系

世界坐標系

驗證

五、語法與配置要點

五.如何知道世界坐標系的原點和xyz方向，如何知道模型坐標系的原點和xyz方向

一、確定 Gazebo 世界坐標系的原點和 XYZ 方向

1. 世界坐標系的定義

2. 如何確認原點和方向

方法 1：檢查世界文件

方法 2：通過 Gazebo GUI 可視化

方法 3：檢查 /gazebo/model_states

二、確定 SDF 模型坐標系的原點和 XYZ 方向

1. 模型坐標系的定義

2. 如何確認原點和方向

方法 1：分析 SDF 文件

方法 2：Gazebo GUI 可視化

方法 3：檢查傳感器數據

三、兩者的關系與驗證

四、總結

世界坐標系

模型坐標系

實用建議

六.依然是坐標系問題

一、解釋 /gazebo/model_states 輸出

命令與輸出

輸出內容

分析

總結

二、Gazebo GUI 中坐標軸顏色對應

操作

顏色與 XYZ 的對應

三、rotor_0 在右前方，模型坐標系的 XYZ 方向

rotor_0 的位置

模型坐標系的 XYZ 方向

驗證

四、綜合分析

五、建議

一.方法一（沒成功）

1.運行PX4?

make px4_sitl gazebo

默認啟動的是iris四旋翼模型

2.運行mavros通訊

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

“Gazebo ROS 插件未初始化”意味著 Gazebo 仿真器無法正確與 ROS（Robot Operating System）通信，導致 ROS 插件（如 libgazebo_ros_camera.so）無法加載或發布話題。這種情況通常會表現為你在 rostopic list 中看不到預期的傳感器話題（如 /downward_camera/image_raw），或者 rosnode list 中缺少 /gazebo 節點。以下是問題的原因分析、排查步驟和解決方法。

3.啟動仿真世界和無人機

（1）單獨測試相機

~/test_camera/test_camera.sdf代碼為：?

<sdf version="1.6"><world name="test_world"><model name="camera_test"><link name="camera_link"><pose>0 0 0 0 0 0</pose><inertial><mass>0.01</mass><inertia><ixx>1e-05</ixx><ixy>0</ixy><ixz>0</ixz><iyy>1e-05</iyy><iyz>0</iyz><izz>1e-05</izz></inertia></inertial><sensor name="test_camera" type="camera"><always_on>true</always_on><update_rate>10.0</update_rate><camera><horizontal_fov>1.047</horizontal_fov><image><width>640</width><height>480</height><format>R8G8B8</format></image><clip><near>0.1</near><far>1000</far></clip></camera><plugin name="camera_plugin" filename="libgazebo_ros_camera.so"><ros><namespace>/test_camera</namespace></ros><frame_name>camera_link</frame_name><cameraName>test_camera</cameraName></plugin></sensor></link></model></world>
</sdf>

不要直接用 gazebo 命令，而是通過 ROS 啟動：

rosrun gazebo_ros gazebo ~/test_camera/test_camera.sdf

rosrun gazebo_ros gazebo?= 啟動 Gazebo + 啟用 ROS 接口。

即可成功出現gazebo節點和相關話題?

中間的某一步export?

leaf@LAPTOP-NI05EP5K:~$ find /usr/lib /opt/ros -name "libCameraPlugin.so"
/usr/lib/x86_64-linux-gnu/gazebo-11/plugins/libCameraPlugin.so
leaf@LAPTOP-NI05EP5K:~$ ldd /opt/ros/noetic/lib/libgazebo_ros_camera.so | grep libCameraPluginlibCameraPlugin.so => not found
leaf@LAPTOP-NI05EP5K:~$ export LD_LIBRARY_PATH=/usr/lib/x86_64-linux-gnu/gazebo-11/plugins:$LD_LIBRARY_PATH
leaf@LAPTOP-NI05EP5K:~$ ldd /opt/ros/noetic/lib/libgazebo_ros_camera.so | grep libCameraPluginlibCameraPlugin.so => /usr/lib/x86_64-linux-gnu/gazebo-11/plugins/libCameraPlugin.so (0x00007fe3b6f25000)
leaf@LAPTOP-NI05EP5K:~$ cat gazebo_log.txt
WARNING: Package name "MSTC_Star_update" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
WARNING: Package name "MSTC_Star_update" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
WARNING: Package name "MSTC_Star_update" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
leaf@LAPTOP-NI05EP5K:~$ ldd /opt/ros/noetic/lib/libgazebo_ros_camera.so | grep libCameraPluginlibCameraPlugin.so => /usr/lib/x86_64-linux-gnu/gazebo-11/plugins/libCameraPlugin.so (0x00007f77cf450000)

?

清理所有節點進程?

killall -9 gzserver gzclient roslaunch rosmaster

leaf@LAPTOP-NI05EP5K:~/test_camera$ killall -9 gzserver gzclient roslaunch rosmaster
gzserver: no process found
gzclient: no process found
roslaunch: no process found

（2）make px4_sitl gazebo啟動四旋翼iris無人機

從你的反饋來看，問題已經非常清晰：rosrun gazebo_ros gazebo 可以成功啟動 Gazebo 并與 ROS 集成，而 make px4_sitl gazebo 雖然啟動了 Gazebo 和 PX4 仿真，但未能正確與 ROS 集成，導致 /gazebo 節點和話題未出現。以下是分析和最終解決方案。

二.方法二（成功）

1.通過?`roslaunch`?啟動

PX4 提供了集成 ROS 的啟動文件，可同時啟動 Gazebo、PX4 SITL 和 MAVROS：

roslaunch px4 mavros_posix_sitl.launch

效果：

自動加載 Gazebo-ROS 插件，發布?/gazebo?話題。
啟動 MAVROS，連接 PX4 和 ROS。

2.運行rostopic list和rosnode list有gazebo節點和相關話題

leaf@LAPTOP-NI05EP5K:~$ rostopic list
/clock
/diagnostics
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/performance_metrics
/gazebo/set_link_state
/gazebo/set_model_state
/mavlink/from
/mavlink/gcs_ip
/mavlink/to
/mavros/actuator_control
/mavros/adsb/send
/mavros/adsb/vehicle
/mavros/altitude
/mavros/battery
/mavros/cam_imu_sync/cam_imu_stamp
/mavros/camera/image_captured
/mavros/cellular_status/status
/mavros/companion_process/status
/mavros/debug_value/debug
/mavros/debug_value/debug_float_array
/mavros/debug_value/debug_vector
/mavros/debug_value/named_value_float
/mavros/debug_value/named_value_int
/mavros/debug_value/send
/mavros/esc_info
/mavros/esc_status
/mavros/esc_telemetry
/mavros/estimator_status
/mavros/extended_state
/mavros/fake_gps/mocap/tf
/mavros/geofence/waypoints
/mavros/global_position/compass_hdg
/mavros/global_position/global
/mavros/global_position/gp_lp_offset
/mavros/global_position/gp_origin
/mavros/global_position/local
/mavros/global_position/raw/fix
/mavros/global_position/raw/gps_vel
/mavros/global_position/raw/satellites
/mavros/global_position/rel_alt
/mavros/global_position/set_gp_origin
/mavros/gps_input/gps_input
/mavros/gps_rtk/rtk_baseline
/mavros/gps_rtk/send_rtcm
/mavros/gpsstatus/gps1/raw
/mavros/gpsstatus/gps1/rtk
/mavros/gpsstatus/gps2/raw
/mavros/gpsstatus/gps2/rtk
/mavros/hil/actuator_controls
/mavros/hil/controls
/mavros/hil/gps
/mavros/hil/imu_ned
/mavros/hil/optical_flow
/mavros/hil/rc_inputs
/mavros/hil/state
/mavros/home_position/home
/mavros/home_position/set
/mavros/imu/data
/mavros/imu/data_raw
/mavros/imu/diff_pressure
/mavros/imu/mag
/mavros/imu/static_pressure
/mavros/imu/temperature_baro
/mavros/imu/temperature_imu
/mavros/landing_target/lt_marker
/mavros/landing_target/pose
/mavros/landing_target/pose_in
/mavros/local_position/accel
/mavros/local_position/odom
/mavros/local_position/pose
/mavros/local_position/pose_cov
/mavros/local_position/velocity_body
/mavros/local_position/velocity_body_cov
/mavros/local_position/velocity_local
/mavros/log_transfer/raw/log_data
/mavros/log_transfer/raw/log_entry
/mavros/mag_calibration/report
/mavros/mag_calibration/status
/mavros/manual_control/control
/mavros/manual_control/send
/mavros/mission/reached
/mavros/mission/waypoints
/mavros/mocap/pose
/mavros/mount_control/command
/mavros/mount_control/orientation
/mavros/mount_control/status
/mavros/nav_controller_output
/mavros/obstacle/send
/mavros/odometry/in
/mavros/odometry/out
/mavros/onboard_computer/status
/mavros/param/param_value
/mavros/play_tune
/mavros/px4flow/ground_distance
/mavros/px4flow/raw/optical_flow_rad
/mavros/px4flow/raw/send
/mavros/px4flow/temperature
/mavros/radio_status
/mavros/rallypoint/waypoints
/mavros/rc/in
/mavros/rc/out
/mavros/rc/override
/mavros/setpoint_accel/accel
/mavros/setpoint_attitude/cmd_vel
/mavros/setpoint_attitude/thrust
/mavros/setpoint_position/global
/mavros/setpoint_position/global_to_local
/mavros/setpoint_position/local
/mavros/setpoint_raw/attitude
/mavros/setpoint_raw/global
/mavros/setpoint_raw/local
/mavros/setpoint_raw/target_attitude
/mavros/setpoint_raw/target_global
/mavros/setpoint_raw/target_local
/mavros/setpoint_trajectory/desired
/mavros/setpoint_trajectory/local
/mavros/setpoint_velocity/cmd_vel
/mavros/setpoint_velocity/cmd_vel_unstamped
/mavros/state
/mavros/statustext/recv
/mavros/statustext/send
/mavros/sys_status
/mavros/target_actuator_control
/mavros/terrain/report
/mavros/time_reference
/mavros/timesync_status
/mavros/trajectory/desired
/mavros/trajectory/generated
/mavros/trajectory/path
/mavros/tunnel/in
/mavros/tunnel/out
/mavros/vfr_hud
/mavros/vision_pose/pose
/mavros/vision_pose/pose_cov
/mavros/vision_speed/speed_twist_cov
/mavros/wind_estimation
/move_base_simple/goal
/rosout
/rosout_agg
/test_camera/camera_info
/test_camera/image_raw
/test_camera/image_raw/compressed
/test_camera/image_raw/compressed/parameter_descriptions
/test_camera/image_raw/compressed/parameter_updates
/test_camera/image_raw/compressedDepth
/test_camera/image_raw/compressedDepth/parameter_descriptions
/test_camera/image_raw/compressedDepth/parameter_updates
/test_camera/image_raw/theora
/test_camera/image_raw/theora/parameter_descriptions
/test_camera/image_raw/theora/parameter_updates
/test_camera/parameter_descriptions
/test_camera/parameter_updates
/tf
/tf_static
leaf@LAPTOP-NI05EP5K:~$ rosnode list
/gazebo
/gazebo_gui
/mavros
/rosout

結論：
- 該命令正確啟動了 Gazebo、PX4 SITL 和 MAVROS，并完成了 ROS 集成。

優點

官方支持：mavros_posix_sitl.launch 是 PX4 提供的標準 launch 文件，集成了 Gazebo、PX4 SITL 和 MAVROS。
自動化：自動處理環境變量、符號鏈接和模型加載，無需手動配置。
穩定性：避免了手動啟動可能出現的路徑或參數錯誤。

與之前方法的對比

之前的手動方法和腳本需要手動管理 roscore、Gazebo 和 PX4 的啟動順序，而 roslaunch px4 mavros_posix_sitl.launch 一次性完成所有步驟。

3.如何修改iris.sdf，我應該在iris的哪里添加相機呢？

在 iris.sdf 中添加相機需要將其作為一個新的 <link> 和 <sensor>，并通過 <joint> 將其固定到 iris 的主體（通常是 base_link）。你的目標是添加一個下視相機，因此我們需要選擇合適的位置和朝向。

以下是如何在 iris.sdf 中添加相機的詳細步驟和代碼：

一、添加相機的位置

目標：下視相機，通常安裝在機身底部。
坐標系：
- base_link 是 iris 的主體坐標系，位于機身中心。
- X 軸向前，Y 軸向右，Z 軸向上。
- 下視相機應放在 base_link 下方（Z 負方向），朝下（繞 Y 軸旋轉 90°，即俯仰角 -90°）。
建議位置：
- <pose>0 0 -0.1 0 1.5708 0</pose> 表示相機位于機身下方 0.1 米，朝下。

在哪里添加？

在 <model name='iris'> 內，添加新的 <link> 和 <joint>，通常放在現有 <link>（如 rotor_* 或 gps0）之后，但在 <plugin> 之前。

二、修改后的 iris.sdf

以下是將相機添加到你的 iris.sdf 的完整代碼（在原始文件基礎上添加相機部分）：

<!-- DO NOT EDIT: Generated from iris.sdf.jinja -->
<sdf version='1.6'><model name='iris'><link name='base_link'><pose>0 0 0 0 0 0</pose><inertial><pose>0 0 0 0 0 0</pose><mass>1.5</mass><inertia><ixx>0.029125</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.029125</iyy><iyz>0</iyz><izz>0.055225</izz></inertia></inertial><collision name='base_link_inertia_collision'><pose>0 0 0 0 0 0</pose><geometry><box><size>0.47 0.47 0.11</size></box></geometry><surface><contact><ode><min_depth>0.001</min_depth><max_vel>0</max_vel></ode></contact><friction><ode/></friction></surface></collision><visual name='base_link_inertia_visual'><pose>0 0 0 0 0 0</pose><geometry><mesh><scale>1 1 1</scale><uri>model://iris/meshes/iris.stl</uri></mesh></geometry><material><script><name>Gazebo/DarkGrey</name><uri>file://media/materials/scripts/gazebo.material</uri></script></material></visual><gravity>1</gravity><velocity_decay/></link><!-- 其他現有 link 和 joint（如 imu_link, rotor_0 等）保持不變 --><link name='/imu_link'><!-- 原有內容 --></link><joint name='/imu_joint' type='revolute'><!-- 原有內容 --></joint><link name='rotor_0'><!-- 原有內容 --></link><joint name='rotor_0_joint' type='revolute'><!-- 原有內容 --></joint><!-- rotor_1, rotor_2, rotor_3 等保持不變 --><!-- 添加相機 link --><link name="camera_link"><pose>0 0 -0.1 0 1.5708 0</pose> <!-- 下方 0.1 米，朝下 (繞 Y 軸旋轉 90°) --><inertial><pose>0 0 0 0 0 0</pose><mass>0.01</mass> <!-- 相機質量，通常很小 --><inertia><ixx>1e-05</ixx><ixy>0</ixy><ixz>0</ixz><iyy>1e-05</iyy><iyz>0</iyz><izz>1e-05</izz></inertia></inertial><!-- 可選：添加可視化幾何體 --><visual name="camera_visual"><pose>0 0 0 0 0 0</pose><geometry><box><size>0.02 0.02 0.02</size> <!-- 小盒子表示相機 --></box></geometry><material><script><name>Gazebo/Red</name><uri>file://media/materials/scripts/gazebo.material</uri></script></material></visual><!-- 相機傳感器 --><sensor name="test_camera" type="camera"><always_on>true</always_on><update_rate>10.0</update_rate> <!-- 每秒 10 幀 --><camera><horizontal_fov>1.047</horizontal_fov> <!-- 視場角約 60° --><image><width>640</width><height>480</height><format>R8G8B8</format> <!-- RGB 格式 --></image><clip><near>0.1</near><far>1000</far></clip></camera><plugin name="camera_plugin" filename="libgazebo_ros_camera.so"><ros><namespace>/test_camera</namespace></ros><frame_name>camera_link</frame_name><cameraName>test_camera</cameraName></plugin></sensor></link><!-- 將相機固定到 base_link --><joint name="camera_joint" type="fixed"><parent>base_link</parent><child>camera_link</child></joint><!-- 以下是原有插件部分，保持不變 --><plugin name='rosbag' filename='libgazebo_multirotor_base_plugin.so'><!-- 原有內容 --></plugin><!-- 其他插件如 motor_model, gps, groundtruth 等保持不變 --></model>
</sdf>

三、代碼說明

相機位置：
- <pose>0 0 -0.1 0 1.5708 0</pose>：
  - 0 0 -0.1：相機位于 base_link 下方 0.1 米。
  - 0 1.5708 0：繞 Y 軸旋轉 90°（1.5708 弧度），使相機朝下。
慣性參數：
- <mass>0.01</mass> 和 <inertia>：相機質量和慣性矩陣，值較小以模擬輕型相機。
可視化（可選）：
- 添加了一個紅色小盒子作為相機模型，便于在 Gazebo 中看到。
傳感器：
- <sensor name="test_camera" type="camera"> 定義相機參數。
- <plugin name="camera_plugin" filename="libgazebo_ros_camera.so"> 使用 ROS 插件發布圖像到 /test_camera/image_raw。
關節：
- <joint name="camera_joint" type="fixed"> 將相機固定到 base_link。

四、總結

添加位置：在 <model name='iris'> 內，rotor_* 和 gps0 之后，<plugin> 之前。
效果：相機固定在 base_link 下方，朝下拍攝，圖像通過 ROS 話題發布。

4.檢查相機是否符合要求

話題列表：

/test_camera/image_raw：原始圖像數據。
/test_camera/camera_info：相機內參和畸變參數。
/test_camera/image_raw/compressed 等：壓縮格式圖像（由插件自動生成）。

運行仿真：

roslaunch px4 mavros_posix_sitl.launch

檢查相機話題：

rostopic list | grep test_camera

查看圖像：

rosrun image_view image_view image:=/test_camera/image_raw

檢查發布頻率：

rostopic hz /test_camera/image_raw

輸出話題的平均發布頻率（單位：Hz）。
預期：你在 iris.sdf 中設置了 <update_rate>10.0</update_rate>，應接近 10 Hz。
調整：如果需要更高幀率，修改 iris.sdf 中的 <update_rate>（如 30.0），保存后重啟仿真：

查看圖像信息：

rostopic echo /test_camera/image_raw -n 1

使用 camera_info：

rostopic echo /test_camera/camera_info -n 1

三.解釋和分析上述 iris.sdf 文件的內容，包括其結構、語法和每個部分的含義

我將詳細解釋和分析上述 iris.sdf 文件的內容，包括其結構、語法和每個部分的含義。SDF（Simulation Description Format）是 Gazebo 仿真器使用的 XML 格式文件，用于定義機器人模型的物理屬性、可視化、傳感器和插件等。

一、SDF 文件總體結構與語法

1. 文件頭

<sdf version='1.6'>

含義：聲明 SDF 文件的版本為 1.6，表明使用的語法和功能符合 SDF 1.6 規范。
語法：<sdf> 是根標簽，所有內容必須包裹在此標簽內，version 屬性指定版本號。

2. 模型定義

<model name='iris'>

含義：定義一個名為 iris 的模型（四旋翼無人機）。
語法：<model> 標簽表示一個獨立的物理實體，name 屬性是模型的唯一標識符。
嵌套內容：包含 <link>（物理部件）、<joint>（關節）和 <plugin>（插件）等。

二、主要組成部分分析

1. <link> - 物理部件

示例：base_link（主體）

<link name='base_link'><pose>0 0 0 0 0 0</pose><inertial><pose>0 0 0 0 0 0</pose><mass>1.5</mass><inertia><ixx>0.029125</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.029125</iyy><iyz>0</iyz><izz>0.055225</izz></inertia></inertial><collision name='base_link_inertia_collision'><pose>0 0 0 0 0 0</pose><geometry><box><size>0.47 0.47 0.11</size></box></geometry><surface><contact><ode><min_depth>0.001</min_depth><max_vel>0</max_vel></ode></contact><friction><ode/></friction></surface></collision><visual name='base_link_inertia_visual'><pose>0 0 0 0 0 0</pose><geometry><mesh><scale>1 1 1</scale><uri>model://iris/meshes/iris.stl</uri></mesh></geometry><material><script><name>Gazebo/DarkGrey</name><uri>file://media/materials/scripts/gazebo.material</uri></script></material></visual><gravity>1</gravity><velocity_decay/>
</link>

<pose>：x y z roll pitch yaw（單位：米和弧度），表示相對于模型坐標系的位置和姿態。
- 0 0 0 0 0 0：位于原點，無旋轉。
<inertial>：定義物理屬性。
- <mass>：1.5 kg，主體質量。
- <inertia>：慣性張量（對角矩陣），描述質量分布，單位 kg·m2。
  - ixx, iyy, izz：主軸慣性，ixy, ixz, iyz：交叉項（這里為 0，表示對稱）。
<collision>：碰撞體，用于物理計算。
- <geometry><box><size>：0.47x0.47x0.11 m 的盒子。
- <surface>：接觸和摩擦屬性，ode 表示使用 ODE 物理引擎。
  - <min_depth>：最小穿透深度。
  - <max_vel>：最大接觸速度。
<visual>：可視化外觀。
- <geometry><mesh>：加載 iris.stl 網格文件。
- <material>：顏色為深灰（Gazebo/DarkGrey）。
<gravity>：1，表示受重力影響。
<velocity_decay>：速度衰減（默認空值，無阻尼）。

示例：rotor_0（旋翼）

<link name='rotor_0'><pose>0.13 -0.22 0.023 0 0 0</pose><inertial><mass>0.005</mass><inertia><ixx>9.75e-07</ixx><iyy>0.000273104</iyy><izz>0.000274004</izz></inertia></inertial><collision name='rotor_0_collision'><geometry><cylinder><length>0.005</length><radius>0.128</radius></cylinder></geometry></collision><visual name='rotor_0_visual'><geometry><mesh><uri>model://iris/meshes/iris_prop_ccw.dae</uri></mesh></geometry><material><script><name>Gazebo/Blue</name></script></material></visual><gravity>1</gravity>
</link>

<pose>：旋翼位置 (0.13, -0.22, 0.023)，相對于 base_link。
<inertial>：質量 0.005 kg，慣性較小。
<collision>：圓柱體（半徑 0.128 m，厚度 0.005 m）。
<visual>：加載逆時針螺旋槳模型，顏色為藍色。

示例：camera_link（相機）

<link name="camera_link"><pose>0 0 -0.1 0 1.5708 0</pose><inertial><mass>0.01</mass><inertia><ixx>1e-05</ixx><iyy>1e-05</iyy><izz>1e-05</izz></inertia></inertial><visual name="camera_visual"><geometry><box><size>0.02 0.02 0.02</size></box></geometry><material><script><name>Gazebo/Red</name></script></material></visual><sensor name="test_camera" type="camera"><always_on>true</always_on><update_rate>10.0</update_rate><camera><horizontal_fov>1.047</horizontal_fov><image><width>640</width><height>480</height><format>R8G8B8</format></image><clip><near>0.1</near><far>1000</far></clip></camera><plugin name="camera_plugin" filename="libgazebo_ros_camera.so"><ros><namespace>/test_camera</namespace></ros><frame_name>camera_link</frame_name><cameraName>test_camera</cameraName></plugin></sensor>
</link>

<pose>：下方 0.1 m，繞 Y 軸旋轉 90°（朝下）。
<inertial>：質量 0.01 kg，慣性小。
<visual>：紅色小盒子表示相機。
<sensor>：相機傳感器。
- <update_rate>：10 Hz。
- <horizontal_fov>：約 60°。
- <image>：分辨率 640x480，RGB 格式。
- <clip>：近裁剪面 0.1 m，遠裁剪面 1000 m。
- <plugin>：libgazebo_ros_camera.so 將圖像發布到 ROS 話題 /test_camera/image_raw。

2. <joint> - 關節

<joint> 定義部件之間的連接。

示例：rotor_0_joint

<joint name='rotor_0_joint' type='revolute'><child>rotor_0</child><parent>base_link</parent><axis><xyz>0 0 1</xyz><limit><lower>-1e+16</lower><upper>1e+16</upper></limit><dynamics><spring_reference>0</spring_reference><spring_stiffness>0</spring_stiffness></dynamics><use_parent_model_frame>1</use_parent_model_frame></axis>
</joint>

<type>：revolute（旋轉關節）。
<child> 和 <parent>：連接 rotor_0 和 base_link。
<axis>：旋轉軸。
- <xyz>：Z 軸 (0 0 1)。
- <limit>：旋轉范圍（幾乎無限制）。
- <dynamics>：無彈簧效應。

示例：camera_joint

<joint name="camera_joint" type="fixed"><parent>base_link</parent><child>camera_link</child>
</joint>

<type>：fixed（固定關節），相機不可動。
連接：將 camera_link 固定到 base_link。

3. <plugin> - 插件

<plugin> 添加仿真功能，如動力學或傳感器數據發布。

示例：motor_model（電機）

<plugin name='front_right_motor_model' filename='libgazebo_motor_model.so'><jointName>rotor_0_joint</jointName><linkName>rotor_0</linkName><turningDirection>ccw</turningDirection><maxRotVelocity>1100</maxRotVelocity><motorConstant>5.84e-06</motorConstant><commandSubTopic>/gazebo/command/motor_speed</commandSubTopic><motorNumber>0</motorNumber>
</plugin>

<filename>：加載 libgazebo_motor_model.so。
參數：
- <jointName>：控制 rotor_0_joint。
- <turningDirection>：逆時針旋轉。
- <maxRotVelocity>：最大轉速 1100 rad/s。
- <commandSubTopic>：訂閱電機速度指令。

示例：mavlink_interface（MAVLink 接口）

<plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'><imuSubTopic>/imu</imuSubTopic><mavlink_tcp_port>4560</mavlink_tcp_port><enable_lockstep>1</enable_lockstep>
</plugin>

功能：連接 Gazebo 和 PX4，通過 MAVLink 協議通信。
參數：
- <imuSubTopic>：訂閱 IMU 數據。
- <mavlink_tcp_port>：TCP 端口 4560。
- <enable_lockstep>：同步仿真和 PX4 時間。

4. <include> - 外部模型

<include><uri>model://gps</uri><pose>0.1 0 0 0 0 0</pose><name>gps0</name>
</include>

含義：引入外部 GPS 模型，位置在前方 0.1 m。

三、語法規則

XML 格式：層級結構，標簽必須閉合。
單位：
- 長度：米 (m)。
- 角度：弧度 (rad)。
- 質量：千克 (kg)。
坐標系：
- 模型坐標系：base_link 為原點。
- <pose>：x y z roll pitch yaw。
命名：name 屬性必須唯一。

四、整體功能分析

模型：iris 是一個四旋翼無人機。
部件：
- base_link：機身。
- rotor_*：四個旋翼。
- imu_link：IMU 傳感器。
- camera_link：下視相機。
關節：旋翼可旋轉，IMU 和相機固定。
插件：實現電機控制、傳感器數據發布和 PX4 集成。

五、總結

這個 iris.sdf 文件定義了一個功能完整的四旋翼模型，包含物理屬性、可視化、傳感器（相機、IMU、GPS 等）和 PX4 仿真接口。語法符合 SDF 1.6 規范，結構清晰，便于擴展（如添加更多傳感器）。如果有具體部分需要深入分析，請告訴我！

四.sdf模型坐標系是什么，gazebo仿真世界坐標系是什么

在 Gazebo 中，SDF（Simulation Description Format）模型坐標系和 Gazebo 仿真世界坐標系是兩個相關但不同的概念。理解它們的定義和關系對于正確配置機器人模型（如 iris.sdf）以及在仿真中定位和控制非常重要。以下是詳細解釋。

一、SDF 模型坐標系

定義

SDF 模型坐標系 是指一個 SDF 文件中定義的模型（例如 iris）自身的局部坐標系。
原點：由 <model> 標簽內的第一個 <link>（通常是主體部件，如 base_link）的 <pose> 定義。如果未指定，默認位于模型的幾何中心。
方向：
- X 軸：向前（正方向）。
- Y 軸：向右（正方向）。
- Z 軸：向上（正方向）。
單位：米（長度），弧度（角度）。

在 iris.sdf 中的體現

<model name='iris'><link name='base_link'><pose>0 0 0 0 0 0</pose>...</link><link name='rotor_0'><pose>0.13 -0.22 0.023 0 0 0</pose>...</link><link name="camera_link"><pose>0 0 -0.1 0 1.5708 0</pose>...</link>
</model>

原點：base_link 的 <pose>0 0 0 0 0 0</pose> 定義了模型坐標系的原點（位于機身中心）。
相對位置：
- rotor_0 位于 (0.13, -0.22, 0.023)，即相對于 base_link 前方 0.13 m，左側 0.22 m，上方 0.023 m。
- camera_link 位于 (0, 0, -0.1)，下方 0.1 m，繞 Y 軸旋轉 1.5708 弧度（90°，朝下）。
特性：所有 <link> 和 <joint> 的 <pose> 都是相對于模型坐標系的局部偏移。

特點

獨立性：模型坐標系是模型內部的參考系，與外部世界無關。
移動性：當模型在仿真中移動時，整個模型坐標系隨模型一起移動。

二、Gazebo 仿真世界坐標系

定義

Gazebo 仿真世界坐標系 是 Gazebo 仿真環境的全局參考系，固定不變。
原點：通常位于仿真世界的 (0, 0, 0)，對應于 Gazebo 窗口的中心地面。
方向：
- X 軸：向東（正方向）。
- Y 軸：向北（正方向）。
- Z 軸：向上（正方向，與重力相反）。
單位：與 SDF 一致，米和弧度。

在仿真中的體現

模型放置：當你通過 roslaunch px4 mavros_posix_sitl.launch 啟動仿真時，iris 模型會被放置到世界坐標系中的某個位置。
- 默認位置通常由 launch 文件或 Gazebo 的世界文件（.world）指定，例如 (0, 0, 0)。
話題數據：
- /gazebo/model_states 話題提供所有模型相對于世界坐標系的位置和姿態。
- 示例：若 iris 的 base_link 在世界坐標系中位于 (1, 2, 0.5)，則其模型坐標系原點也在此點。

特點

固定性：世界坐標系是全局的，不隨模型移動。
參考性：所有模型、傳感器數據和物理計算都以世界坐標系為基準。

三、兩者的關系

層次關系：
- 模型坐標系是局部坐標系，嵌套在世界坐標系內。
- 模型坐標系的原點位置和姿態由模型在世界坐標系中的 <pose> 決定。
變換：
- 從模型坐標系到世界坐標系需要應用模型的全局位姿（位置和旋轉）。
- 例如，若 iris 在世界坐標系中位于 (1, 0, 0)，則 camera_link 的世界坐標為 (1, 0, -0.1)。
實際應用：
- 在 SDF 中配置時，使用模型坐標系（相對 base_link）。
- 在仿真運行時，傳感器數據（如相機圖像的 frame_id）和運動控制基于世界坐標系。

四、示例分析（基于 iris.sdf）

模型坐標系

原點：base_link 位于 (0, 0, 0)。
旋翼：rotor_0 在 (0.13, -0.22, 0.023)，表示相對于機身中心的偏移。
相機：camera_link 在 (0, 0, -0.1)，下方 0.1 m，朝下。

世界坐標系

假設：iris 被放置在世界坐標系的 (0, 0, 1)（起飛高度 1 m）。
計算：
- base_link 世界坐標：(0, 0, 1)。
- rotor_0 世界坐標：(0.13, -0.22, 1.023)。
- camera_link 世界坐標：(0, 0, 0.9)。

驗證

運行仿真：

roslaunch px4 mavros_posix_sitl.launch

檢查模型位置：

rostopic echo /gazebo/model_states -n 1

- 查找 iris 的 pose，確認其在世界坐標系中的位置。

五、語法與配置要點

SDF 中的 <pose>：
- 格式：x y z roll pitch yaw。
- 在 <link> 中定義相對于模型坐標系的位置。
世界坐標系的指定：
- 通過 Gazebo 的 .world 文件或 ROS launch 文件（如 mavros_posix_sitl.launch）設置模型初始位置。
- 示例（在 .world 文件中）：

<include><uri>model://iris</uri>

五.如何知道世界坐標系的原點和xyz方向，如何知道模型坐標系的原點和xyz方向

在 Gazebo 中，確定世界坐標系和模型坐標系的原點及其 X、Y、Z 方向需要結合文件配置、仿真環境和可視化工具進行觀察和驗證。以下是詳細的步驟和方法，幫助你明確這兩者的原點和方向。

一、確定 Gazebo 世界坐標系的原點和 XYZ 方向

1. 世界坐標系的定義

原點：默認情況下，Gazebo 世界坐標系的原點位于仿真環境的 (0, 0, 0)，通常對應于地面中心。
方向：
- X 軸：向東（正方向）。
- Y 軸：向北（正方向）。
- Z 軸：向上（正方向，與重力相反）。

2. 如何確認原點和方向

方法 1：檢查世界文件

Gazebo 的世界坐標系由 .world 文件定義。
示例：在你的 PX4 SITL 仿真中，可能是 ~/PX4_Firmware/Tools/sitl_gazebo/worlds/empty.world。
查看文件：

cat ~/PX4_Firmware/Tools/sitl_gazebo/worlds/empty.world

查找 <pose> 或 <origin>：

<world name="empty"><scene><ambient>0.4 0.4 0.4 1</ambient><background>0.7 0.7 0.7 1</background></scene><physics type="ode">...</physics><model name="ground_plane"><pose>0 0 0 0 0 0</pose>...</model>
</world>

- 分析：
  - ground_plane 的 <pose>0 0 0 0 0 0</pose> 表示地面位于世界坐標系原點。
  - 原點默認在地面中心，Z=0 為地面高度。

方法 2：通過 Gazebo GUI 可視化

啟動仿真：

roslaunch px4 mavros_posix_sitl.launch

觀察：
- Gazebo 窗口中，地面是一個灰色平面，其中心通常是世界坐標系原點 (0, 0, 0)。
- XYZ 方向：
  - 在 Gazebo 中，右鍵點擊空白處，選擇 “View -> Wireframe” 或 “View -> Orbit”，觀察坐標軸方向。
  - 默認情況下，X 軸向右（東），Y 軸向前（北），Z 軸向上。
添加坐標軸（可選）：
- 在 .world 文件中手動添加一個坐標軸模型：

<include><uri>model://coordinate_frame</uri><pose>0 0 0 0 0 0</pose>
</include>

- 重啟仿真后，坐標軸模型會顯示紅（X）、綠（Y）、藍（Z），直觀確認方向。

方法 3：檢查 /gazebo/model_states

運行命令：

rostopic echo /gazebo/model_states -n 1

輸出示例：

name: ['ground_plane', 'iris']
pose:-position:x: 0.0y: 0.0z: 0.0orientation:x: 0.0y: 0.0z: 0.0w: 1.0-position:x: 0.0y: 0.0z: 0.5orientation:x: 0.0y: 0.0z: 0.0w: 1.0

分析：
- ground_plane 在 (0, 0, 0)，確認世界原點在地面。
- iris 在 (0, 0, 0.5)，表示模型初始位于 Z=0.5 m。
- 方向由四元數 orientation 表示，(0, 0, 0, 1) 表示無旋轉，與世界坐標系對齊。

二、確定 SDF 模型坐標系的原點和 XYZ 方向

1. 模型坐標系的定義

原點：由 <model> 內第一個 <link>（通常是 base_link）的 <pose> 定義，默認情況下是 (0, 0, 0)。
方向：
- X 軸：向前（正方向）。
- Y 軸：向右（正方向）。
- Z 軸：向上（正方向）。
局部性：所有其他 <link> 和 <joint> 的 <pose> 是相對于模型坐標系的偏移。

2. 如何確認原點和方向

方法 1：分析 SDF 文件

查看 iris.sdf：

<model name='iris'><link name='base_link'><pose>0 0 0 0 0 0</pose>...</link><link name='rotor_0'><pose>0.13 -0.22 0.023 0 0 0</pose>...</link><link name="camera_link"><pose>0 0 -0.1 0 1.5708 0</pose>...</link>
</model>

分析：
- 原點：base_link 的 <pose>0 0 0 0 0 0</pose> 定義模型坐標系原點。
- 方向：
  - rotor_0 在 (0.13, -0.22, 0.023)：
    - X 正方向（0.13）：向前。
    - Y 負方向（-0.22）：向左（相對于右為正）。
    - Z 正方向（0.023）：向上。
  - camera_link 在 (0, 0, -0.1)，Z 負方向為下方，pitch=1.5708（90°）使相機朝下。
- 結論：模型坐標系的 X 向前，Y 向右，Z 向上，與慣例一致。

方法 2：Gazebo GUI 可視化

啟動仿真：

roslaunch px4 mavros_posix_sitl.launch

觀察模型：
- 在 Gazebo 中選中 iris，右鍵選擇 “View -> Transparent” 或 “Wireframe”，查看部件布局。
- base_link 是機身中心，旋翼位置（如 rotor_0 在左前方）可確認方向：
  - X 軸：機頭方向。
  - Y 軸：右側方向。
  - Z 軸：頂部方向。
添加坐標軸：
- 修改 iris.sdf，在 base_link 中添加可視化坐標軸：

<visual name="base_link_axes"><pose>0 0 0 0 0 0</pose><geometry><mesh><uri>model://coordinate_frame/meshes/axes.dae</uri><scale>0.1 0.1 0.1</scale></mesh></geometry>
</visual>

- 重啟仿真后，機身中心會顯示坐標軸（紅 X，綠 Y，藍 Z）。

方法 3：檢查傳感器數據

相機朝向：

rosrun image_view image_view image:=/test_camera/image_raw

- camera_link 的 <pose>0 0 -0.1 0 1.5708 0</pose> 表示朝下，圖像應顯示地面，確認 Z 負方向。
IMU 數據：

rostopic echo /imu -n 1

- IMU 的 <pose> 與 base_link 對齊，檢查加速度（Z 軸應接近 -9.81 m/s2，表示向上為正）。

三、兩者的關系與驗證

模型坐標系到世界坐標系：
- 模型坐標系的原點和方向由 iris 在世界坐標系中的位姿決定。
- 示例：若 iris 在世界坐標系的 (1, 0, 0)，無旋轉，則 rotor_0 的世界坐標為 (1.13, -0.22, 0.023)。
驗證方法：
- 在 Gazebo 中移動 iris（通過 GUI 或 /gazebo/set_model_state），觀察模型坐標系如何隨世界坐標系變化。
- 示例命令：

rostopic pub /gazebo/set_model_state gazebo_msgs/ModelState "model_name: 'iris'
pose:position:x: 2.0y: 0.0z: 1.0orientation:x: 0.0y: 0.0z: 0.0w: 1.0"

四、總結

世界坐標系

原點：地面中心 (0, 0, 0)，由 .world 文件或默認設置確定。
方向：X 東，Y 北，Z 上，通過 GUI 或 /gazebo/model_states 驗證。

模型坐標系

原點：base_link 的 <pose>（(0, 0, 0)），即機身中心。
方向：X 前，Y 右，Z 上，通過 SDF 配置和可視化（如旋翼、相機位置）確認。

實用建議

調試：在 Gazebo 中添加坐標軸模型，或用 ROS 話題檢查位姿。
可視化：用 rviz 訂閱 /test_camera/image_raw 和 /tf，直觀顯示坐標系關系。

六.依然是坐標系問題

解釋一下下面的輸出：

leaf@LAPTOP-NI05EP5K:~$ rostopic echo /gazebo/model_states -n 1
name:- ground_plane- asphalt_plane- iris
pose:-position:x: 0.0y: 0.0z: 0.0orientation:x: 0.0y: 0.0z: 0.0w: 1.0-position:x: 0.0y: 0.0z: 0.0orientation:x: 0.0y: 0.0z: 0.0w: 1.0-position:x: -0.03940256735964742y: 0.014403233290510268z: 0.10448983608463772orientation:x: 2.0178038215501333e-05y: 0.0010289933143760208z: -0.016112123443332602w: 0.9998696616278306
twist:-linear:x: 0.0y: 0.0z: 0.0angular:x: 0.0y: 0.0z: 0.0-linear:x: 0.0y: 0.0z: 0.0angular:x: 0.0y: 0.0z: 0.0-linear:x: -4.174362083928031e-06y: -3.40546941860435e-06z: -1.8908038290643137e-06angular:x: 6.162506394544502e-05y: -7.602476140973817e-05z: -5.77627563001367e-08
---

在 Gazebo 中，右鍵點擊空白處，選擇 “View -> Wireframe” 或 “View -> Orbit”，觀察坐標軸，紅綠藍三條線分別對應的是xyz中的什么？

rotor_0 在機身的右前方，模型坐標系的 XYZ 方向是什么

我將逐一解釋你的問題，包括 /gazebo/model_states 的輸出、Gazebo GUI 中坐標軸的顏色對應，以及 rotor_0 位置與模型坐標系方向的關系。

一、解釋 /gazebo/model_states 輸出

命令與輸出

leaf@LAPTOP-NI05EP5K:~$ rostopic echo /gazebo/model_states -n 1

話題：/gazebo/model_states 是 Gazebo 發布的消息，類型為 gazebo_msgs/ModelStates，包含仿真中所有模型的狀態（位置、姿態、速度）。
-n 1：僅顯示一條消息。

輸出內容

name:- ground_plane- asphalt_plane- iris
pose:-position: {x: 0.0, y: 0.0, z: 0.0}orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}-position: {x: 0.0, y: 0.0, z: 0.0}orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}-position: {x: -0.03940256735964742, y: 0.014403233290510268, z: 0.10448983608463772}orientation: {x: 2.0178038215501333e-05, y: 0.0010289933143760208, z: -0.016112123443332602, w: 0.9998696616278306}
twist:-linear: {x: 0.0, y: 0.0, z: 0.0}angular: {x: 0.0, y: 0.0, z: 0.0}-linear: {x: 0.0, y: 0.0, z: 0.0}angular: {x: 0.0, y: 0.0, z: 0.0}-linear: {x: -4.174362083928031e-06, y: -3.40546941860435e-06, z: -1.8908038290643137e-06}angular: {x: 6.162506394544502e-05, y: -7.602476140973817e-05, z: -5.77627563001367e-08}

分析

name：
- ground_plane：地面模型。
- asphalt_plane：可能是額外的地面或跑道模型。
- iris：你的四旋翼無人機模型。
- 順序：與后續 pose 和 twist 的列表對應。
pose（位置和姿態，相對于世界坐標系）：
- ground_plane：
  - position: (0.0, 0.0, 0.0)：位于世界坐標系原點。
  - orientation: (0, 0, 0, 1)：四元數表示無旋轉，與世界坐標系對齊。
- asphalt_plane：
  - position: (0.0, 0.0, 0.0)：同樣位于原點，可能與 ground_plane 重疊。
  - orientation: (0, 0, 0, 1)：無旋轉。
- iris：
  - position: (-0.0394, 0.0144, 0.1045)：
    - X: -0.0394 m（略向西）。
    - Y: 0.0144 m（略向北）。
    - Z: 0.1045 m（上方約 10.45 cm）。
  - orientation: (2.018e-05, 0.001029, -0.01611, 0.99987)：
    - 四元數表示微小旋轉。
    - 轉換為歐拉角（近似）：偏航約 -1.84°（繞 Z 軸），俯仰和滾轉幾乎為 0。
    - 結論：iris 略微偏離原點，姿態接近水平。
twist（線速度和角速度，相對于世界坐標系）：
- ground_plane 和 asphalt_plane：
  - linear: (0, 0, 0)：無線性速度（固定）。
  - angular: (0, 0, 0)：無角速度。
- iris：
  - linear: (-4.17e-06, -3.41e-06, -1.89e-06)：
    - 速度極小（微米/秒級別），幾乎靜止。
  - angular: (6.16e-05, -7.60e-05, -5.78e-08)：
    - 角速度也很小（弧度/秒），表明幾乎無旋轉。
  - 結論：iris 當前處于穩定狀態（可能是剛起飛或懸停）。

總結

世界坐標系：ground_plane 和 asphalt_plane 在 (0, 0, 0)，確認原點在地面中心。
iris 位置：位于世界坐標系的 (-0.0394, 0.0144, 0.1045)，略微偏離原點，高度 10.45 cm，微小偏航。

二、Gazebo GUI 中坐標軸顏色對應

操作

在 Gazebo 中，右鍵空白處，選擇 “View -> Wireframe” 或 “View -> Orbit”。
如果添加了坐標軸模型（例如通過 <include><uri>model://coordinate_frame</uri></include>），會顯示紅、綠、藍三條線。

顏色與 XYZ 的對應

紅色：X 軸。
- 世界坐標系：向東。
綠色：Y 軸。
- 世界坐標系：向北。
藍色：Z 軸。
- 世界坐標系：向上。
驗證：
- 在 Gazebo 中，地面平面（X-Y 平面）為灰色，Z 軸（藍色）垂直向上。
- 若添加坐標軸模型到 iris，紅線指向機頭（X），綠線指向右側（Y），藍線指向頂部（Z）。

三、rotor_0 在右前方，模型坐標系的 XYZ 方向

rotor_0 的位置

在 iris.sdf 中：

<link name='rotor_0'><pose>0.13 -0.22 0.023 0 0 0</pose>...
</link>

解析：
- x: 0.13：向前 0.13 m。
- y: -0.22：向左 0.22 m（Y 負方向）。
- z: 0.023：向上 0.023 m。
結論：相對于 base_link（模型坐標系原點），rotor_0 在 前方且偏左上方。
- 你提到“右前方”，可能是與實際定義不符，需澄清。

模型坐標系的 XYZ 方向

原點：base_link 的 <pose>0 0 0 0 0 0</pose>，即機身中心。
方向：
- X 軸：向前（正方向）。
  - rotor_0 的 x: 0.13 表示 X 正方向是機頭方向。
- Y 軸：向右（正方向）。
  - rotor_0 的 y: -0.22 表示向左（Y 負方向），因此 Y 正方向是右側。
- Z 軸：向上（正方向）。
  - rotor_0 的 z: 0.023 表示 Z 正方向是頂部。
澄清“右前方”：
- 根據 SDF，rotor_0 在左前方（x: 0.13, y: -0.22）。
- 若你認為它是“右前方”，可能是誤解或參照了其他旋翼（如 rotor_2：<pose>0.13 0.22 0.023 0 0 0</pose>，右前方）。

驗證

SDF 檢查：
- rotor_0: (0.13, -0.22, 0.023) → 左前方。
- rotor_2: (0.13, 0.22, 0.023) → 右前方。
Gazebo GUI：
- 啟動仿真，查看 iris 模型，確認旋翼位置。
- 若添加坐標軸模型，紅線（X）向前，綠線（Y）向右，藍線（Z）向上。

四、綜合分析

世界坐標系：
- 原點：(0, 0, 0)（地面中心）。
- 方向：X 東，Y 北，Z 上（紅綠藍對應）。
- iris 當前位置：(-0.0394, 0.0144, 0.1045)。
模型坐標系：
- 原點：base_link（機身中心）。
- 方向：X 前，Y 右，Z 上。
- rotor_0 在左前方（若你指“右前方”，可能是 rotor_2）。
關系：
- iris 的模型坐標系原點在世界坐標系的 (-0.0394, 0.0144, 0.1045)。
- rotor_0 世界坐標：(-0.0394 + 0.13, 0.0144 - 0.22, 0.1045 + 0.023) = (0.0906, -0.2056, 0.1275)。