1、问题概述

训练后的舞蹈模型部署到实体机器人上，机器人快速收回手臂至胸前的动作，手指会和身体发生碰撞，从而造成灵巧手损坏。本文将围绕如何优化肢体碰撞问题进行展开。

训练后模型replay效果录屏：

训练后模型replay效果截图：

右手碰撞	左手碰撞

实机部署：

2、训练端优化

训练代码下载：

git clone https://gitee.com/leju-robot/LejuLab-Train.git

2.1、修改几何结构模型

2.1.1、几何结构模型引用

首先确认训练环境所引用的机器人几何模型定义位置，如下是LejuLab-Train代码中相关引用关系。

source/leju_robot/leju_robot/tasks/tracking/config/robanS14/dance/__init__.py
->
source/leju_robot/leju_robot/tasks/tracking/config/robanS14/dance/tracking_env_cfg.py
->
source/leju_robot/leju_robot/tasks/tracking/config/robanS14/dance/robanS14.py
->
source/leju_robot/leju_robot/assets/leju.py

leju.py中关于几何结构模型相关定义如下。

asset_path=f"{ASSET_DIR}/robanS14/urdf/robanS14_mini_col.urdf"

通过定义可知训练仿真环境引入的机器人几何结构模型文件路径为

source/leju_robot/leju_robot/assets/robots/robanS14/urdf/robanS14_mini_col.urdf

2.1.2、urdf可视化

安装MuJoCo 官方 Python 绑定库自带的交互式可视化工具，安装命令如下。

pip install mujoco

以独立应用方式启动mujoco可视化窗口

python -m mujoco.viewer

加载urdf文件，拖动urdf文件到mujoco窗口即可查看可视化查看模型。

2.1.3、urdf修改

在urdf文件中搜索arm相关定义，找到手臂末端关节<collision>标签定义的碰撞几何体。相关定义如下。

<link name="zarm_l4_link">
    <!--省略部分代码-->
      <geometry>
        <!-- 手部精细模型 -->
        <mesh filename="../meshes/l_forearm_hand.STL" />
       <!--省略部分代码-->
    <!--手部碰撞体-->
    <collision>
      <origin xyz="0 0 -0.225" rpy="0 0 0" />
      <geometry>
        <capsule radius="0.03" length="0.07" />
      </geometry>
    </collision>
  </link>

上述urdf定义的几何结构效果如下。

模型训练时，碰撞检测使用的是简化的几何形状，而不是视觉网格的精确形状，以提高计算效率。图片中可以看到，胶囊碰撞体未包含手部精细模型，因此模型训练时，手指和身体的碰撞可能未被检测。

增大左手碰撞体尺寸，同时新增碰撞体以包含大拇指,效果如下。

修改urdf手部碰撞体尺寸后，进行模型训练。下图训练曲线中whole_body_tracking_01代码未修改手部碰撞体；whole_body_tracking_02代码增大手部碰撞体尺寸，同时新增胶囊体包含大拇指。

通过训练曲线可以看出，增大碰撞体尺寸，手部碰撞惩罚值也大幅增大。说明未修改碰撞体尺寸之前，存在手部碰撞未被检测和计算。

训练replay效果：

replay效果可以看出，增加手部碰撞体尺寸后，手指和胸口未发生碰撞。

2.2、调整奖励函数

LejuLab-Train代码关于手部接触力奖励项启用定义在source/leju_robot/leju_robot/tasks/tracking/config/robanS14/dance/tracking_env_cfg.py文件中，定义如下

hand_contact_forces = RewTerm(
        func=mdp.body_contact_forces,
        weight=-1, #权重
        params={
            "sensor_cfg": SceneEntityCfg(
                "contact_forces",
                body_names=["zarm_l4_link", "zarm_r4_link"],
            ),
            "force_threshold": 10.0,    #接触力阈值
        },
    )

增大weight和减小force_threshold，可以增大训练时对手部和身体碰撞的惩罚力度。需要注意force_threshold参与惩罚值计算，如果设置太小的话，会导致惩罚值太高。总奖励值为负数，从而终止训练。

手部接触力奖励项计算函数定义在source/leju_robot/leju_robot/tasks/tracking/mdp/rewards.py文件中，force_threshold相关计算代码如下。

(注：本案例对奖励函数做了一定程度修改)

