How to use the pybullet.POSITION_CONTROL function in pybullet

def reset(self):
        """
        Reset the environment
        """
        objects = p.loadSDF(os.path.join(self.urdf_root_path, "kuka_iiwa/kuka_with_gripper2.sdf"))
        self.kuka_uid = objects[0]

        p.resetBasePositionAndOrientation(self.kuka_uid, [-0.100000, 0.000000, -0.15],
                                          [0.000000, 0.000000, 0.000000, 1.000000])
        self.joint_positions = [0.006418, 0.113184, -0.011401, -1.289317, 0.005379, 1.737684, -0.006539, 0.000048,
                                -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200]
        self.num_joints = p.getNumJoints(self.kuka_uid)
        for jointIndex in range(self.num_joints):
            p.resetJointState(self.kuka_uid, jointIndex, self.joint_positions[jointIndex])
            p.setJointMotorControl2(self.kuka_uid, jointIndex, p.POSITION_CONTROL,
                                    targetPosition=self.joint_positions[jointIndex], force=self.max_force)

        self.end_effector_pos = np.array([0.537, 0.0, 0.5])
        self.end_effector_angle = 0

        self.motor_names = []
        self.motor_indices = []

        for i in range(self.num_joints):
            joint_info = p.getJointInfo(self.kuka_uid, i)
            q_index = joint_info[3]
            if q_index > -1:
                self.motor_names.append(str(joint_info[1]))
                self.motor_indices.append(i)

    def gripper(self, cmd, mode=pb.POSITION_CONTROL):
        '''
        Gripper commands need to be mirrored to simulate behavior of the actual
        UR5. Converts one command input to 6 joint positions, used for the
        robotiq gripper. This is a rough simulation of the way the robotiq
        gripper works in practice, in the absence of a plugin like the one we
        use in Gazebo.

        Parameters:
        -----------
        cmd: 1x1 array of floating point position commands in [-0.8, 0]
        mode: PyBullet control mode
        '''

        cmd = cmd[0]
        # This is actually only a 1-DOF gripper
        if cmd < -0.1:

    def arm(self, cmd, mode=pb.POSITION_CONTROL):
        '''
        Set joint commands for the robot arm.
        '''
        if len(cmd) > 6:
            raise RuntimeError('too many joint positions')
        for i, q in enumerate(cmd):
            pb.setJointMotorControl2(self.handle, i, mode, q)

def _setupRobot(self, handle):
        '''
        Configure the robot so that it is ready to begin the task. Robot should
        be oriented so the gripper is near the cluttered area with the mug.
        '''
        self.robot.place([0, 0, 0], [0, 0, 0, 1], self.joint_positions)
        self.robot2 = self.cloneRobot()
        self.robot2.load()
        self.robot2.place([-1, 0, 0], [0, 0, 1, 0],
                          self.joint_positions)
        self.robot.arm(self.joint_positions, pb.POSITION_CONTROL)
        self.robot.gripper(0, pb.POSITION_CONTROL)

def set_gripper_open_position(self, indices, positions, set_instantly=False, force=500):
        if set_instantly:
            self.set_joint_angles(indices, positions, use_limits=True)
        p.setJointMotorControlArray(self.body, jointIndices=indices, controlMode=p.POSITION_CONTROL, targetPositions=positions, positionGains=np.array([0.05]*len(indices)), forces=[force]*len(indices), physicsClientId=self.id)

p.setPhysicsEngineParameter(solverResidualThreshold=1e-2)

index = 0
numX = 10
numY = 10

for i in range (numX):
	for j in range (numY):
		print("loading animal ", index)
		index+=1
		
		#anymal = p.loadURDF("atlas/atlas_v4_with_multisense.urdf",[i*3,j*3,1], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
		anymal = p.loadURDF("ANYmal/robot.urdf",[(i-numX/2)*2,(j-numY/2)*2,0.6], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES, useMaximalCoordinates=False)
		
		for j in range(p.getNumJoints(anymal)):
			p.setJointMotorControl2(anymal,j,p.POSITION_CONTROL,targetPosition=0, force=500)

p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)


p.setGravity(0,0,-10)
#p.setRealTimeSimulation(1)

while (1):
	p.stepSimulation()
	time.sleep(1./240.)

def _setupRobot(self, handle):
        self.robot.place([0, 0, 0], [0, 0, 0, 1], self.joint_positions)
        self.robot.arm(self.joint_positions, pb.POSITION_CONTROL)
        self.robot.gripper(0, pb.POSITION_CONTROL)

def control_joints(body, joints, positions):
    # TODO: the whole PR2 seems to jitter
    #kp = 1.0
    #kv = 0.3
    #forces = [get_max_force(body, joint) for joint in joints]
    #forces = [5000]*len(joints)
    #forces = [20000]*len(joints)
    return p.setJointMotorControlArray(body, joints, p.POSITION_CONTROL,
                                       targetPositions=positions,
                                       targetVelocities=[0.0] * len(joints),
                                       physicsClientId=CLIENT) #,
                                        #positionGains=[kp] * len(joints),

def _setupRobot(self, handle):
        self.robot.place([0, 0, 0], [0, 0, 0, 1], self.joint_positions)
        self.robot.arm(self.joint_positions, pb.POSITION_CONTROL)
        self.robot.gripper(0, pb.POSITION_CONTROL)

def set_position(self, position):
        p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)