pybullet机器人仿真环境搭建 4.镜头跟随移动机器人，在机器人上设置图像传感器
- 前言
- 建立仿真环境
- 镜头跟随移动机器人
- 在机器人上设置图像传感器
- 获取伪相机的位姿
- 获取伪相机的投影参数
- 从相机位姿与投影参数获得图像
- 完整代码
前言
本篇继续记录在pybullet环境中配置机器人，包括让镜头跟随机器人，在机器人上设置图像传感器。
建立仿真环境
第一步仍然是建立仿真环境，我把上次运动的r2d2环境放到这来：
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)镜头跟随移动机器人
使用resetresetDebugVisualizerCamera()这个api可以设置当前镜头的视角。因为我们要让镜头跟随机器人，因此就实时获得机器人的位置，然后把镜头对准机器人：
position, orientation = pybullet.getBasePositionAndOrientation(r_ind)
pybullet.resetDebugVisualizerCamera(cameraDistance=5,
cameraYaw=120,
cameraPitch=-45,
cameraTargetPosition=position)resetresetDebugVisualizerCamera()的参数中，cameraTargetPosition表示镜头对准的位置，其它参数表示镜头与该位置的距离、视角方向。
因为要跟随移动机器人，因此上面的代码需要与循环步进仿真放在一起。
在机器人上设置图像传感器
pybullet中图像传感器的设置是一个伪相机的放置，即在机器人的位置上实时获取其视角上的图像，因此要自己设置传感器在世界坐标中的位置，以及传感器的投影矩阵。
获取伪相机的位姿
首先，通过computeViewMatrix()这个api设置伪相机的视角位姿，需要提供的参数为相机位置，视角位置和相机纵轴。
其中，相机位置决定成像位置，相机位置与视角位置形成的矢量方向决定了相机朝向，相机纵轴朝上并与相机朝向垂直。
r_mat = pybullet.getMatrixFromQuaternion(orientation)
tx_vec = np.array([r_mat[0], r_mat[3], r_mat[6]])
tz_vec = np.array([r_mat[2], r_mat[5], r_mat[8]])
camera_position = np.array(position)
target_position = cameraPos - 1 * ty_vec
view_mat = pybullet.computeViewMatrix(cameraEyePosition=camera_position,
cameraTargetPosition=target_position,
cameraUpVector=tz_vec)获取伪相机的投影参数
通过computeProjectionMatrixFOV()这个函数来设置伪相机的FOV，视距：
proj_mat = pybullet.computeProjectionMatrixFOV(fov=60.0, # field of view
aspect=1.0, # scale, default=1
nearVal=0.01, # view distance min
farVal=10 # view distance max)从相机位姿与投影参数获得图像
最后，就是从上面的两个矩阵，获得仿真环境中对应伪相机采集的图像了：
w, h, rgb, depth, seg = pybullet.getCameraImage(width=640,
height=480,
viewMatrix=view_mat,
projectionMatrix=proj_mat,
physicsClientId=server_id)完整代码
import time
import numpy as np
import pybullet
import pybullet_data
def set_camera(robot_id: int, width: int = 224, height: int = 224, server_id: int = 0):
position, orientation = pybullet.getBasePositionAndOrientation(robot_id, physicsClientId=server_id)
r_mat = pybullet.getMatrixFromQuaternion(orientation)
tx_vec = np.array([r_mat[0], r_mat[3], r_mat[6]])
ty_vec = np.array([r_mat[1], r_mat[4], r_mat[7]])
tz_vec = np.array([r_mat[2], r_mat[5], r_mat[8]])
camera_position = np.array(position)
target_position = camera_position - 1 * ty_vec
view_mat = pybullet.computeViewMatrix(cameraEyePosition=camera_position,
cameraTargetPosition=target_position,
cameraUpVector=tz_vec)
proj_mat = pybullet.computeProjectionMatrixFOV(fov=60.0, # 摄像头的视线夹角
aspect=1.0,
nearVal=0.01, # 摄像头视距min
farVal=10 # 摄像头视距max)
w, h, rgb, depth, seg = pybullet.getCameraImage(width=width,
height=height,
viewMatrix=view_mat,
projectionMatrix=proj_mat,
physicsClientId=server_id)
return w, h, rgb, depth, seg
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_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, 2), pybullet.getQuaternionFromEuler([0, 0, 0]))
r_ind = pybullet.loadURDF('r2d2.urdf', (1, 1, 2), 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 = 3.14
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)
flag = True
while True:
position, orientation = pybullet.getBasePositionAndOrientation(r_ind)
pybullet.resetDebugVisualizerCamera(cameraDistance=5,
cameraYaw=120,
cameraPitch=-45,
cameraTargetPosition=position)
pmin, pmax = pybullet.getAABB(r_ind)
collide_ids = pybullet.getOverlappingObjects(pmin, pmax)
for collide_id in collide_ids:
if collide_id[0] != r_ind:
print('detect robot collide with object id {}!'.format(r_ind))
flag = False
break
set_camera(r_ind)
pybullet.stepSimulation()
time.sleep(1. / 240.)
if not flag:
break运行效果：
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
