pybullet杂谈：pybullet实现SlideBar的封装，任何机械臂可直接调用API完成SlideBars控制

题目有些啰嗦，到底说了个啥意思呢？ 首先，是一个SlideBars控制，那啥是SlideBars控制呢？
  就是图中右侧的滑条，可以通过滑条控制机械臂的关节角度，作用就是可以看到每个关节的属性、每个关节对机械臂的运动产生的作用，以及一些其他的调试操作。   再来说说做这个SlideBars封装的意义。如果是按照pybullet提供的API，添加所有关节的SlideBar和实现对应关节电机的控制，需要为每一种机械臂做一些修改，针对不同的性质，而且做的修改部分还要去调试，总之很麻烦，做过的都知道。   现在进行的封装是什么意思呢？就是说，只要按照我提供的接口实例化一个SlideBars类，然后调用类中的创建slidebar函数，就可以实现想要的功能，而且对于任何一款机械臂，不需要自己做修改，在SlideBar类中，程序已经自己做了判断，做了对应的正确的操作。下面来看一个例子：  

1. 我要创建一个panda机械臂的slidebars。

import sys
sys.path.append('./')

import pybullet as p
import time
import math
from datetime import datetime
import pybullet_data
from utils.calc_inverse_kinematics import calc_inverse_kinematics
from utils.SlideBars import SlideBars

clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
    p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-9.8)
p.loadURDF("plane.urdf", [0, 0, -0.0])

pandaUid=p.loadURDF("franka_panda/panda.urdf",useFixedBase=True)
rest_poses = [0,-0.215,0,-2.57,0,2.356,2.356,0.08,0.08]

slide_bars=SlideBars(pandaUid)
motorIndices=slide_bars.add_slidebars()
while True:
    p.stepSimulation()
    slide_values=slide_bars.get_slidebars_values()
    p.setJointMotorControlArray(pandaUid,
                                motorIndices,
                                p.POSITION_CONTROL,
                                targetPositions=slide_values,
                                )

  创建slidebars的API就是这么几个，就可以实现一个slidebar。  

slide_bars=SlideBars(pandaUid)
motorIndices=slide_bars.add_slidebars()
while True:
    p.stepSimulation()
    slide_values=slide_bars.get_slidebars_values()
    p.setJointMotorControlArray(pandaUid,
                                motorIndices,
                                p.POSITION_CONTROL,
                                targetPositions=slide_values,
                                )

  创建kuka机械臂的slidebars  

import sys
sys.path.append('./')

import pybullet as p
import time
import math
from datetime import datetime
import pybullet_data
from utils.calc_inverse_kinematics import calc_inverse_kinematics
from utils.SlideBars import SlideBars

clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
    p.connect(p.GUI)

p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-9.8)
p.loadURDF("plane.urdf", [0, 0, -0.0])
#kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0.0])
kukaId=p.loadSDF("kuka_iiwa/kuka_with_gripper2.sdf")
kukaId=kukaId[0]
p.resetBasePositionAndOrientation(kukaId, [-0.1, 0, 0.07], [0, 0, 0, 1])

cube_small_Uid=p.loadURDF("cube_small.urdf", [0.7, 0 ,0])

slide_bars=SlideBars(kukaId)

motorIndices=slide_bars.add_slidebars()

while True:
    p.stepSimulation()
    slide_values=slide_bars.get_slidebars_values()
    p.setJointMotorControlArray(kukaId,
                                motorIndices,
                                p.POSITION_CONTROL,
                                targetPositions=slide_values,
                                )

  可以看到，还是使用相同的API，即可创建kuka机械臂的slidebar，大大减少了工作量。   因为代码量不多，直接将代码粘贴在此处。   SlideBars.py  

# -*- coding: utf-8 -*-

import pybullet as p

class SlideBars():
    def __init__(self,Id):
        self.Id=Id
        self.motorNames=[]
        self.motorIndices=[]
        self.motorLowerLimits=[]
        self.motorUpperLimits=[]
        self.slideIds=[]

        self.numJoints=p.getNumJoints(self.Id)

    def add_slidebars(self):
        for i in range(self.numJoints):
            jointInfo=p.getJointInfo(self.Id,i)
            jointName=jointInfo[1].decode('ascii')
            qIndex = jointInfo[3]
            lowerLimits=jointInfo[8]
            upperLimits=jointInfo[9]
            if qIndex > -1:
                self.motorNames.append(jointName)
                self.motorIndices.append(i)
                self.motorLowerLimits.append(lowerLimits)
                self.motorUpperLimits.append(upperLimits)

        for i in range(len(self.motorIndices)): 
            if self.motorLowerLimits[i]<=self.motorUpperLimits[i]:  
                slideId=p.addUserDebugParameter(self.motorNames[i],self.motorLowerLimits[i],self.motorUpperLimits[i],0)
            else: 
                slideId=p.addUserDebugParameter(self.motorNames[i],self.motorUpperLimits[i],self.motorLowerLimits[i],0)
            self.slideIds.append(slideId)

        return self.motorIndices

    def get_slidebars_values(self):
        slidesValues=[]
        for i in self.slideIds:
            value=p.readUserDebugParameter(i)
            slidesValues.append(value)
        return slidesValues

  创建slidebars：  

slide_bars=SlideBars(Uid)
motorIndices=slide_bars.add_slidebars()

  电机控制：  

slide_values=slide_bars.get_slidebars_values()
p.setJointMotorControlArray(Uid,
                            motorIndices,
                            p.POSITION_CONTROL,
                            targetPositions=slide_values,
                            )