pitch control and yaw control

scripts/bot.py

@@ -29,20 +29,21 @@ def setpoint(self,
             self.target_pos = ((target_x, target_y, target_z), (target_roll, target_pitch, target_yaw))
         else:
             self.target_pos = target_pos
+        return target_pos
     def control(self):
         '''
         This is responsible for the bots control algorithm using PID 
         '''
         pos, orie = p.getBasePositionAndOrientation(self.id, physicsClientId = self.pClient)
-    	euler = p.getEulerFromQuaternion(orie)
+        euler = p.getEulerFromQuaternion(orie)
         feedback = control_instance(pos, euler)
         p.setJointMotorControl2(self.id,
                                 self.wheels[0],
                                 p.VELOCITY_CONTROL,
                                 targetVelocity = -max(min(15,feedback),-15),
                                 force = self.maxForce,
                                 physicsClientId = self.pClient)
-    	p.setJointMotorControl2(self.id,
+        p.setJointMotorControl2(self.id,
                                 self.wheels[1],
                                 p.VELOCITY_CONTROL,
                                 targetVelocity= -max(min(15,feedback),-15),

scripts/initial.py

@@ -0,0 +1,40 @@
+import pybullet as p
+import pybullet_data
+from time import sleep, time
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf")
+botpos=[0,0,0.1]
+bot = p.loadURDF("urdf/Paucibot.urdf",*botpos)
+p.setGravity(0,0,-10)
+numJoints = p.getNumJoints(bot)
+for joint in range(numJoints):
+	print(p.getJointInfo(bot,joint))
+wheels = [ 2, 5 ]
+targetVel = 15
+maxForce = 6
+kp, kd, ki = 0.005, 0.005, 0.000
+init = time()
+target_pos = 0
+prev_error = None
+encoder_pos = [0,0]
+while(1):
+	orie = p.getBasePositionAndOrientation(bot)[1]
+	euler = p.getEulerFromQuaternion(orie)
+	pitch = euler[1]
+	dt = time()-init
+	error = (pitch-target_pos)
+	if prev_error is None:
+		prev_error = error
+	feedback = kp*error + kd*(error - prev_error)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0)
+	print("feedback: ",feedback)
+	print("error: ", error)
+	feedback/=500
+	encoder_pos[0]-=feedback
+	encoder_pos[1]-=feedback
+	p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),)
+	p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), )
+	#print(list(p.getJointState(bot, wheel) for wheel in wheels))
+	p.stepSimulation()
+	sleep(0.05)
+	init = time()

scripts/lqr.py

@@ -0,0 +1,72 @@
+import pybullet as p
+import pybullet_data
+from time import sleep, time 
+from control import lqr
+import numpy as np
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf")
+botpos=[0,0,0.1]
+bot = p.loadURDF("urdf/Paucibot.urdf",*botpos)
+p.setGravity(0,0,-10)
+numJoints = p.getNumJoints(bot)
+for joint in range(numJoints):
+	print(p.getJointInfo(bot,joint))
+wheels = [ 2, 5 ]
+targetVel = 15
+maxForce = 6
+A = [[38.7316,12.879,5.6979,9.9588],
+	 [-104.7609,39.2818,18.0856,30.5764],
+	 [83.2656,-30.5537,-13.2043,-23.5357],
+	 [-44.2548,16.7696,6.3255,12.654]]
+
+B = [[-0.7188],
+	 [-2.0030],
+	 [1.5985],
+	 [-0.8379]]
+
+Q = [[100,0,0,0],
+     [0,1,0,0],
+     [0,0,100,0],
+     [0,0,0,1]]
+R = 0.5
+kp, kd, ki = 0.005, 0.005, 0.000
+init = time()
+target_pos = 0
+prev_error = 0
+inti_term = 0
+encoder_pos = [0,0]
+while(1):
+	orie = p.getBasePositionAndOrientation(bot)[1]
+	euler = p.getEulerFromQuaternion(orie)
+	pitch = euler[1]
+	dt = time()-init
+	error = (pitch-target_pos)
+	# if prev_error is None:
+	# 	prev_error = error
+	# feedback = kp*error + kd*(error - prev_error)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0)
+	k , s , e = lqr(A,B,Q,R)
+	print("k:   ")
+	# k = np.resize(k,(4,1))
+	print(k)
+	print(" s :")
+	print(s)
+	print(" e :")
+	print(e)
+	if abs(error)<0.01:
+		inti_term += error*dt
+	else:
+		inti_term = 0
+	feedback = -k[0] * error - k[1]*(error - prev_error) - k[2]*inti_term
+	feedback/=500
+	print("feedback: ", feedback)
+	print("error: ", error)
+	prev_error = error
+	encoder_pos[0]-=feedback
+	encoder_pos[1]-=feedback
+	p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),)
+	p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), )
+	#print(list(p.getJointState(bot, wheel) for wheel in wheels))
+	p.stepSimulation()
+	sleep(0.05)
+	init = time()

