diff --git a/.claude/skills/mujoco-cpp-build/SKILL.md b/.claude/skills/mujoco-cpp-build/SKILL.md new file mode 100644 index 0000000..8f499ed --- /dev/null +++ b/.claude/skills/mujoco-cpp-build/SKILL.md @@ -0,0 +1,16 @@ +--- +name: mujoco-cpp-build +description: Use when building C++ examples, compiling simulation executables, configuring CMake, or resolving C++ building errors in the MuJoCo learning repository. It requires the agent to actively prompt the user for build requirements (source vs release path, target directories). +--- + +# MuJoCo C++ Build + +This is the Claude Code project-local adapter for the canonical skill at: + +`../../../skills/mujoco-cpp-build/SKILL.md` + +When this skill is loaded, read and follow the canonical skill instructions: + +- `../../../skills/mujoco-cpp-build/SKILL.md` + +Use repository-relative tutorial paths only. Do not store user-specific paths in this adapter. diff --git a/.claude/skills/mujoco-engineering/SKILL.md b/.claude/skills/mujoco-engineering/SKILL.md new file mode 100644 index 0000000..ee848f4 --- /dev/null +++ b/.claude/skills/mujoco-engineering/SKILL.md @@ -0,0 +1,17 @@ +--- +name: mujoco-engineering +description: Use when an agent needs to implement, reproduce, debug, or extend MuJoCo projects using this repository's MJCF, Python, C++, ray casting, soft contact, sensors, rendering, viewer, force, or build examples. +--- + +# MuJoCo Engineering + +This is the Claude Code project-local adapter for the canonical skill at: + +`../../../skills/mujoco-engineering/SKILL.md` + +When this skill is loaded, read and follow the canonical skill instructions and its reference map: + +- `../../../skills/mujoco-engineering/SKILL.md` +- `../../../skills/mujoco-engineering/references/tutorial-map.md` + +Use repository-relative tutorial paths only. Do not store user-specific paths in this adapter. diff --git a/.claude/skills/mujoco-teaching/SKILL.md b/.claude/skills/mujoco-teaching/SKILL.md new file mode 100644 index 0000000..1ae3b1e --- /dev/null +++ b/.claude/skills/mujoco-teaching/SKILL.md @@ -0,0 +1,17 @@ +--- +name: mujoco-teaching +description: Use when a user wants to learn MuJoCo concepts, MJCF modeling, Python/C++ MuJoCo APIs, sensors, rendering, contact, soft contact, ray casting, or asks conceptual/tutorial questions about this repository's MuJoCo lessons. +--- + +# MuJoCo Teaching + +This is the Claude Code project-local adapter for the canonical skill at: + +`../../../skills/mujoco-teaching/SKILL.md` + +When this skill is loaded, read and follow the canonical skill instructions and its reference map: + +- `../../../skills/mujoco-teaching/SKILL.md` +- `../../../skills/mujoco-teaching/references/tutorial-map.md` + +Use repository-relative tutorial paths only. Do not store user-specific paths in this adapter. diff --git a/.gitignore b/.gitignore index 3ec7f79..c22a762 100644 --- a/.gitignore +++ b/.gitignore @@ -22,8 +22,21 @@ extend/equality/.cache/ extend/plugin/.cache/ +__pycache__/ +*.py[cod] +.venv/ +*.egg-info/ +build/ +dist/ +.pytest_cache/ +.ruff_cache/ +.mypy_cache/ +.uv/ +uv.lock +skills/*/.local/ + extend/soft_contact/C++/build/ extend/soft_contact/C++/.cache/ extend/touch/C++/build/ -extend/touch/C++/.cache/ \ No newline at end of file +extend/touch/C++/.cache/ diff --git a/.opencode/skills/mujoco-cpp-build/SKILL.md b/.opencode/skills/mujoco-cpp-build/SKILL.md new file mode 100644 index 0000000..700aa06 --- /dev/null +++ b/.opencode/skills/mujoco-cpp-build/SKILL.md @@ -0,0 +1,18 @@ +--- +name: mujoco-cpp-build +description: Use when building C++ examples, compiling simulation executables, configuring CMake, or resolving C++ building errors in the MuJoCo learning repository. It requires the agent to actively prompt the user for build requirements (source vs release path, target directories). +metadata: + canonical-source: skills/mujoco-cpp-build +--- + +# MuJoCo C++ Build + +This is the OpenCode project-local adapter for the canonical skill at: + +`../../../skills/mujoco-cpp-build/SKILL.md` + +When this skill is loaded, read and follow the canonical skill instructions: + +- `../../../skills/mujoco-cpp-build/SKILL.md` + +Use repository-relative tutorial paths only. Do not store user-specific paths in this adapter. diff --git a/.opencode/skills/mujoco-engineering/SKILL.md b/.opencode/skills/mujoco-engineering/SKILL.md new file mode 100644 index 0000000..d5326c6 --- /dev/null +++ b/.opencode/skills/mujoco-engineering/SKILL.md @@ -0,0 +1,19 @@ +--- +name: mujoco-engineering +description: Use when an agent needs to implement, reproduce, debug, or extend MuJoCo projects using this repository's MJCF, Python, C++, ray casting, soft contact, sensors, rendering, viewer, force, or build examples. +metadata: + canonical-source: skills/mujoco-engineering +--- + +# MuJoCo Engineering + +This is the OpenCode project-local adapter for the canonical skill at: + +`../../../skills/mujoco-engineering/SKILL.md` + +When this skill is loaded, read and follow the canonical skill instructions and its reference map: + +- `../../../skills/mujoco-engineering/SKILL.md` +- `../../../skills/mujoco-engineering/references/tutorial-map.md` + +Use repository-relative tutorial paths only. Do not store user-specific paths in this adapter. diff --git a/.opencode/skills/mujoco-teaching/SKILL.md b/.opencode/skills/mujoco-teaching/SKILL.md new file mode 100644 index 0000000..bad336b --- /dev/null +++ b/.opencode/skills/mujoco-teaching/SKILL.md @@ -0,0 +1,19 @@ +--- +name: mujoco-teaching +description: Use when a user wants to learn MuJoCo concepts, MJCF modeling, Python/C++ MuJoCo APIs, sensors, rendering, contact, soft contact, ray casting, or asks conceptual/tutorial questions about this repository's MuJoCo lessons. +metadata: + canonical-source: skills/mujoco-teaching +--- + +# MuJoCo Teaching + +This is the OpenCode project-local adapter for the canonical skill at: + +`../../../skills/mujoco-teaching/SKILL.md` + +When this skill is loaded, read and follow the canonical skill instructions and its reference map: + +- `../../../skills/mujoco-teaching/SKILL.md` +- `../../../skills/mujoco-teaching/references/tutorial-map.md` + +Use repository-relative tutorial paths only. Do not store user-specific paths in this adapter. diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..8982557 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,24 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/albusgive2/unitree_ros2/cyclonedds_ws/install/unitree_hg/include/**", + "/home/albusgive2/unitree_ros2/cyclonedds_ws/install/unitree_go/include/**", + "/home/albusgive2/unitree_ros2/cyclonedds_ws/install/unitree_api/include/**", + "/home/albusgive2/rlfw/rlfw_interface/install/rlfw_msgs/include/**", + "/opt/ros/humble/include/**", + "/usr/include/**" + ], + "name": "ros2", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++17" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index abc5eaa..28470a2 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -2,5 +2,21 @@ "cmake.sourceDirectory": "/home/albusgive2/mujoco_learning/CPP/Chapter1-make", "python-envs.defaultEnvManager": "ms-python.python:conda", "python-envs.defaultPackageManager": "ms-python.python:conda", - "python-envs.pythonProjects": [] + "ROS2.distro": "humble", + "python.autoComplete.extraPaths": [ + "/home/albusgive2/unitree_ros2/cyclonedds_ws/install/unitree_hg/local/lib/python3.10/dist-packages", + "/home/albusgive2/unitree_ros2/cyclonedds_ws/install/unitree_go/local/lib/python3.10/dist-packages", + "/home/albusgive2/unitree_ros2/cyclonedds_ws/install/unitree_api/local/lib/python3.10/dist-packages", + "/home/albusgive2/rlfw/rlfw_interface/install/rlfw_msgs/local/lib/python3.10/dist-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/home/albusgive2/unitree_ros2/cyclonedds_ws/install/unitree_hg/local/lib/python3.10/dist-packages", + "/home/albusgive2/unitree_ros2/cyclonedds_ws/install/unitree_go/local/lib/python3.10/dist-packages", + "/home/albusgive2/unitree_ros2/cyclonedds_ws/install/unitree_api/local/lib/python3.10/dist-packages", + "/home/albusgive2/rlfw/rlfw_interface/install/rlfw_msgs/local/lib/python3.10/dist-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] } \ No newline at end of file diff --git a/CPP/Chapter3-get_obj/tutorial.md b/CPP/Chapter3-get_obj/tutorial.md index c1fa30a..df0d974 100644 --- a/CPP/Chapter3-get_obj/tutorial.md +++ b/CPP/Chapter3-get_obj/tutorial.md @@ -1,23 +1,107 @@ -# get obj -## 获取名字 -数量 -![](../../MJCF/asset/entity.png) -**先看mjModel结构体开头部分，这里的命名方式都是nXXX，这代表各个元素的数量** -![](../../MJCF/asset/names.png) -## 获取id -`MJAPI int mj_name2id(const mjModel* m, int type, const char* name);` -**通过name获取实体的id -m :mjModel -type: mjmodel.h文件中的mjtObj中定义，这个是要获取id的实体类型一下是部分type类型枚举，在mjtObj中找到 -name: name -** -![](../../MJCF/asset/enum_mjtobj.png) -## 获取位置 -![](../../MJCF/asset/xpos.png) -**可以通过 xpos和xxx_xpos获取各个对象的位置** -## 获取姿态 -**通过xquat可以获取body的姿态** +# 获取仿真世界中的实体信息 (Get Object Info) +在 MuJoCo 中，获取仿真世界中各种实体（Body, Joint, Geom 等）的数量、ID、位置及姿态是非常高频的操作。以下是实现逻辑与核心数据结构。 +--- +## 1. 获取实体数量与名称 (Get Counts & Names) +所有的数量与命名信息都存储在 `mjModel` 结构体中。 +### 1.1 实体数量定义 +在 `mjModel` 的开头，定义了所有仿真元素的数量（以 `n` 开头）： +```cpp +// 摘自 mujoco/mjmodel.h: 实体数量声明 +int nbody; // 身体 (Body) 数量 +int njnt; // 关节 (Joint) 数量 +int ngeom; // 几何体 (Geom) 数量 +int nsite; // 标记点 (Site) 数量 +int ncam; // 相机 (Camera) 数量 +int nlight; // 光源 (Light) 数量 +int nmesh; // 网格 (Mesh) 数量 +int nsensor; // 传感器 (Sensor) 数量 +int nactuator; // 驱动器 (Actuator) 数量 +``` +* 源码链接：[mujoco/mjmodel.h (sizes段)](https://github.com/google-deepmind/mujoco/blob/main/include/mujoco/mjmodel.h) + +### 1.2 实体名字缓冲区 +在 `mjModel` 中，名字均以一维字符缓冲区存储，并通过对应的地址数组记录每个实体的名字指针偏置： +```cpp +// 摘自 mujoco/mjmodel.h: 命名缓冲区 +char* names; // 名字缓存区，字符大数组 (nnames x 1) +int* name_bodyadr; // 各 Body 名字在 names 中的起始索引 (nbody x 1) +int* name_jntadr; // 各 Joint 名字在 names 中的起始索引 (njnt x 1) +int* name_geomadr; // 各 Geom 名字在 names 中的起始索引 (ngeom x 1) +int* name_sensoradr; // 各 Sensor 名字在 names 中的起始索引 (nsensor x 1) +``` +* 源码链接：[mujoco/mjmodel.h (names段)](https://github.com/google-deepmind/mujoco/blob/main/include/mujoco/mjmodel.h) + +--- + +## 2. 获取实体 ID (Get ID by Name) +因为 MuJoCo 底层物理计算全部基于数组索引（即 ID），在 API 中我们通常需要通过名字（string）获取其 ID。 +使用 API 函数： +```cpp +MJAPI int mj_name2id(const mjModel* m, int type, const char* name); +``` + +### 2.1 参数说明 +* `m`: `mjModel` 指针。 +* `type`: 实体的类型，由枚举类型 `mjtObj` 定义。 +* `name`: 实体的名称。 + +### 2.2 实体类型枚举 `mjtObj` +`mjtObj` 定义在 `mjmodel.h` 中，代表各种可能的实体类型： +```cpp +typedef enum mjtObj_ { + mjOBJ_UNKNOWN = 0, // 未知对象类型 + mjOBJ_BODY, // 身体 (Body) + mjOBJ_XBODY, // Body的替代表示，用于读取常规参考系 (而非惯性系) + mjOBJ_JOINT, // 关节 (Joint) + mjOBJ_DOF, // 自由度 (Degrees of freedom) + mjOBJ_GEOM, // 几何体 (Geom) + mjOBJ_SITE, // 标记点 (Site) + mjOBJ_CAMERA, // 相机 (Camera) + mjOBJ_LIGHT, // 光源 (Light) + mjOBJ_FLEX, // 柔性可变形体 (Flex) + mjOBJ_MESH, // 三角网格 (Mesh) + mjOBJ_SKIN, // 皮肤网格 (Skin) + mjOBJ_HFIELD, // 高度图 (Heightfield) + mjOBJ_TEXTURE, // 纹理 (Texture) + mjOBJ_MATERIAL, // 材质 (Material) + mjOBJ_PAIR, // 特殊碰撞 geom 对 (Pair) + mjOBJ_SENSOR // 传感器 (Sensor) +} mjtObj; +``` +* 源码链接：[mujoco/mjmodel.h (mjtObj定义)](https://github.com/google-deepmind/mujoco/blob/main/include/mujoco/mjmodel.h) + +演示代码： +```cpp +int body_id = mj_name2id(model, mjOBJ_BODY, "robot_link1"); +``` + +--- + +## 3. 获取位置与姿态 (Get Pos & Ori) +物体的实时笛卡尔空间状态（三维位置、姿态矩阵等）全部存储在 **`mjData`** 中，由求解器在 `mj_step` 时进行实时更新更新： + +```cpp +// 摘自 mujoco/mjdata.h: 笛卡尔坐标状态 +mjtNum* xpos; // 各个 Body 的三维笛卡尔中心坐标 (nbody x 3) +mjtNum* xquat; // 各个 Body 的笛卡尔四元数旋转姿态 (nbody x 4) +mjtNum* xmat; // 各个 Body 的笛卡尔旋转矩阵姿态 (nbody x 9) +mjtNum* xipos; // 各个 Body 质心 (com) 的笛卡尔坐标 (nbody x 3) +mjtNum* ximat; // 各个 Body 质心的笛卡尔旋转矩阵 (nbody x 9) +``` +* 源码链接：[mujoco/mjdata.h (Cartesian state)](https://github.com/google-deepmind/mujoco/blob/main/include/mujoco/mjdata.h) + +演示代码： +```cpp +// 获取特定 body 实时三维位置坐标 +int body_id = mj_name2id(model, mjOBJ_BODY, "support"); +if (body_id != -1) { + double x = data->xpos[body_id * 3 + 0]; + double y = data->xpos[body_id * 3 + 1]; + double z = data->xpos[body_id * 3 + 2]; + std::cout << "Body position: " << x << ", " << y << ", " << z << std::endl; +} +``` diff --git a/CPP/Chapter4-sensor_data/tutorial.md b/CPP/Chapter4-sensor_data/tutorial.md index 031a499..e86a6a3 100644 --- a/CPP/Chapter4-sensor_data/tutorial.md +++ b/CPP/Chapter4-sensor_data/tutorial.md @@ -1,9 +1,15 @@ # 传感器数据获取 -![](../../MJCF/asset/sensor_adr.png) -**sensordata的索引需要依靠mjData的sensor_adr获取，这个可以使用sensor的id** -**这个我们要注意传感器具有的数据量，有的传感器是一个值，而有的传感器是三个值。我们可以使用mjModel中的sensor_dim获得传感器输出的参数量** -*演示：* +**`sensordata` 的索引需要依靠 `mjModel` 中的 `sensor_adr` 获取，也可以使用 sensor 的 id。** + +**我们要注意传感器具有的数据量，有的传感器是一个值，而有的传感器是三个值。我们可以使用 `mjModel` 中的 `sensor_dim` 获得传感器输出的参数量。** + +* `mjModel.sensor_adr` 的定义类似于： +```cpp +int* sensor_adr; // address in sensor array (nsensor x 1) +``` + +演示： ```C++ std::vector get_sensor_data(const mjModel *model, const mjData *data, @@ -21,8 +27,24 @@ std::vector get_sensor_data(const mjModel *model, const mjData *data, return sensor_data; } ``` -对于传感器其他的属性在 mjModel中可以直接获得。如下： -![](../../MJCF/asset/modelsensors.png) + +对于传感器的其他属性，在 `mjModel` 中可以直接获得： +```cpp +// mjModel 中的传感器相关成员变量说明 +int* sensor_type; // 传感器类型 (mjtSensor) (nsensor x 1) +int* sensor_datatype; // 数值数据类型 (mjtDataType) (nsensor x 1) +int* sensor_needstage; // 所需 Jun 算阶段 (mjtStage) (nsensor x 1) +int* sensor_objtype; // 被测物体的类型 (mjtObj) (nsensor x 1) +int* sensor_objid; // 被测物体的 ID (nsensor x 1) +int* sensor_reftype; // 参考系物体的类型 (mjtObj) (nsensor x 1) +int* sensor_refid; // 参考系物体的 ID; -1: 全局参考系 (nsensor x 1) +int* sensor_dim; // 标量输出的数量 (nsensor x 1) +int* sensor_adr; // 传感器数据在 sensordata 中的地址 (nsensor x 1) +mjtNum* sensor_cutoff; // 实数/正数的截止值; 0: 忽略 (nsensor x 1) +mjtNum* sensor_noise; // 噪声音频标准差 (nsensor x 1) +mjtNum* sensor_user; // 用户数据 (nsensor x nuser_sensor) +int* sensor_plugin; // 插件实例 ID; -1: 非插件 (nsensor x 1) +``` ### 读取相机画面   相机来源一般是在模型文件中创建相机，或者创建一个相机手动控制，就像 base中 diff --git a/CPP/Chapter5-draw/tutorial.md b/CPP/Chapter5-draw/tutorial.md index 0c3f2eb..a2d1e66 100644 --- a/CPP/Chapter5-draw/tutorial.md +++ b/CPP/Chapter5-draw/tutorial.md @@ -1,13 +1,45 @@ # 3D绘制   mujoco提供显示基础几何体和mujoco提供的一些特殊渲染几何体。查看文档可知mjv_initGeom函数能在渲染场景中增加几何体，mjv_connector可以用mjv_initGeom初始化的几何体绘制提供的一些特殊形状（如箭头，直线等）。mujoco显示画面的原理是通过mjv_updateScene 将仿真数据储存到mjvScene中，这是已经处理好的几何数据，接下来使用mjr_render传递给opengl渲染。我们在绘制过程中是要在仿真的几何数据处理完之后，加入绘制信息，再交给opengl渲染。   在mjvScene中添加信息，其实是直接在mjvScene的geoms后面续写，而且要增加ngeom长度。这里通过注释可以理解，mjvScene根据ngeom确定几何体数量再从geoms中获取资源。 -![](../../MJCF/asset/mjvScene.png) + +```c +struct _mjvScene { + // abstract geoms + int maxgeom; // size of allocated geom buffer + int ngeom; // number of geoms currently in buffer + mjvGeom* geoms; // buffer for geoms + int* geomorder; // buffer for ordering geoms by distance to camera + // ... +}; +typedef struct _mjvScene mjvScene; +``` + mjv_initGeom函数原型： -![](../../MJCF/asset/initGeom.png) + +```c +void mjv_initGeom(mjvGeom* geom, int type, const mjtNum size[3], + const mjtNum pos[3], const mjtNum mat[9], const float rgba[4]); +``` + mjv_connector函数原型： -![](../../MJCF/asset/mjv_connector.png) + +```c +void mjv_connector(mjvGeom* geom, int type, mjtNum width, + const mjtNum from[3], const mjtNum to[3]); +``` + geom是传入的仅绘制的几何体，需要使用mjv_initGeom初始化，type见下面，width是绘制的宽度，这个是对于渲染出来的画面的宽度，from起点，to终点 -![](../../MJCF/asset/mjtGeom.png) + +```c +typedef enum _mjtGeom { + // ... + mjGEOM_ARROW = 100, // 箭头 + mjGEOM_ARROW1, // 无楔形箭头 + mjGEOM_ARROW2, // 双向箭头 + mjGEOM_LINE, // 直线 + // ... +} mjtGeom; +``` 这里是可以绘制的几何形状类型，分别是箭头，无楔形箭头，双向箭头，直线。 *演示——绘制几何体函数：* @@ -71,9 +103,20 @@ mjr_render(viewport, &scn, &con); # 2D绘制 字体尺寸的初始化： -![](../../MJCF/asset/font_size.png) -查阅文档我们可知2D绘制要在mjr_render之后进行 -![](../../MJCF/asset/2D_draw_point.png) + +```c +typedef enum _mjtFontScale { + mjFONTSCALE_50 = 50, // 50% 缩放，适用于低分辨率渲染 + mjFONTSCALE_100 = 100, // 100% 缩放，默认普通比例 + mjFONTSCALE_150 = 150, // 150% 缩放 + mjFONTSCALE_200 = 200 // 200% 缩放 +} mjtFontScale; +``` + +查阅文档我们可知，2D 绘制函数需要在 `mjr_render` 之后进行调用： + +> [!IMPORTANT] +> `mjr_render` 会在开始渲染时清除视口（Viewport）。因此，所有自定义的 2D 绘制操作（如绘制文本、矩形、表格或标签等）必须在调用 `mjr_render` **之后** 进行，否则绘制的内容会被视口清除覆盖。 *绘制文本：* diff --git a/CPP/Chapter6-force/tutorial.md b/CPP/Chapter6-force/tutorial.md index b7bfd3e..6958f0d 100644 --- a/CPP/Chapter6-force/tutorial.md +++ b/CPP/Chapter6-force/tutorial.md @@ -1,22 +1,47 @@ # 作用力 **平动** + $$ m \cdot a = F $$ + **旋转** + $$ I \cdot \alpha = \tau $$ + $$ F/\tau = 外部力+驱动力+被动力+约束力+偏置力 $$ # 外部力 我们查看文档计算部分可以看到有qfrc_passive,qfrc_actuator,qfrc_applied三个力分别对应被动力，驱动力，外部力 -![](../../MJCF/asset/force.png) +```c +mjtNum* qfrc_applied; // applied generalized force (nv x 1) +mjtNum* qfrc_passive; // passive generalized force (nv x 1) +mjtNum* qfrc_actuator; // actuator generalized force (nv x 1) +``` + 只要看手册的api或者头文件中，找到mj_applyFT函数应用外部力。 -![](../../MJCF/asset/forcefunc1.png) + +```c +void mj_applyFT(const mjModel* m, mjData* d, + const mjtNum force[3], const mjtNum torque[3], + const mjtNum point[3], int body, + mjtNum* qfrc_target); +``` + 或者还可以使用xfrc_applied直接作用外部力在质心上。 -![](../../MJCF/asset/frc_.png) + +```c +mjtNum* xfrc_applied; // applied Cartesian forces (nbody x 6) +``` + **这里也说明了是笛卡尔力。**   mj_applyFT函数的参数，是三维的力，三维扭矩，三维坐标(worldbody坐标系)，bodyid。qfrc_target可以直接使用d->qfrc_applied。mj_applyFT是对于body在 **“自由度”** 上施加力。于是我们可以使用两个方式对body施加外部力。   qfrc_target还可以是以下这些被动力等qfrc_xxx的力 -![](../../MJCF/asset/mj_passive.png) + +```c +mjtNum* qfrc_passive; // passive generalized force (nv x 1) +mjtNum* qfrc_bias; // constraint bias generalized force (nv x 1) +mjtNum* qfrc_constraint; // constraint generalized force (nv x 1) +``` **mj_Data接口演示（作用在质心上）：** ```C++ @@ -35,29 +60,65 @@ mj_applyFT(m, d, force, torque, point, id, d->qfrc_applied); **mj_applyFT每次调用都是增量式，如果我们想清除力可以使用mju_zero，如mju_zero(d->qfrc_applied, m->nv);** # 驱动力 -![](../../MJCF/asset/efc_force.png) +```c +mjtNum* qfrc_actuator; // actuator generalized force (nv x 1) +```   mjData.qfrc_actuator是驱动器执行的力，不同驱动器最终会计算出力或者扭矩作用到关节上。 # 被动力   mjData.qfrc_passive是被动力，关节参数的damping，stiffness,摩擦力，流体阻力都会最终计算到改力中。 + $$ damping_force = (0-qvel)*damping $$ + $$ stiffness_force = (0-qpos)*stiffness $$ # 约束力 +```c +mjtNum* efc_force; // constraint force (nefc x 1) +```   mjData.efc_force是约束力，关节的frictionloss，equality计算出来的合力为改力。 -  jar = Jac*qacc-aref 残差=雅可比*关节加速度-参考伪加速度 +  这里 $jar_0 = Jac \cdot qacc_0 - aref$ 表示无约束时的约束空间残差（未施加摩擦力时的相对加速度偏离量），$InverseConstraintMass$ 是该约束维度的质量倒数（即惯量响应矩阵 $A = J M^{-1} J^T + R$ 的对应对角元素）。 + +关节干摩擦力（静摩擦与滑动摩擦）的完整解算公式为： $$ -frictionlossforce = +frictionloss\_force = \begin{cases} - frictionloss, & \text{if } jar <= -InverseConstraintMass ⋅ floss \\ - -frictionloss, & \text{else if } jar>=InverseConstraintMass ⋅ floss \\ - （没看懂）, & \text{else } + frictionloss, & \text{if } jar_0 \le -InverseConstraintMass \cdot floss & \text{(正向滑动摩擦)} \\ + -frictionloss, & \text{else if } jar_0 \ge InverseConstraintMass \cdot floss & \text{(反向滑动摩擦)} \\ + -\frac{jar_0}{InverseConstraintMass}, & \text{else } & \text{(静摩擦/粘滞状态，此时实际残差 } jar = 0\text{)} \end{cases} $$ + +**原理解析**： +1. **滑动摩擦阶段**：当外力推动关节的趋势（即 $jar_0$）超过了最大静摩擦力所能提供的阻碍加速度时，摩擦力饱和，大小恒为最大摩擦力 $floss$（即 `frictionloss`），方向与相对运动趋势相反。 +2. **静摩擦（粘滞）阶段**：当外力较小，在摩擦力能平衡的范围内（即 `else` 分支），摩擦力会自动产生一个恰好抵消相对运动趋势的力 $-\frac{jar_0}{InverseConstraintMass}$，使得最终的实际加速度残差 $jar$ 完美归零，即物体保持静止或相对匀速运动。 **源码实现(SRC/engine/engine_core_constraint.c)** -![](../../MJCF/asset/frictionloss.png) +```c +// linear friction limits +else if (floss && floss[i] > 0) { + // linear negative friction (正向滑动摩擦) + if (jar[i] <= -R[i] * floss[i]) { + force[i] = floss[i]; + state[i] = mjCNSTRSTATE_LINEARNEG; + } + + // linear positive friction (反向滑动摩擦) + else if (jar[i] >= R[i] * floss[i]) { + force[i] = -floss[i]; + state[i] = mjCNSTRSTATE_LINEARPOS; + } + + // quadratic/inactive friction (静摩擦/粘滞状态) + else { + force[i] = -jar[i] / R[i]; + state[i] = mjCNSTRSTATE_QUADRATIC; + } +} +``` # 偏置力 -![](../../MJCF/asset/qfrc_bias.png) +```c +mjtNum* qfrc_bias; // constraint bias generalized force (nv x 1) +```   mjData.qfrc_bias科里奥利力等，由引擎自动计算。 \ No newline at end of file diff --git a/CPP/Chapter8-ray/tutorial.md b/CPP/Chapter8-ray/tutorial.md index 0c8e218..2a2fd6f 100644 --- a/CPP/Chapter8-ray/tutorial.md +++ b/CPP/Chapter8-ray/tutorial.md @@ -1,18 +1,53 @@ -# Ray -射线测距接口 -![](../../MJCF/asset/ray.png) +# 射线检测 (Ray Casting) + +射线检测（Ray Casting）常用于模拟激光雷达、超声波测距传感器，或者进行视线、碰撞预判。在仿真运行到 `mj_kinematics` 或 `mj_fwdPosition` 之后方可调用（因为射线检测依赖实体的实时位置信息）。 + +--- + +## 1. 核心 API 说明 + +### 1.1 多束射线检测 `mj_multiRay` +在一处起点发射多条不同方向的射线，常用于激光雷达雷达阵列： +```cpp void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, int* geomid, mjtNum* dist, int nray, mjtNum cutoff); -* m: mjModel -* d: mjData -* pnt: 射线起点 -* vec: 射线向量 -* geomgroup：检测分组，NULL代表检测所有 -* flg_static: 是否检测静态物体 1检测 0不检测 -* bodyexclude: -1 可用于指示包括所有主体 -* geomid: 检测到的几何体id，没有为-1 -* dist: 到geom表面的距离，数据是距离比例，是vec的倍数，无限远为-1 -* nray：射线数量 -* cutoff: 距离截断，超过截断距离的射线不检测，判断条件(pnt到geompos中心>cutoff+几何体外接球半径) -**其余ratXXX参数同理** \ No newline at end of file +``` + +### 1.2 单条射线检测 `mj_ray` +检测单个射线的交点： +```cpp +mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], + const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, + int geomid[1]); +``` + +### 1.3 其他特定几何类型检测 +* `mj_rayHfield`: 与高度图进行求交检测。 +* `mj_rayMesh`: 与三角网格模型（Mesh）求交检测。 +* `mju_rayGeom`: 用于纯几何计算（不受当前世界状态约束，计算指定位置尺寸的 geom）。 + +--- + +## 2. 关键参数详解 + +* **`pnt`** (`const mjtNum[3]`): 射线发射起点的 3D 笛卡尔坐标。 +* **`vec`** (`const mjtNum*`): 射线方向向量。 + * 对于 `mj_multiRay`，其为长度为 `nray * 3` 的数组，每 3 个元素代表一条射线的方向向量。 +* **`geomgroup`** (`const mjtByte*`): 几何体分组使能数组（长度为 `mjNGROUP`，通常为 5）。 + * 设为 `NULL` 代表检测所有 Geom 分组。若不为 `NULL`，则只有对应位为 1 的 Geom 组才会被检测。 +* **`flg_static`** (`mjtByte`): 是否检测静态（Static）物体。`1` 代表检测静态物体，`0` 代表忽略静态物体。 +* **`bodyexclude`** (`int`): 需要排除检测的 Body ID。 + * 设为 `-1` 代表检测所有 Body。 +* **`geomid`** (`int*`): 传出参数。检测到相交的 Geom ID。若未击中任何物体，则返回 `-1`。 +* **`dist`** (`mjtNum*`): 传出参数。到 Geom 表面的距离比例值。 + * **重要**：返回值不是绝对距离（单位米），而是**方向向量 `vec` 的缩放倍数**。即实际碰撞点坐标为： + $$pos_{hit} = pnt + dist \times vec$$ + * 若未发生碰撞，返回值为 `-1`。 +* **`nray`** (`int`): `mj_multiRay` 批量发射的射线总数。 +* **`cutoff`** (`mjtNum`): 距离截断阈值。如果起点到几何体中心点的距离大于 `cutoff` 加上该几何体的包围球半径，则直接跳过检测以提升性能。 + +--- + +## 3. 官方文档入口 +* [MuJoCo 官方 API 射线检测 (Ray Collisions)](https://mujoco.readthedocs.io/en/latest/programming.html#ray-collisions) \ No newline at end of file diff --git a/MJCF/Chapter2-virtual_world/tutorial.md b/MJCF/Chapter2-virtual_world/tutorial.md index cdf0683..2d6efeb 100644 --- a/MJCF/Chapter2-virtual_world/tutorial.md +++ b/MJCF/Chapter2-virtual_world/tutorial.md @@ -21,13 +21,29 @@ density="1.225" viscosity="1.8e-5"/> * magnetic="0 -0.5 0"世界磁场，影响磁力传感器 * density 介质密度，水，空气等，单位kg/m³ * viscosity 介质粘度 -* integrator= [Euler/RK4/implicit/implicitfast]积分器，默认欧拉，用于仿真世界每步就求解计算，各个优势见下图 -* solver= [PGS, CG, Newton]求解器配置，默认牛顿 -* iterations="100" 约束求解器最大迭代次数,按需配置。 -求解器其他配置： -![](../asset/slove.png) -积分器比较： -![](../asset/integrator.png) +* **integrator** (`[Euler/RK4/implicit/implicitfast]`): 积分器，默认欧拉（Euler），用于每个仿真步长的物理求解。各自特点如下： + * `Euler`: 简单快速，但精度低，适用于快速测试或简单系统。 + * `RK4`: 精度高，适用于对精度有要求但计算量不敏感的场景。 + * `Implicit`: 隐式积分，适合刚性系统（Stiff systems）和稳定性要求高的场景，但计算复杂度较高。 + * `ImplicitFast`: 在保证稳定性的前提下加快计算速度，适用于大规模复杂仿真。 +* **solver** (`[PGS, CG, Newton]`): 约束求解器类型，默认牛顿法（Newton）。 +* **iterations** (`"100"`): 约束求解器的最大迭代次数，按需配置。 + +#### 求解器细分配置参数 (Solver Attributes) +当需要对求解精度、性能进行更微观的调优时，可以在 `