force_velocity_product = contact_force_norm * velocity_norm  # (num_envs, num_feet)
# 对超过阈值的部分进行惩罚，然后对所有脚求和
violation = torch.clamp(force_velocity_product - force_velocity_threshold, min=0.0)  # (num_envs, num_feet)
reward = torch.sum(violation / force_velocity_threshold, dim=1)  # (num_envs,)
return reward
#force_velocity_product < force_velocity_threshold时，violation值为0,返回reward值也为0

基于whole_body_tracking_02代码基础上，修改force_threshold从10.0减小为1.0时，训练曲线如下,对应图中绿色曲线，平均奖励值为负数，然后训练终止。

基于whole_body_tracking_02代码基础上，修改weight从-1.0到-5.0。训练曲线如下，对应图中紫色曲线，惩罚值明显增加，负值更小。

训练replay效果：

replay效果可以看出，增加weight权重值后，手指和胸口未发生碰撞，同时手指和胸口保持一定距离。

3、部署端优化

模型训练完后，需要先sim2sim仿真部署测试后，再sim2real部署到实机。因此需要尽可能的在仿真中发现问题，所以在仿真中，应当减小碰撞体尺寸，观察手部和身体是否碰撞。

部署代码下载：

git clone https://gitee.com/leju-robot/LejuLab-Deploy.git

3.1、仿真部署

3.1.1、几何结构模型引用

首先确认mujoco仿真环境所引用的机器人几何模型定义位置，如下是LejuLab-Deploy代码中相关引用关系。

src/leju_launch/launch/load_mujoco_sim.launch
->
src/leju_assets/models/biped_s14/xml/scene.xml
->
src/leju_assets/models/biped_s14/xml/biped_s14.xml

3.1.2、mujoco仿真

按照LejuLab-Deploy仓库的README.md文档操作，启动mujoco仿真。

如下图所示，mujuco加载xml模型文件后，机器人精细模型为白色，碰撞体为红色部分。

碰撞体过小	碰撞体过大

左图中手指和身体发生碰撞；右边图中由于碰撞体尺寸设置过大，可以看到碰撞体间有相互作用力箭头。为了能够直观查看训练出的模型是否存在碰撞问题，应该减小碰撞体尺寸，通过模型穿模反映碰撞，如左边图所示。

3.2、实机部署

mujoco仿真部署测试后，没有明显碰撞再部署到实机上测试。因为实机的关节电机和结构件，存在不同程度的磨损与变形，所以仿真和环境存在差异，实机之间也存在差异。部署实机之后，需要对参数进行调试优化。

3.2.1、优化控制参数

LejuLab-Deploy仓库中控制器相关配置文件路径如下。

(注：本案例碰撞问题是其他舞蹈，这里仅以config_mimic_HPNY_dance为例，讲解控制器配置文件路径)

src/leju-controllers/leju-rl-controller/config/14/controller_manager.yaml
->
src/leju-controllers/leju-rl-controller/config/14/config_mimic_HPNY_dance.yaml

主要调试的三个参数分别为KP、KD、action_scale。

参数项	作用	调试建议
kp	比例系数 (追踪目标值)	KP高：跟踪更紧、更硬，碰撞时冲击更大。 KP低：更软，能减冲击，但动作可能跟不上目标动作。
kd	阻尼系数 (抑制超调和振荡)	KD 适度增大：能减小“甩手-回弹-二次碰撞”。 KD过大：动作延迟。
action_scale	动作缩放系数 (策略输出动作偏移幅度)	action_scale增大：动作幅度变大、更激进，越偏离策略输出动作。更容易出现手臂扫到躯干、超调、抖动。 action_scale减小：动作幅度变小、更保守，越贴近策略输出动作。碰撞风险降低，但舞蹈表现可能不够顺滑。

本案例中的问题是，机器人快速收回手臂至胸前的动作，手指会和身体发生碰撞。因此分析是手臂arm_2和arm_4电机转动角度过大，导致手部末端和身体碰撞。电机的关节顺序如下

由于本案例中，只存在手部和身体碰撞问题，因此只调试手臂arm_2和arm_4电机参数。

调试过程总结：

（1）由于碰撞发生在手臂动作范围末端位置，首先增大手臂arm_2和arm_4电机kd参数。实机测试碰撞力度减轻，但碰撞仍然存在。

（2）由于初始实机参数是和训练环境参数一致的，为了动作优先紧跟策略输出动作。减小手臂arm_2和arm_4电机action_scale参数，减小动作偏离策略动作幅度。实机测试手部和身体未发生碰撞。

（3）若手部动作相比mujoco仿真效果，存在超调则减小kp。

优化参数后实机效果如下：