引言

pubullet作为仿真引擎在机械臂仿真中应用广泛，其可以根据内部丰富的接口实现机器人的不同控制，在之前饿博文中，我们学习了pybullet搭建机械臂仿真环境以及一些接口函数等使用。但是当机械臂在执行复杂的动作任务时，例如接触式任务中，不可避免得需要用到接触力，今天我们就一起来学习下pybullet中关于力交互的一些函数。

力传感器

在传统的机器人控制中，如果需要用到与环境的力交互，可以通过在机械臂末端安装六维力传感器的方式实现。但是在pybullet提供的文档PyBullet Quickstart Guide中并不能找到关于末端六维力传感器的信息，其中有一个接口函数是这个：

这个接口函数很容易把我们迷惑住，因为它里面右外部力/力矩的字样，但是它的实际作用其实是给物件施加的力或者力矩，从函数的参数总也可以看出。它没有返回值，同时需要给定世家力或者力矩的大小以及作用点（对应什么坐标系下）

我们同时设置一个简单的场景，对这个函数进行验证

代码和效果如下：

# _*_coding:utf-8-*-
import sys
import gym
from gym import spaces
import os
# sys.path.append(os.pardir)
import pybullet as p
import pybullet_data
gym.logger.set_level(40)
class UREnv(gym.Env):
    metadata = {'render.modes': ['human']}

    def __init__(self, gui=False):
        if gui:
            p.connect(p.GUI)
        else:
            p.connect(p.DIRECT)
        p.resetDebugVisualizerCamera(cameraDistance=1.48, cameraYaw=-0.8, cameraPitch=-30.00,
                                     cameraTargetPosition=[0.5, -0.05, 0.22])
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        p.setGravity(0, 0, -10)
    def step(self):
        p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)  #平滑视觉渲染
        p.applyExternalForce(self.car, 0, [10.0,0.0,0.0], [0.0, 0.0, 0.0], flags=p.LINK_FRAME)
        p.stepSimulation()
    def reset(self):
        p.resetSimulation()
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
        p.loadURDF("plane.urdf", basePosition=[0, 0, -0.65])
        p.loadURDF("table/table.urdf", basePosition=[0.5, 0, -0.65])
        self.car = p.loadURDF("husky/husky.urdf",basePosition=[0.355, 0, 0])

        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
        self.id = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, '~/record.mp4')
        p.stepSimulation()
    def close(self):
        p.stopStateLogging(self.id)
        p.disconnect()
if __name__ == '__main__':
    U = UREnv(True)
    U.reset()
    m = 1000
    while(m):
        m = m-1
        U.step()
    U.close()

既然这个接口函数不能达到我们的目的，那我么找找有没有其他的？其实还真的有，我们可以选择关节力矩传感器来代替末端六维力传感器进行控制。

1、在这里我们可以在工具坐标系使能力控函数，然后使用关节力矩传感器的值代替六维力传感器读数。这部分对应手册中的描述如下方所示

因此，在reset中使能关节力矩传感器

 p.enableJointForceTorqueSensor(self.UR5,7,1)

2.、然后需要获取力信息，这部分的手册中对应为

因此我们在step中可以这样写：

force = np.array(p.getJointState(self.UR5, 7)[2][0:3],dtype=float).reshape(3,1)
torque = np.array(p.getJointState(self.UR5, 7)[2][3:6],dtype=float).reshape(3,1)

May the force be with you !