上节我们已经搭建完成了基本的场景Losgy浩：基于Pybullet搭建强化学习机械臂（一），也就是皮儿，这一节将继续完善机械臂的相关接口

完整项目地址：https://github.com/PiggyCh/RL_arm_under_sparse_reward)

一. 机械臂正逆运动学的实现

首先我们利用p.getJointInfo() 来测试一下机械手臂的各个关节

如表中所述，getJointInfo函数需要提供目标的Uid（也就是在loadurdf时返回的编号）以及关节的索引，如果bullet引擎是多开的，也可以提供引擎的id，不过大多数非并行的场合是用不到的。

在下面的长表中是它的返回值。

这里我们通过for 循环，将机械臂的所有关节信息进行输出

 for i in range(24):
     with open('bmirobot_joints_info_pybullet.txt','a') as f:
          f.write(p.getJointInfo(armid, i))

可以得到如下信息：

 jointindex、jointname、jointtype、qindex、uindex、flags、jointdamping、jointfriction、jointlowerlimit、jointUpperLimit、jointmaxForce、jointMaxVelocity、linkname、jointaxis、parentFramePos、parentFrameOrn、ParentIndex
 (0, b'fixed', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'odom_combined', (0.0, 0.0, 0.0), (0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), -1)
 (1, b'virtual_joint', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'base_link', (0.0, 0.0, 0.0), (0.0, 0.0, 0.45), (0.0, 0.0, 0.0, 1.0), 0)
 (2, b'rightvirtual_joint', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'right_link1', (0.0, 0.0, 0.0), (0.22, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 1)
 (3, b'right_joint1', 0, 7, 6, 1, 0.7, 0.0, -1.57079632679, 3.14159265359, 1000.0, 9.42477796077, b'right_link2', (1.0, 0.0, 0.0), (0.081, 0.0, -1.0), (0.0, 0.0, 0.0, 1.0), 2)
 (4, b'right_joint2', 0, 8, 7, 1, 0.7, 0.0, -3.14159265359, 0.174532925199, 1000.0, 9.42477796077, b'right_link3', (0.0, 1.0, 0.0), (0.04, 0.0, -1.0), (0.0, -0.7071067811848163, 0.0, 0.7071067811882787), 3)
 (5, b'right_joint3', 0, 9, 8, 1, 0.7, 0.0, -1.57079632679, 1.57079632679, 1000.0, 9.42477796077, b'right_link4', (1.0, 0.0, 0.0), (0.0555, 0.0, -1.0), (0.7071067811848163, 0.0, 0.0, 0.7071067811882787), 4)
 (6, b'right_joint4', 0, 10, 9, 1, 0.7, 0.0, -0.872664625997, 2.09439510239, 1000.0, 9.42477796077, b'right_link5', (0.0, 1.0, 0.0), (0.1945, 0.0, -1.0), (0.0, 0.7071067811848163, 0.0, 0.7071067811882787), 5)
 (7, b'right_joint5', 0, 11, 10, 1, 0.7, 0.0, -3.14159265359, 1.74532925199, 1000.0, 9.42477796077, b'right_link6', (1.0, 0.0, 0.0), (0.037, 0.0, -1.0), (-0.7071067811848163, 0.0, 0.0, 0.7071067811882787), 6)
 (8, b'right_joint6', 0, 12, 11, 1, 0.7, 0.0, -1.2217304764, 1.57079632679, 1000.0, 9.42477796077, b'right_link7', (0.0, 1.0, 0.0), (0.203, 0.0, -1.0), (0.7071067811848163, 0.0, 0.0, 0.7071067811882787), 7)
 (9, b'right_joint7', 0, 13, 12, 1, 0.7, 0.0, -1.57079632679, 1.57079632679, 1000.0, 9.42477796077, b'right_link8', (0.0, 0.0, 1.0), (0.0, 0.0, -1.0), (0.0, 0.0, 0.0, 1.0), 8)
 (10, b'right_hand_joint1', 0, 14, 13, 1, 0.7, 0.0, -1.57079632679, 1.57079632679, 1000.0, 9.42477796077, b'right_hand1', (0.0, 0.0, 1.0), (0.0865, 0.0265, -0.976), (0.0, 0.0, 0.0, 1.0), 9)
 (11, b'right_hand_joint2', 0, 15, 14, 1, 0.7, 0.0, -1.57079632679, 1.57079632679, 1000.0, 9.42477796077, b'right_hand2', (0.0, 0.0, 1.0), (0.0865, 0.0, -0.976), (0.0, 0.0, 0.0, 1.0), 9)
 (12, b'rightgrasping_frame_joint', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rightgrasping_frame', (0.0, 0.0, 0.0), (0.1565, 0.0265, -0.976), (0.0, 0.0, 0.0, 1.0), 9)
 (13, b'leftvirtual_joint', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'left_link1', (0.0, 0.0, 0.0), (0.03, 0.0, 0.0), (0.0, 0.0, 1.0, 1.0341155355510722e-13), 1)
 (14, b'left_joint1', 0, 16, 15, 1, 0.7, 0.0, -1.57079632679, 3.14159265359, 1000.0, 9.42477796077, b'left_link2', (1.0, 0.0, 0.0), (0.081, 0.0, -1.0), (0.0, 0.0, 0.0, 1.0), 13)
 (15, b'left_joint2', 0, 17, 16, 1, 0.7, 0.0, -3.14159265359, 0.174532925199, 1000.0, 9.42477796077, b'left_link3', (0.0, 1.0, 0.0), (0.04, 0.0, -1.0), (0.0, -0.7071067811848163, 0.0, 0.7071067811882787), 14)
 (16, b'left_joint3', 0, 18, 17, 1, 0.7, 0.0, -1.57079632679, 1.57079632679, 1000.0, 9.42477796077, b'left_link4', (1.0, 0.0, 0.0), (0.0555, 0.0, -1.0), (-0.7071067811848163, 0.0, 0.0, 0.7071067811882787), 15)
 (17, b'left_joint4', 0, 19, 18, 1, 0.7, 0.0, -0.872664625997, 2.09439510239, 1000.0, 9.42477796077, b'left_link5', (0.0, 1.0, 0.0), (0.1945, 0.0, -1.0), (0.0, 0.7071067811848163, 0.0, 0.7071067811882787), 16)
 (18, b'left_joint5', 0, 20, 19, 1, 0.7, 0.0, -3.14159265359, 1.74532925199, 1000.0, 9.42477796077, b'left_link6', (1.0, 0.0, 0.0), (0.037, 0.0, -1.0), (-0.7071067811848163, 0.0, 0.0, 0.7071067811882787), 17)
 (19, b'left_joint6', 0, 21, 20, 1, 0.7, 0.0, -1.2217304764, 1.57079632679, 1000.0, 9.42477796077, b'left_link7', (0.0, 1.0, 0.0), (0.203, 0.0, -1.0), (0.7071067811848163, 0.0, 0.0, 0.7071067811882787), 18)
 (20, b'left_joint7', 0, 22, 21, 1, 0.7, 0.0, -1.57079632679, 1.57079632679, 1000.0, 9.42477796077, b'left_link8', (0.0, 0.0, 1.0), (0.0, 0.0, -1.0), (0.0, 0.0, 0.0, 1.0), 19)
 (21, b'left_hand_joint1', 0, 23, 22, 1, 0.7, 0.0, -1.74532925199, 1.74532925199, 1000.0, 9.42477796077, b'left_hand1', (0.0, 0.0, 1.0), (0.0865, 0.0265, -0.976), (0.0, 0.0, 0.0, 1.0), 20)
 (22, b'left_hand_joint2', 0, 24, 23, 1, 0.7, 0.0, -0.785398163397, 0.0, 1000.0, 9.42477796077, b'left_hand2', (0.0, 0.0, 1.0), (0.0865, 0.0, -0.976), (0.0, 0.0, 0.0, 1.0), 20)
 (23, b'leftgrasping_frame_joint', 4, -1, -1, 0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'leftgrasping_frame', (0.0, 0.0, 0.0), (0.1565, 0.0265, -0.976), (0.0, 0.0, 0.0, 1.0), 20)
 ​
 其中：
 jointindex：关节索引
 jointname：关节名
 jointtype：关节类型：：这也意味着位置和速度变量的数量 0 Joint_revolute:旋转副 1  Joint_prismatic:移动副 2 JOINT_SPHERICAL(球副):对一个空间中任意轴的转动,一个joint里面至多只有一个Spherical,而且决不能和Revolute一起出现,记为S 3 joint_planar :平面关节    4 joint_fixed:固定关节
 qindex：此主体的位置状态变量中的第一个位置索引 ？？？
 uindex：这个物体的速度状态变量中的第一个速度索引 ？？？
 flags： reserved
 jointdamping：关节阻尼值，在URDF文件中指定
 jointfriction：关节摩擦值，在URDF文件中指定
 jointlowerlimit：定位滑块和转动(铰链)关节的下限。
 jointUpperLimit：滑动块和转动关节的位置上限。如果上限<下限，则忽略值。
 jointmaxForce:在URDF中指定的最大力(可能是其他文件格式)，注意这个值不会自动使用。你可以在'setJointMotorControl2'中使用maxForce。
 jointMaxVelocity:URDF中指定的最大速度。注意，此时最大速度并没有被用于实际的电机控制命令中
 linkname：link的名称，在URDF(或SDF等)文件中指定
 jointaxis:当前frame下的joint轴， 忽略了固定joint
 parentFramePos：在父frame的关节position
 parentFrameOrn：在夫frame的关节orientation
 ParentIndex:父frame index

得到的信息如：关节名称、类型、阻尼、摩擦、上下限范围等等，不过这些信息都是在URDF中定义的，这里我们可以将它们统一输出作为设置时的参考。

下一步我们去实现手臂的随机移动，这一步我们可以通过实现关节角的转动来实现，在本文中，虽然机械臂的模型是双臂的，但实际上我们只控制了单个机械臂，也就是关节值从1到12的位置，我们可以对关节进行随机赋值来驱动手臂进行运动。

使用的是p.setJointMotorControl2 (为什么有2？ 因为1的API 版本被弃用了 )

需要提供的参数有：

首先肯定还是有机械臂的Uid了,在加载URDF文件时我们就将它记录好，然后是关节的索引，控制模式，控制模式它提供了Position_control（点控制）、Velocity_Control（速度控制）、Torque_Control（扭矩控制）和PD_Control， 需要注意的是这里使用int值来代表控制模式，当然也是可以直接用p.XXX_Control的形式来写

上述几个参数是必须的，下面的参数是optional可选的参数，不过如果你选择了电控制的模式，当然 还是要提供targetposition的，这里我们还是使用点控制来进行操作。

for i in range(3,12):
     p.setJointMotorControl2(bodyIndex=self.bmirobotid,
                                     jointIndex=i,
                                     controlMode=p.POSITION_CONTROL,
                                     targetPosition=jointPoses[i-3],
                                     targetVelocity=0,
                                     force=500,
                                     positionGain=0.03,
                                     velocityGain=1)

除了必须要有的参数外，这里我们也使用了一些参数来限制机械臂的运动情况，这些参数是参考了官方自带的一个kuka机器人的example。 这里我们给一些随机动作让机械臂动起来。

下面我们使用逆运动学来控制机械臂到达某个位置，

使用的是p.calculateInverseKinematics

需要的参数就是id、末端link索引、目标位置。 可选的参数有orientation、limits、IKsolver等等。

这里我们定义一个逆运动学方法，以方便调用

 def getinversePoisition(bmirobot_id,position_desired,orientation_desired=[]):
     joints_info = []
     joint_damping = []
     joint_ll = []
     joint_ul = []
     useOrientation=len(orientation_desired)
     for i in range(24):
         joints_info.append(p.getJointInfo(bmirobot_id, i))
     bmirobotEndEffectorIndex = 11
     numJoints = p.getNumJoints(bmirobot_id)
     useNullSpace = 1
     ikSolver = 1
     pos = [position_desired[0], position_desired[1], position_desired[2]]
     # end effector points down, not up (in case useOrientation==1)
     if useOrientation:
         orn = p.getQuaternionFromEuler([orientation_desired[0],orientation_desired[1] , orientation_desired[2]])
     if (useNullSpace == 1):
         if (useOrientation == 1):
             jointPoses = p.calculateInverseKinematics(bmirobot_id, bmirobotEndEffectorIndex, pos, orn)
         else:
             jointPoses = p.calculateInverseKinematics(bmirobot_id,
                                                   bmirobotEndEffectorIndex,
                                                   pos,
                                                   lowerLimits=joint_ll,
                                                   upperLimits=joint_ul,
                                                )
         # print(jointPoses)
     else:
       if (useOrientation == 1):
         jointPoses = p.calculateInverseKinematics(bmirobot_id,
                                                   bmirobotEndEffectorIndex,
                                                   pos,
                                                   orn,
                                                   solver=ikSolver,
                                                   maxNumIterations=100,
                                                   residualThreshold=.01)
         # print(jointPoses)
       else:
         jointPoses = p.calculateInverseKinematics(bmirobot_id,
                                                   bmirobotEndEffectorIndex,
                                                   pos,
                                                   solver=ikSolver)
         p.getJointInfo()
     return jointPoses

参考的代码也是kuka机器人的官方example，总而言之就是调用了逆运动学通过输入目标位置和方位，来反算出了关节的角度，最后我们整合整个过程，即：输入一个目标位置-》通过IK反算关节位置-》将反算出的关节位置施加到关节上

 def get_to_place(self,position):
       orn=[]
       jointPoses = inverse.getinversePoisition(self.bmirobotid, position, orn) #.getinversePoisition是上面的写的函数
       for i in range(3,12):
             p.setJointMotorControl2(bodyIndex=self.bmirobotid,
                                     jointIndex=i,
                                     controlMode=p.POSITION_CONTROL,
                                     targetPosition=jointPoses[i-3],
                                     targetVelocity=0,
                                     force=500,
                                     positionGain=0.03,
                                     velocityGain=1)
       return

也就完成了整个机械臂的正逆运动学过程。