How to use the pybullet.setJointMotorControl2 function in pybullet

position,orientation = p.getBasePositionAndOrientation(body)
print position

num_joints = p.getNumJoints(body)

minLimit = -math.pi/2
maxLimit = math.pi/2

p_gain =2
speed = 5

t = 0
while True:
	print math.sin(t*speed)/2*(maxLimit-minLimit)+minLimit
	for i in range(num_joints):
		p.setJointMotorControl2(body,i,p.POSITION_CONTROL,math.sin(t*speed)/2*(maxLimit-minLimit),p_gain)	
	p.stepSimulation()
	t = t + fixedTimeStep

	print p.getBasePositionAndOrientation(body)
	
	if t > 2:
		print "resetting position..."
		p.resetBasePositionAndOrientation(body, position, (0,0,0,1))
		
		print "resetting joint states..."
		for i in range(num_joints):
			p.resetJointState(body,i,0)
		t = 0

	print t

self.end_effector_angle += motor_commands[7]
            finger_angle = motor_commands[8]

        if self.use_simulation:
            # using dynamic control
            for i in range(self.kuka_end_effector_index + 1):
                p.setJointMotorControl2(bodyUniqueId=self.kuka_uid, jointIndex=i, controlMode=p.POSITION_CONTROL,
                                        targetPosition=joint_poses[i], targetVelocity=0, force=self.max_force,
                                        maxVelocity=self.max_velocity, positionGain=0.3, velocityGain=1)
        else:
            # reset the joint state (ignoring all dynamics, not recommended to use during simulation)
            for i in range(self.kuka_end_effector_index + 1):
                p.resetJointState(self.kuka_uid, i, joint_poses[i])

        # Effectors grabbers angle
        p.setJointMotorControl2(self.kuka_uid, 7, p.POSITION_CONTROL, targetPosition=self.end_effector_angle,
                                force=self.max_force)
        p.setJointMotorControl2(self.kuka_uid, 8, p.POSITION_CONTROL, targetPosition=-finger_angle,
                                force=self.fingerA_force)
        p.setJointMotorControl2(self.kuka_uid, 11, p.POSITION_CONTROL, targetPosition=finger_angle,
                                force=self.fingerB_force)

        p.setJointMotorControl2(self.kuka_uid, 10, p.POSITION_CONTROL, targetPosition=0,
                                force=self.finger_tip_force)
        p.setJointMotorControl2(self.kuka_uid, 13, p.POSITION_CONTROL, targetPosition=0,
                                force=self.finger_tip_force)

