云深处x20机械狗与kinova-gen3-lite机械臂的whole-body全身控制仿真与部署（二）

本文链接：https://blog.youkuaiyun.com/qq_61874429/article/details/147239504

在上篇文章中我们搭建完成了需要的环境，现在我们需要构建云深处x20机械狗和kinova-gen3-lite全身控制模型。

仿真模型的构建

1.下载机械臂与机械狗模型

1.1下载官网开源的机械狗模型

由于官网并没有x20urdf模型，询问官方人员后得知只需将x20运动主机升级后即可运行x30版本的sdk，这样就可以利用x30的模型训练完之后部署到x20身上。以下为x30模型链接，包括meshes和urdf文件：

https://github.com/DeepRoboticsLab/URDF_model

1.2下载官方开源的机械臂模型

下载地址如下，同样包括meshes和urdf文件:

https://github.com/Kinovarobotics/ros_kortex/tree/kinetic-devel/kortex_description/arms/gen3_lite/6dof

2.组合机械臂和机械狗代码

2.1组合机械狗代码

根据b1z1的urdf模型，我们需要在x30urdf模型文件上增加颜色，基座，浮动基座和imu信息，其中部分代码如下:

  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0" />
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0" />
  </material>
  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0" />
  </material>
  <material name="grey">
    <color rgba="0.2 0.2 0.2 1.0" />
  </material>
  <material name="silver">
    <color rgba="0.9137254901960784 0.9137254901960784 0.8470588235294118 1.0" />
  </material>
  <material name="orange">
    <!-- <color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/> -->
    <color rgba="0.12 0.15 0.2 1.0" />
  </material>
  <material name="brown">
    <color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0" />
  </material>
  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0" />
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0" />
  </material>

  <link name="base_link">
    <inertial>
      <origin xyz="0.0 0.0 0.0"/>
      <mass value="1e-12" />
      <inertia ixx="1e-12" ixy="0" ixz="0" iyy="1e-12" iyz="0" izz="1e-12" />
    </inertial>
  </link>

  <joint name="floating_base" type="fixed" dont_collapse="true">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="TORSO" />
  </joint>

  <link name="TORSO">
    <inertial>
      <origin xyz="-0.0029257 7.5034e-06 0.020095" rpy="0 0 0" />
      <mass value="30.7" />
      <inertia ixx="0.364306" ixy="0.00018421" ixz="0.00027469" iyy="0.597627" iyz="0.00026784" izz="0.757267" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="../meshes/torso.dae" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.44 0.26 0.15"/>
      </geometry>
    </collision>
  </link>

  <joint name="imu_joint" type="fixed">
    <parent link="TORSO" />
    <child link="imu_link" />
    <origin rpy="0 0 0" xyz="0 0 0" />
  </joint>

  <link name="imu_link">
    <inertial>
      <mass value="0.001" />
      <origin rpy="0 0 0" xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001" />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0" />
      <geometry>
        <box size="0.001 0.001 0.001" />
      </geometry>
      <material name="red" />
    </visual>
    <!-- <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision> -->
  </link>

2.2 组合机械臂代码

同样，我们需要在机械臂和机械狗的之间建立连接，代码如下:

<joint name="base_static_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.1 0 0.09>>" />
    <parent link="base_link" />
    <child link="BASE" />
  </joint>

  <link name="BASE">
    <inertial>
      <origin xyz="3 0 0" rpy="0 0 0" />
      <mass value="1.14608471" />
      <inertia ixx="0.00335854" ixy="3.9E-07" ixz="0.00010989" iyy="0.003311" iyz="1.91E-06" izz="0.00077158" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="../meshes/base_link.STL" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="../meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>

2.3组合夹爪代码

官方kinova-gen3-lite的机械臂中是不带夹爪的，这里我们用b1z1的夹爪，

 <link name="gripperStator">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/z1_GripperStator.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/z1_GripperStator.dae" scale="1 1 1"/>
      </geometry>
    </collision>
    <!-- <collision>
      <geometry>
        <cylinder length="0.051" radius="0.0325"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision> -->
    <inertial>
      <origin rpy="1.5707963267948966 0 0" xyz="0.04764427 -0.00035819 -0.00249162"/>
      <mass value="0.52603655"/>
      <inertia ixx="0.00038683" ixy="3.59e-06" ixz="-7.662e-05" iyy="0.00068614" iyz="-2.09e-06" izz="0.00066293"/>
    </inertial>
  </link>

  <joint name="z1_jointGripper" type="revolute">
    <origin rpy="0 0 0" xyz="0.049 0.0 0"/>
    <parent link="gripperStator"/>
    <child link="gripperMover"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="1.0" friction="1.0"/>
    <limit effort="30.0" lower="-1.5707963267948966" upper="0.0" velocity="10"/>
  </joint>

  <link name="gripperMover">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/z1_GripperMover.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/z1_GripperMover.dae" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="1.5707963267948966 0 0" xyz="0.01320633 0.00476708 0.00380534"/>
      <mass value="0.27621302"/>
      <inertia ixx="0.00017716" ixy="-1.683e-05" ixz="1.786e-05" iyy="0.00026787" iyz="-2.62e-06" izz="0.00035728"/>
    </inertial>
  </link>

  <joint name="ee_gripper" type="fixed" dont_collapse="true">
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0.135 0 0"/>
    <parent link="gripperStator"/>
    <child link="ee_gripper_link"/>
  </joint>

  <link name="ee_gripper_link">
    <inertial>
      <mass value="0.001"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>
  </link>

注意:

