Pybullet导入URDF文件——并创建约束

问题描述

一般地，在Pybullet中导入URDF文件，受到碰撞会产生移动 ；要想将导入的URDF文件固定，类似静态障碍物一样，需要对其另外创建约束。

导入URDF文件

pybullet.loadURDF(path,position,orientation,flags=0)：向物理服务器发送信息，加载URDF文件；返回创建模型的ID

path：URDF文件路径
position：世界坐标系X、Y、Z三维坐标
orientation：四元数
useFixedBase：True or False，是否使Base连杆保持固定；注意不是使其整体不动的意思，使其整体不动，需要createConstraint创建约束
flags：某些标志

创建约束

pybullet.createConstraint(parentBodyUniqueId, parentLinkIndex, childBodyUniqueId, childLinkIndex, jointType, jointAxis, parentFramePosition, childFramePosition)：对两个连杆的关节进行约束

注：关节和连杆的关系如下：
在这里插入图片描述

各参数意义（Pybullet官方）：

parentBodyUniqueId：父实体索引，loadURDF得到
parentLinkIndex：父实体的连杆索引，getNumJoints得到其关节数量，然后利用getLinkState依次输出各关节信息
childBodyUniqueId：子实体索引
childLinkIndex：子实体的连杆索引
jointType：关节的约束类型，如固定等（JOINT_PRISMATIC, JOINT_FIXED, JOINT_POINT2POINT, JOINT_GEAR）
jointAxis：在子连杆坐标系表示的关节轴
parentFramePosition：关节坐标系相对于父实体质心坐标系的位置（如果父实体为地面，那么则为子实体质心坐标系）
childFramePosition：关节坐标系相对于子实体质心坐标系的位置（getJointInfo函数返回的第15个值）
其他参数省略（非必选参数）

创建一个约束：

floor_id = p.loadURDF('plane.urdf')
box_id = p.loadURDF(fileName='cube_no_rotation.urdf',basePosition=[2,2,0],baseOrientation=p.getQuaternionFromEuler([0, 0, 0]),useFixedBase=True)
p.createConstraint(floor_id, -1, box_id, 0, p.JOINT_FIXED, [0, 0, 0], [2, 2, 0], [0, 0, 0])  # 添加约束

注：有时候URDF文件并非只有一个关节，只固定一个还是会使其运动，所以需要尽可能得使多个关节固定，才能保证最后加载的物体不会移动。

完整代码见：

import pybullet as p
import pybullet_data
import os
# p.connect(p.DIRECT)
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,-9.8)
p.setTimeStep(1./60)
p.setRealTimeSimulation(0)
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# 加载模型
floor_id = p.loadURDF('plane.urdf')
box_id = p.loadURDF(fileName='cube_no_rotation.urdf',basePosition=[2,2,0],baseOrientation=p.getQuaternionFromEuler([0, 0, 0]),useFixedBase=True)
# 需要改一下自己的路径
robot_id1 = p.loadURDF(fileName='/home/xxx/urdf_and_meshes/robomaster.urdf',basePosition=[-3,2,1],baseOrientation=p.getQuaternionFromEuler([0, 0, 0]))
robot_id2 = p.loadURDF(fileName='/home/xxx/urdf_and_meshes/robomaster.urdf',basePosition=[2,-3,1],baseOrientation=p.getQuaternionFromEuler([0, 0, 0]))

numJoints = p.getNumJoints(box_id)
print("机器人关节的数量：", numJoints)

for index in range(numJoints):
    pos = p.getLinkState(box_id,index)
    print('rbox_id index is:',pos)

print("机器人关节的信息：")
for joint_index in range(numJoints):
    joint_info = p.getJointInfo(box_id, joint_index)
    # [0]关节索引：{joint_info[0]}
    # [1]关节名称：{joint_info[1]}
    # [2]关节类型：{joint_info[2]}
    # [3]此主体的位置状态变量中的第一个位置索引：{joint_info[3]}
    # [4]在这个物体的速度状态变量中的第一个速度索引：{joint_info[4]}
    # [5]保留参数：{joint_info[5]}
    # [6]关节阻尼大小：{joint_info[6]}
    # [7]关节摩擦系数：{joint_info[7]}
    # [8]滑动或旋转关节的位置下限：{joint_info[8]}
    # [9]滑动或旋转关节的位置上限：{joint_info[9]}
    # [10]关节最大力矩：{joint_info[10]}
    # [11]关节最大速度：{joint_info[11]}
    # [12]连杆名称：{joint_info[12]}
    # [13]在当前连杆坐标系中表示的移动或转动的关节轴（忽略JOINT_FIXED固定关节）：{joint_info[13]}
    # [14]在父连杆坐标系中表示的关节位置：{joint_info[14]}
    # [15]在父连杆坐标系中表示的关节姿态（四元数x、y、z、w）：{joint_info[15]}
    # [16]父连杆的索引，若是base连杆则返回-1：{joint_info[16]}

# 打印可使用关节
joints_indexes = [i for i in range(numJoints) if p.getJointInfo(box_id, i)[2] != p.JOINT_FIXED]  # 可以使用的关节索引
print([p.getJointInfo(box_id, i) for i in joints_indexes])

# 注意因为有四个关节，所以要有四个约束；否则最终还是会移动
cid1 = p.createConstraint(floor_id, -1, box_id, 0, p.JOINT_FIXED, [0, 0, 0], [2, 2, 0], [0, 0, 0])  # 添加约束
cid2 = p.createConstraint(floor_id, -1, box_id, 1, p.JOINT_FIXED, [0, 0, 0], [2, 2, 0], [0, 0, 0])  # 添加约束
cid3 = p.createConstraint(floor_id, -1, box_id, 2, p.JOINT_FIXED, [0, 0, 0], [2, 2, 0], [0, 0, 0])  # 添加约束
cid4 = p.createConstraint(floor_id, -1, box_id, 3, p.JOINT_FIXED, [0, 0, 0], [2, 2, 0], [0, 0, 0])  # 添加约束

while True:
    p.stepSimulation()
    # 验证小车装上之后，障碍物是否会移动
    p.resetBaseVelocity(objectUniqueId=robot_id1,
                        linearVelocity=[0.1, 0, 0],
                        angularVelocity=[0, 0, 0],
                        )
    p.resetBaseVelocity(objectUniqueId=robot_id2,
                        linearVelocity=[0, 0.1, 0],
                        angularVelocity=[0, 0, 0],
                        )

PS：

也可以利用createMultiBody函数创建不能移动的障碍物，只需要在创建createCollisionShape、createVisualShape的时候将第一个参数改为GEOM_BOX

box_id = p.createCollisionShape(p.GEOM_BOX, halfExtents=[1+0.05, 1+0.05, 1])# 创建碰撞箱模型
box_visual_id = p.createVisualShape(p.GEOM_BOX,halfExtents=[1, 1, 1],rgbaColor=(0.1, 0.5, 0.1, 1))# 创建视觉模型
OBSTACLES_IDS[index] = p.createMultiBody(baseMass=0,baseCollisionShapeIndex=box_id,baseVisualShapeIndex=box_visual_id,basePosition=[1, 2, 0])

参考：
PyBullet仿真软件常用API函数
 闭链机构的问题