pybullet机器人仿真环境搭建 3.让机器人运动起来，检测物体的碰撞状态

最新推荐文章于 2024-04-25 09:55:46 发布

RuiH.AI

最新推荐文章于 2024-04-25 09:55:46 发布

阅读量5.3k

点赞数 5

分类专栏： python相关学习文章标签： python

本文链接：https://blog.csdn.net/qq_41035283/article/details/124109722

版权

python相关学习专栏收录该内容

41 篇文章 13 订阅

订阅专栏

pybullet机器人仿真环境搭建 3.让机器人运动起来，检测物体的碰撞状态

前言
搭建环境，载入模型
让机器人运动
- 获得机器人各关节信息
- 控制关节
碰撞检测
完整代码

前言

前两篇记录了pybullet中的基础环境搭建和模型载入。

本篇记录在pybullet中让模型运动起来，并检测与其它物体的碰撞状态。

搭建环境，载入模型

第一步仍然是建立pybullet环境，并载入pybullet_data自带的r2d2模型，此外再加载一个自带的正方体和球体模型：

if __name__ == '__main__':
    client = pybullet.connect(pybullet.GUI)
    pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
    pybullet.setPhysicsEngineParameter(numSolverIterations=10)
    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_TINY_RENDERER, 0)

    pybullet.setGravity(0, 0, -9.8)
    pybullet.setRealTimeSimulation(1)

    shift = [0, 0, 0]
    scale = [1, 1, 1]

    visual_shape_id = pybullet.createVisualShape(
        shapeType=pybullet.GEOM_MESH,
        fileName="sphere_smooth.obj",
        rgbaColor=[1, 1, 1, 1],
        specularColor=[0.4, 0.4, 0],
        visualFramePosition=[0, 0, 0],
        meshScale=scale)
    collision_shape_id = pybullet.createCollisionShape(
        shapeType=pybullet.GEOM_MESH,
        fileName="sphere_smooth.obj",
        collisionFramePosition=[0, 0, 0],
        meshScale=scale)
    pybullet.createMultiBody(
        baseMass=1,
        baseCollisionShapeIndex=collision_shape_id,
        baseVisualShapeIndex=visual_shape_id,
        basePosition=[0, 0, 2],
        useMaximalCoordinates=True)

	plane_id = pybullet.loadURDF("plane100.urdf", useMaximalCoordinates=True)
    cube_ind = pybullet.loadURDF('cube.urdf', (0, 1, 0), pybullet.getQuaternionFromEuler([0, 0, 0.785]))
    r_ind = pybullet.loadURDF('r2d2.urdf', (3, 1, 1),  pybullet.getQuaternionFromEuler([0, 0, 0.785]))
    # 创建结束，重新开启渲染
    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)

运行效果：
在这里插入图片描述

让机器人运动

机器人是靠关节运动的，因此通过向关节发出控制指令，就能让机器人动起来。

获得机器人各关节信息

首先要得到r2d2机器人的各关节信息：

# 得到机器人的关节数量
num_joints = pybullet.getNumJoints(r_ind)
# 获得各关节的信息
joint_infos = []
for i in range(num_joints):
	joint_info = pybullet.getJointInfo(r_ind, i)
	print(joint_info)
	if joint_info[2] != pybullet.JOINT_FIXED:
		if 'wheel' in str(joint_info[1]):
			joint_infos.append(joint_info)

我们打印了关节信息，可以得到：

(0, b'base_to_right_leg', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'right_leg', (0.0, 0.0, 0.0), (0.22, 0.0, 0.25), (0.0, -0.7070904020014416, 0.0, 0.7071231599922604), -1)

依次分别表示关节序号，关节名称，关节类型等等。

当关节类型为pybullet.JOINT_FIXED时，是不能动的。因此要筛掉这种关节，剩下来的就可以控制了。

r2d2机器人是有轮子的，因此我们想获得这些控制轮子的关节，即wheel在关节名字内。

控制关节

获得轮子关节后，我们就可以设置并控制这些关节的速度和加速度了：

maxforce = 1
velocity = 31.4
for i in range(len(joint_infos)):
     pybullet.setJointMotorControl2(bodyUniqueId=r_ind,
                                   jointIndex=joint_infos[i][0],
                                   controlMode=pybullet.VELOCITY_CONTROL,
                                   targetVelocity=target_v,
                                   force=max_force)
while True:
    pybullet.stepSimulation()
    time.sleep(1. / 240.)

setJointMotorControl2()是控制单个关节的api，也可以用setJointMotorControlArray()一次设置多个关节。其中，controlMode参数常用速度控制和位置控制，对应参数为targetVelocity和targetPosition，force参数限制了最大的力（加速度），防止加速度太大导致运动不稳定。速度、位置是通过PID控制获得的。

最后循环步进仿真。

运行结果：可以看到机器人向正方体运动。
在这里插入图片描述

碰撞检测

上面的机器人与正方体发生了碰撞，接下来就是在pybullet中检测到这种碰撞情况。

AABB碰撞检测

首先要知道常用的AABB碰撞检测方法。为了易于理解，先从2D碰撞检测解释：2D中的AABB碰撞检测，就是把场景物体框住，然后比较各物体框的坐标。如下图目标检测结果所示，这个框是与xy坐标轴平行的：
在这里插入图片描述
3D场景中的AABB也是一样的，用平行于xyz坐标轴的3D框包围物体，然后根据3D框坐标来判断碰撞：