scripts/movement.py

@@ -16,7 +16,7 @@
 target_pos = 0
 prev_error = None
 while(1):
-	orie = p.getBasePositionAndOrientation(bot)[1]
+	orie = p.getBasePositionAndOrientation(bot[1])
 	euler = p.getEulerFromQuaternion(orie)
 	pitch = euler[1]
 	dt = time()-init

scripts/pitch.py

@@ -0,0 +1,52 @@
+import pybullet as p
+import pybullet_data
+from time import sleep, time
+# import control 
+# import slycot
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf")
+botpos=[0,0,0.08]
+botori = p.getQuaternionFromEuler([0, 0, 0])
+
+bot = p.loadURDF("urdf/Paucibot.urdf", *botpos, *botori)
+# bot = p.loadURDF("urdf/Paucibot.urdf",*botpos)
+p.setGravity(0,0,-10)
+numJoints = p.getNumJoints(bot)
+for joint in range(numJoints):
+	print(p.getJointInfo(bot,joint))
+wheels = [ 2, 5 ]
+targetVel = 15
+maxForce = 6
+kp, kd, ki = 255,26,34
+init = time()
+target_pos = 0.0
+prev_error = 0
+inti_term = 0
+encoder_pos = [0,0]
+while(1):
+    orie = p.getBasePositionAndOrientation(bot)[1]
+    euler = p.getEulerFromQuaternion(orie)
+    pitch = euler[1]
+    dt = time()-init
+    error = (pitch-target_pos)
+    #k = control.lqr(A,B,Q,R)
+    if abs(error)<0.01:
+        inti_term += error*dt
+    else:
+        inti_term = 0
+    feedback = kp*error + kd*(error - prev_error)/dt + ki*inti_term
+    prev_error = error
+    print("error: ", error)
+    feedback/=500
+    #feedback = - k * error
+    print("feedback: ",feedback)
+    encoder_pos[0] -= feedback
+    encoder_pos[1] -= feedback
+    p.setJointMotorControl2(bot, wheels[0], p.VELOCITY_CONTROL, targetVelocity=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),)
+    p.setJointMotorControl2(bot, wheels[1], p.VELOCITY_CONTROL, targetVelocity=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), )
+	#print(list(p.getJointState(bot, wheel) for wheel in wheels))
+    p.stepSimulation()
+    sleep(0.05)
+    init = time()
+

scripts/position.py

@@ -0,0 +1,70 @@
+import pybullet as p
+import pybullet_data
+from time import sleep, time
+import numpy as np
+
+def magnitude(a):
+    return (a[0]**2+a[1]**2)**0.5
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+p.loadURDF("plane.urdf")
+botpos=[0,0,0.1]
+bot = p.loadURDF("urdf/Paucibot.urdf",*botpos)
+p.setGravity(0,0,-10)
+numJoints = p.getNumJoints(bot)
+for joint in range(numJoints):
+	print(p.getJointInfo(bot,joint))
+wheels = [ 2, 5 ]
+targetVel = 15
+maxForce = 6
+kp, kd, ki = 0.005, 0.005, 0.000
+init = time()
+target_x = 0
+target_pitch =0
+prev_error = 0
+prev_error_pitch = 0
+encoder_pos = [0,0]
+encoder_vel = [0,0]
+while(1):
+    posi = p.getBasePositionAndOrientation(bot)[0]
+    orie = p.getBasePositionAndOrientation(bot)[1]
+    euler = p.getEulerFromQuaternion(orie)
+    pitch = euler[1]
+    dt = time()-init
+    p1 = p.getLinkState(bot,9)[0]
+    p2 = p.getLinkState(bot,10)[0]
+    p3 = p.getLinkState(bot,11)[0]
+    p1 = np.array(p1)
+    p2 = np.array(p2)
+    p3 = np.array(p3)
+    direc = p1- (p2+p3)/2
+    sum = direc + posi
+    sum = magnitude(sum)
+    direc = magnitude(direc)
+    posi = magnitude(posi)
+    error = sum - direc -posi
+    feedback = kp*error + kd*(error - prev_error)/dt
+    feedback/=10
+    error_pitch = (pitch-target_pitch)
+    feedback1 = kp*error_pitch + kd*(error_pitch - prev_error_pitch)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0)
+    prev_error_pitch = error_pitch
+    feedback1/=500
+    if (error_pitch<0.01 and error_pitch>-0.01):
+        encoder_vel[0] = -feedback
+        encoder_vel[1] = feedback
+        print("direction: ", feedback, error)
+        p.setJointMotorControl2(bot, wheels[0], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),)
+        p.setJointMotorControl2(bot, wheels[1], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), )
+    else:
+        encoder_pos[0]-=feedback1 
+        encoder_pos[1]-=feedback1 
+        print("pitch: ", feedback1, error_pitch)
+        p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),)
+        p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), )
+	#print(list(p.getJointState(bot, wheel) for wheel in wheels))
+    prev_error = error
+    p.stepSimulation()
+    sleep(0.05)
+    init = time()
+

