Pybullet中获取关节力和力矩反馈

在机械臂抓取中，末端夹爪是否抓取成功可以通过获取末端夹爪关节上所受的力或力矩来判断，那么我介绍一下pybullet中获取关节的力或者力矩的方法。

首先是要启用每个关节中关节力/扭矩传感器，具体函数如下所示：

p.enableJointForceTorqueSensor(self.ur3_arm.id, self.ur3_arm.mimic_parent_id, 1)

该函数所必需传入的参数有机器人的id和你要获取的那个关节数据的索引，1表示启用，0表示禁用。

enableJointForceTorqueSensor：

required	bodyUniqueId	int	body unique id as returned by loadURDF etc
required	jointIndex	int	joint index in range [0..getNumJoints(bodyUniqueId)]
optional	enableSensor	int	1/True to enable, 0/False to disable the force/torque sensor
optional	physicsClientId	int	if you are connected to multiple servers, you can pick one.

然后就是如何获取关节的力/力矩，这里我们使用getJointState该函数所返回的数据包括关节的位姿、关节的速度、关节的反作用力，最后一个数据我没用过所以还不清楚其具体作用，数据表示具体如下：

getJointState：

jointPosition	list of 1 or 4 float	The position value of this joint (as jonit angle/position or joint orientation quaternion)
jointVelocity	list of 1 or 3 float	The velocity value of this joint.
jointReactionForces	list of 6 floats	These are the joint reaction forces, if a torque sensor is enabled for this joint it is [Fx, Fy, Fz, Mx, My, Mz]. Without torque sensor, it is [0,0,0,0,0,0].
appliedJointMotorTorque	float	This is the motor torque applied during the last stepSimulation. Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor torque is exactly what you provide, so there is no need to report it separately.

因此我们要获取关节的力或力矩就需要获取jointReactionForces的数据，那么具体获取方法如下：

force = np.array(p.getJointState(bodyUniqueId=self.ur3_arm.id, jointIndex=self.ur3_arm.mimic_parent_id)[2][0:3],
                dtype=float).reshape(3, 1) #这个就是返回jointReactionForces中的前三个数

torque = np.array(p.getJointState(bodyUniqueId=self.ur3_arm.id, jointIndex=self.ur3_arm.mimic_parent_id)[2][3:6],
                  dtype=float).reshape(3, 1) #这个就是返回jointReactionForces中后三个数据

自此，关节力/力矩获取的方式已经介绍完毕。如果大家对pybullet的基础知识不够了解，可以访问我下面放的链接下载pybullet快速入门手册。

pybullet快速入门指南