applied {dict} -- dict of joint states (position, velocity, reaction forces, applied torque)
        """
        applied = {}

        for name in joints.keys():
            if name in self.joints:
                if name.endswith('_speed'):
                    p.setJointMotorControl2(
                    self.robot, self.joints[name], p.VELOCITY_CONTROL, targetVelocity=joints[name])
                else:
                    if name in self.maxTorques:
                        maxTorque = self.maxTorques[name]
                        p.setJointMotorControl2(
                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name], force=maxTorque)
                    else:
                        p.setJointMotorControl2(
                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name])

                applied[name] = p.getJointState(self.robot, self.joints[name])
            else:
                raise Exception("Can't find joint %s" % name)

        return applied

def gripper(self, cmd, mode=pb.POSITION_CONTROL):
        '''
        Gripper commands need to be mirrored to simulate behavior of the actual
        UR5.
        '''
        pb.setJointMotorControl2(self.handle, self.left_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(
            self.handle, self.left_inner_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.left_finger, mode,  cmd)
        pb.setJointMotorControl2(self.handle, self.left_fingertip, mode,  cmd)

        pb.setJointMotorControl2(self.handle, self.right_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(
            self.handle, self.right_inner_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.right_finger, mode,  cmd)
        pb.setJointMotorControl2(self.handle, self.right_fingertip, mode,  cmd)

#p.loadURDF("plane_transparent2.urdf")
p.loadURDF("plane.urdf")
dobot = p.loadURDF("dobot/dobot.urdf",useFixedBase=True)
p.setRealTimeSimulation(1)
p.setPhysicsEngineParameter(numSolverIterations=300)
p.setPhysicsEngineParameter(numSubSteps=10)

for i in range (p.getNumJoints(dobot)):
	print(p.getJointInfo(dobot,i))
	p.setJointMotorControl2(dobot,i,p.VELOCITY_CONTROL,targetVelocity=0,force=20)
	
#p.setJointMotorControl2(dobot,1,p.VELOCITY_CONTROL,targetVelocity=0.1,force=100)
#p.setJointMotorControl2(dobot,2,p.VELOCITY_CONTROL,targetVelocity=0.,force=100)

p.setJointMotorControl2(dobot,0,p.VELOCITY_CONTROL,targetVelocity=0,force=1000)
p.setJointMotorControl2(dobot,3,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.setJointMotorControl2(dobot,5,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.setJointMotorControl2(dobot,6,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.setJointMotorControl2(dobot,8,p.VELOCITY_CONTROL,targetVelocity=0,force=0)

p.resetJointState(dobot,6,-1.57)
p.resetJointState(dobot,7,-1.57)
p.resetJointState(dobot,8,1.57)

#colors for debugging
#p.changeVisualShape(dobot,4,rgbaColor=[1,1,1,1])
#p.changeVisualShape(dobot,3,rgbaColor=[0,1,0,1])
#p.changeVisualShape(dobot,6,rgbaColor=[0,1,0,1])
#p.changeVisualShape(dobot,5,rgbaColor=[1,1,0,1])

c = p.createConstraint(dobot,6,dobot,8,jointType=p.JOINT_POINT2POINT,jointAxis =[1,0,0],parentFramePosition=[-0.05,0,0.08],childFramePosition=[-0.05,0.026,-0.006])#0.014-0.02 (inertial com)

t5 = mult*time.clock()
		
	#if ((frameNr % 32) == 0):
	#	p.removeUserDebugItem(lines[i])
	#	for i in range (numLines):
	#		spacing = 0.01
	#		textPos = [.1-(i+1)*spacing,.1,0.011]
	#		text = "Demo:"+demos[currentDemo][0]+" Frame:"+str(frameNr)+"\nObject UID:"+objectInfo
	#		textUid = p.addUserDebugText(text,textColorRGB=[0,0,0], textSize = 0.02, textPosition = textPos,textOrientation = textOrn,parentObjectUniqueId=uiCube, parentLinkIndex = -1)
	#		lines[i] = textUid
	
	
	#keep the gripper centered/symmetric
	b = p.getJointState(pr2_gripper,2)[0]
	p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
	
	t6 = mult*time.clock()
	
	events = p.getVREvents()
	
	t7 = mult*time.clock()
	
	#print ("len events=",len(events))
	for e in (events):
		if e[CONTROLLER_ID] == uiControllerId:
			p.resetBasePositionAndOrientation(uiCube,e[POSITION], e[ORIENTATION])
			if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED ):
				currentDemo = currentDemo+1
				if (currentDemo>=len(demos)):
					currentDemo = 0

def gripper(self, cmd, mode=pb.POSITION_CONTROL):
        '''
        Gripper commands need to be mirrored to simulate behavior of the actual
        UR5.
        '''
        pb.setJointMotorControl2(self.handle, self.left_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.left_inner_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.left_finger, mode,  cmd)
        pb.setJointMotorControl2(self.handle, self.left_fingertip, mode,  cmd)

        pb.setJointMotorControl2(self.handle, self.right_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.right_inner_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.right_finger, mode,  cmd)
        pb.setJointMotorControl2(self.handle, self.right_fingertip, mode,  cmd)

pybullet.setJointMotorControl2(
        self.kukaUid, 7, pybullet.POSITION_CONTROL,
        targetPosition=self.endEffectorAngle, force=self.maxForce,
        physicsClientId=self.cid)
    pybullet.setJointMotorControl2(
        self.kukaUid, 8, pybullet.POSITION_CONTROL,
        targetPosition=-fingerAngle, force=self.fingerAForce,
        physicsClientId=self.cid)
    pybullet.setJointMotorControl2(
        self.kukaUid, 11, pybullet.POSITION_CONTROL,
        targetPosition=fingerAngle, force=self.fingerBForce,
        physicsClientId=self.cid)
    pybullet.setJointMotorControl2(
        self.kukaUid, 10, pybullet.POSITION_CONTROL, targetPosition=0,
        force=self.fingerTipForce, physicsClientId=self.cid)
    pybullet.setJointMotorControl2(
        self.kukaUid, 13, pybullet.POSITION_CONTROL, targetPosition=0,
        force=self.fingerTipForce,physicsClientId=self.cid)

def gripper(self, cmd, mode=pb.POSITION_CONTROL):
        '''
        Gripper commands need to be mirrored to simulate behavior of the actual
        UR5.
        '''
        pb.setJointMotorControl2(self.handle, self.left_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(
            self.handle, self.left_inner_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.left_finger, mode,  cmd)
        pb.setJointMotorControl2(self.handle, self.left_fingertip, mode,  cmd)

        pb.setJointMotorControl2(self.handle, self.right_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(
            self.handle, self.right_inner_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.right_finger, mode,  cmd)
        pb.setJointMotorControl2(self.handle, self.right_fingertip, mode,  cmd)