b1z1默认的夹爪与机械臂末端最后一个关节方向不一致，我们将其旋转保持一致。同时机械臂和机械狗的名称需要进行修改，如下代码所示

  <link name="FR_SHANK">
    <inertial>
      <origin xyz="0.014851 2.0687e-05 -0.12283" rpy="0 0 0" />
      <mass value="0.71386" />
      <inertia ixx="0.0093039" ixy="1.7692e-07" ixz="0.00034843" iyy="0.0096272" iyz="-3.7714e-06" izz="0.00041542" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="../meshes/shank.dae" />
      </geometry>
    </visual>
    <collision>
      <origin rpy="0. -0.40 0." xyz="0.02 0 -0.046" />
      <geometry>
        <cylinder length="0.1" radius="0.025"/>
      </geometry>
    </collision>
      <collision>
        <origin rpy="0. 0 0." xyz="0.045 0 -0.155" />
        <geometry>
          <cylinder length="0.12" radius="0.025"/>
        </geometry>
    </collision>
    <collision>
        <origin rpy="0. 0.45 0." xyz="0.025 0 -0.246" />
        <geometry>
          <cylinder length="0.08" radius="0.022"/>
        </geometry>
    </collision>
  </link>

  <joint name="FR_calf_joint" type="revolute">
    <origin xyz="0 0 -0.30" rpy="0 0 0" />
    <parent link="FR_THIGH" />
    <child link="FR_SHANK" />
    <axis xyz="0 -1 0" />
    <dynamics damping="0.01" friction="0.03"/>
    <limit lower="0.349" upper="2.531" effort="180" velocity="16.1" />
  </joint>

注意:

每个关节有三部分名称需要注意，link name对应meshes文件中的关节文件名称，joint name对应cfg文件中定义的初始关节名称，这里我们需要修改原文件将其与b1z1的cfg名称相匹配，mesh filename为meshes对应各个关节文件所在地址,这里包括机械臂和机械狗18个关节都需要更改，以上为其中一个关节范例。

3. 编写测试文件，观察各个关节是否安装正常:

我们的测试文件代码如下:

import gym
import numpy as np
from isaacgym.torch_utils import *
from isaacgym import gymtorch, gymapi, gymutil
import torch
import os
import math
import random

# 初始化gymAPI
gym = gymapi.acquire_gym()

# 创建仿真环境
sim_params = gymapi.SimParams()
sim_params.dt = 1 / 60
sim_params.substeps = 2
sim_params.up_axis = gymapi.UP_AXIS_Z
sim_params.gravity = gymapi.Vec3(0.0, 0.0, -9.8)

# PhysX-specific parameters
sim_params.physx.use_gpu = True
sim_params.physx.solver_type = 1
sim_params.physx.num_position_iterations = 6
sim_params.physx.num_velocity_iterations = 1
sim_params.physx.contact_offset = 0.01
sim_params.physx.rest_offset = 0.0

# Flex-specific parameters
sim_params.flex.solver_type = 5
sim_params.flex.num_outer_iterations = 4
sim_params.flex.num_inner_iterations = 20
sim_params.flex.relaxation = 0.8
sim_params.flex.warm_start = 0.5

# 配置地面
plane_params = gymapi.PlaneParams()
plane_params.normal = gymapi.Vec3(0, 0, 1)  # z-up
plane_params.distance = 0
plane_params.static_friction = 1
plane_params.dynamic_friction = 1
plane_params.restitution = 0

# 创建仿真
compute_device_id = 0  # 计算设备ID
graphics_device_id = 0  # 图形设备ID
physics_engine = gymapi.SIM_PHYSX  # 选择物理引擎

# 创建仿真
sim = gym.create_sim(compute_device_id, graphics_device_id, physics_engine, sim_params)

# 创建地面
gym.add_ground(sim, plane_params)

# URDF文件的路径
asset_root = os.path.expanduser("~/whn/visual_wholebody-main/low-level/resources/robots/x30-gen3")
asset_file = "urdf/X30-gen3.urdf"
asset_options = gymapi.AssetOptions()
asset_options.fix_base_link = True
asset_options.armature = 0.01

# 加载资产
asset = gym.load_asset(sim, asset_root, asset_file, asset_options)

# 创建环境网格
spacing = 2.0
lower = gymapi.Vec3(-spacing, 0.0, -spacing)
upper = gymapi.Vec3(spacing, spacing, spacing)

num_envs = 3
envs_per_row = 8

# 创建并填充环境
envs = []
actor_handles = []

for i in range(num_envs):
    # 创建环境
    env = gym.create_env(sim, lower, upper, envs_per_row)
    envs.append(env)

    # 随机高度生成
    height = random.uniform(1.0, 2.5)

    # 设置起始位置
    pose = gymapi.Transform()
    pose.p = gymapi.Vec3(0.0, height, 0.0)

    # 创建 actor
    actor_handle = gym.create_actor(env, asset, pose, "MyActor", i, 1)
    actor_handles.append(actor_handle)

# 创建视图器
cam_props = gymapi.CameraProperties()
viewer = gym.create_viewer(sim, cam_props)

# 进入仿真循环
while not gym.query_viewer_has_closed(viewer):
    # 进行物理仿真
    gym.simulate(sim)
    gym.fetch_results(sim, True)

    # 更新视图
    gym.step_graphics(sim)
    gym.draw_viewer(viewer, sim, True)

    # 等待仿真步长与实际时间同步
    gym.sync_frame_time(sim)

# 销毁视图器和仿真
gym.destroy_viewer(viewer)
gym.destroy_sim(sim)

注意:urdf文件路径为你所创建的模型文件路径