3D框坐标为 ${P_{min}}=\{x_{min}, y_{min}, z_{min}\}, P_{max}=\{x_{max}, y_{max}, z_{max}\}$ ，物体所有点都在 ${P_{min},P_{max}\}$ 内。如果两个物体的3D AABB框的 $P$ 有交并，则两者碰撞了。

pybullet中，碰撞检测分为两步：获得需要检测碰撞的机器人的AABB框，检测AABB框的交并情况。

获得AABB框

使用getAABB()这个api，可以获得机器人的AABB框：

pmin, pmax = pybullet.getAABB(r_ind)

返回机器人AABB框的 $P_{min}, P_{max}$

检测AABB框的交并情况

使用getOverlappintObject()这个api，可以获得所有与指定AABB框有交并的模型的id：

collide_ids = pybullet.getOverlappingObjects(pmin, pmax)

返回的collide_ids是一个元组，其元素是二元tuple，比如：

((3, 8), (3, 1), (3, 0), (3, 5), (3, 4), (3, 13), (3, -1))

元组中每个元素代表一个与该AABB框交并的模型，每个元素tuple中，第一个元素是交并的模型id，第二个元素是交并的关节id（-1表示与该模型的base交并）。

注意：pybullet在检测碰撞时，默认会检测到自己与自己交并。比如上面我要检测碰撞的机器人id是3，碰撞检测结果也会检测到与id=3发生碰撞。

完整代码

import time

import numpy as np
import pybullet
import pybullet_data


if __name__ == '__main__':
    client = pybullet.connect(pybullet.GUI)
    pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
    pybullet.setPhysicsEngineParameter(numSolverIterations=10)
    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_TINY_RENDERER, 0)

    pybullet.setGravity(0, 0, -9.8)
    pybullet.setRealTimeSimulation(1)

    shift = [0, 0, 0]
    scale = [1, 1, 1]

    visual_shape_id = pybullet.createVisualShape(
        shapeType=pybullet.GEOM_MESH,
        fileName="sphere_smooth.obj",
        rgbaColor=[1, 1, 1, 1],
        specularColor=[0.4, 0.4, 0],
        visualFramePosition=[0, 0, 0],
        meshScale=scale)
    collision_shape_id = pybullet.createCollisionShape(
        shapeType=pybullet.GEOM_MESH,
        fileName="sphere_smooth.obj",
        collisionFramePosition=[0, 0, 0],
        meshScale=scale)
    pybullet.createMultiBody(
        baseMass=1,
        baseCollisionShapeIndex=collision_shape_id,
        baseVisualShapeIndex=visual_shape_id,
        basePosition=[-2, -1, 1],
        useMaximalCoordinates=True)

    plane_id = pybullet.loadURDF("plane100.urdf", useMaximalCoordinates=True)
    cube_ind = pybullet.loadURDF('cube.urdf', (3, 1, 1), pybullet.getQuaternionFromEuler([0, 0, 0]))
    r_ind = pybullet.loadURDF('r2d2.urdf', (1, 1, 1),  pybullet.getQuaternionFromEuler([0, 0, 1.57]))
    # 创建结束，重新开启渲染
    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)

    num_joints = pybullet.getNumJoints(r_ind)
    # 获得各关节的信息
    joint_infos = []
    for i in range(num_joints):
        joint_info = pybullet.getJointInfo(r_ind, i)
        if joint_info[2] != pybullet.JOINT_FIXED:
            if 'wheel' in str(joint_info[1]):
                print(joint_info)
                joint_infos.append(joint_info)

    maxforce = 10
    velocity = 31.4
    for i in range(len(joint_infos)):
        pybullet.setJointMotorControl2(bodyUniqueId=r_ind,
                                       jointIndex=joint_infos[i][0],
                                       controlMode=pybullet.VELOCITY_CONTROL,
                                       targetVelocity=velocity,
                                       force=maxforce)
    while True:
        pmin, pmax = pybullet.getAABB(r_ind)
        collide_ids = pybullet.getOverlappingObjects(pmin, pmax)
        print(collide_ids)
        for collide_id in collide_ids:
            if collide_id[0] != r_ind:
                print('detect robot collide with object id {}!'.format(r_ind))

        pybullet.stepSimulation()
        time.sleep(1. / 240.)

RuiH.AI

关注

5
点赞
踩
44

收藏

觉得还不错? 一键收藏
8
评论
pybullet机器人仿真环境搭建 3.让机器人运动起来，检测物体的碰撞状态

pybullet机器人仿真环境搭建 3.让机器人运动起来，检测物体的碰撞状态前言搭建环境，载入模型让机器人运动获得机器人各关节信息控制关节碰撞检测AABB碰撞检测获得AABB框检测AABB框的交并情况完整代码前言前两篇记录了pybullet中的基础环境搭建和模型载入。本篇记录在pybullet中让模型运动起来，并检测与其它物体的碰撞状态。搭建环境，载入模型第一步仍然是建立pybullet环境，并载入pybullet_data自带的r2d2模型，此外再加载一个自带的正方体和球体模型：if __na
复制链接

扫一扫