Code:

import numpy as npimport pybullet as pimport timeimport pybullet_dataimport mathphysicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical versionp.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionallytimeStep = 1./240.p.setPhysicsEngineParameter(fixedTimeStep=timeStep)planeId = p.loadURDF("plane.urdf")target_objects = p.loadMJCF("MPL/MPL.xml")target_hand = target_objects[0]pd_objects = p.loadMJCF("MPL/MPL.xml")pd_hand = pd_objects[0]p.resetBasePositionAndOrientation(target_hand, [2.7, -2.2, 2.4], [0, 0, 0, 1])p.resetBasePositionAndOrientation(pd_hand, [2.7, -2.2, 2.6], [0, 0, 0, 1])p.createConstraint(pd_hand, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], #[-0.05, 0, 0.02]                              [2.7, -2.2, 2.6], [0, 0, 0, 1])jointsCount = p.getNumJoints(pd_hand)indices = []targetPositions = []maxForces = []kps = []kds = []for i in range(jointsCount):  indices.append(i)  targetPositions.append(0.0)  maxForces.append(2000000000)  kps.append(1) #1000000000  kds.append(10)#500000#p.setJointMotorControl2(pd_hand, 17, p.POSITION_CONTROL, 1, 0)from pdControllerStable_customized import PDControllerStable#stablePD = PDControllerStable(p)pi = 3.141592while(1):    #p.setJointMotorControl2(hand, 11, p.POSITION_CONTROL, pi / 4.)    #rot = p.getQuaternionFromEuler([math.sin(time.time()*3), 0, 0])    rot1 = math.sin(time.time() * 3)    rot2 = math.cos(time.time() * 3)    #kinematic    p.resetJointState(target_hand, 17, rot1)    p.resetJointState(target_hand, 19, rot2)    p.resetJointState(target_hand, 21, rot1)    #pd    # p.setJointMotorControl2(pd_hand, 17, p.POSITION_CONTROL, targetPosition=rot1)    # p.setJointMotorControl2(pd_hand, 19, p.POSITION_CONTROL, targetPosition=rot2)    # p.setJointMotorControl2(pd_hand, 21, p.POSITION_CONTROL, targetPosition=rot1)    targetPositions[17] = rot1    targetPositions[19] = rot2    targetPositions[21] = rot1    #PD control    jointStates = p.getJointStates(pd_hand, indices)    q1 = []    qdot1 = []    for i in range(jointsCount):        q1.append(jointStates[i][0])        qdot1.append(jointStates[i][1])    q = np.array(q1)    qdot = np.array(qdot1)    qdes = np.array(targetPositions)    qError = qdes - q    Kp = np.diagflat(kps)    Kd = np.diagflat(kds)    P = Kp.dot(qError)    D = Kd.dot(qdot)    taus = P - D    p.setJointMotorControl2(        pd_hand,        17,        controlMode=p.TORQUE_CONTROL,        force=2    )    # for index in range(jointsCount):    #     p.setJointMotorControl2(    #         pd_hand,    #         index,    #         controlMode=p.TORQUE_CONTROL,    #         force=taus[index]    #     )    p.stepSimulation()    time.sleep(timeStep)p.disconnect()