scripts/trial.py

@@ -1,39 +1,70 @@
 import pybullet as p
 import pybullet_data
 from time import sleep, time
+# import control 
+# import slycot
 p.connect(p.GUI)
 p.setAdditionalSearchPath(pybullet_data.getDataPath())
 p.loadURDF("plane.urdf")
-botpos=[0,0,0.1]
-bot = p.loadURDF("urdf/Paucibot.urdf",*botpos)
+botpos=[0,0,0.08]
+botori = p.getQuaternionFromEuler([0, 0, 0])
+
+bot = p.loadURDF("urdf/Paucibot.urdf", *botpos, *botori)
+# bot = p.loadURDF("urdf/Paucibot.urdf",*botpos)
 p.setGravity(0,0,-10)
 numJoints = p.getNumJoints(bot)
 for joint in range(numJoints):
 	print(p.getJointInfo(bot,joint))
 wheels = [ 2, 5 ]
 targetVel = 15
 maxForce = 6
-kp, kd, ki = 0.005, 0.005, 0.000
+kp, kd, ki = 255,26,34
 init = time()
-target_pos = 0
-prev_error = None
+target_pitch = 0.0
+target_yaw = 0
+prev_error = 0
+prev_error1 = 0
+inti_term = 0
+inti_term1 = 0
 encoder_pos = [0,0]
+encoder_vel = [0,0]
 while(1):
 	orie = p.getBasePositionAndOrientation(bot)[1]
 	euler = p.getEulerFromQuaternion(orie)
 	pitch = euler[1]
+	yaw = euler[2]
 	dt = time()-init
-	error = (pitch-target_pos)
-	if prev_error is None:
-		prev_error = error
-	feedback = kp*error + kd*(error - prev_error)/dt + (ki*dt*error/abs(error+1e-6) if abs(error)<0.01 else 0)
-	#print(feedback, pitch)
+	error = (pitch-target_pitch)
+	error1 = yaw -target_yaw
+    #k = control.lqr(A,B,Q,R)
+	if abs(error)<0.01:
+		inti_term += error*dt
+		if abs(error1)<0.01:
+			inti_term1 += error1*dt
+		else:
+			inti_term1 = 0
+		feedback1 = 1*error1 + 1*(error1 - prev_error1)/dt + 1*inti_term1
+		prev_error1 = error1
+		print("Yaw control - error:", error1," feedback:", feedback1)
+		encoder_vel[0] = -feedback1
+		encoder_vel[1] = feedback1
+		p.setJointMotorControl2(bot, wheels[0], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[0], force=maxForce)
+		p.setJointMotorControl2(bot, wheels[1], p.VELOCITY_CONTROL, targetVelocity=encoder_vel[1], force=maxForce)
+		p.stepSimulation()
+	else:
+		inti_term = 0
+	feedback = 255*error + 26*(error - prev_error)/dt + 34*inti_term
+	prev_error = error
+	print("error: ", error)
 	feedback/=500
-	encoder_pos[0]-=feedback
-	encoder_pos[1]-=feedback
+    #feedback = - k * error
+	print("feedback: ",feedback)
+	encoder_pos[0] -= feedback
+	encoder_pos[1] -= feedback
 	p.setJointMotorControl2(bot, wheels[0], p.POSITION_CONTROL, targetPosition=encoder_pos[0], force=maxForce)# targetVelocity=-max(min(15,feedback),-15),)
 	p.setJointMotorControl2(bot, wheels[1], p.POSITION_CONTROL, targetPosition=encoder_pos[1], force=maxForce)# targetVelocity=-max(min(15,feedback),-15), )
 	#print(list(p.getJointState(bot, wheel) for wheel in wheels))
 	p.stepSimulation()
 	sleep(0.05)
 	init = time()
+