From 729e360d91ad4196a43d96f72fcfaecd27976847 Mon Sep 17 00:00:00 2001 From: Anish Suvarna Date: Sun, 19 Dec 2021 15:59:13 -0500 Subject: [PATCH] Added a new file with a function for the PID controller --- PIDFunction.py | 108 +++++++++++++++++++++++++++++++++++++++ PID.py => PIDsinCurve.py | 51 +++++++++++++----- 2 files changed, 145 insertions(+), 14 deletions(-) create mode 100644 PIDFunction.py rename PID.py => PIDsinCurve.py (66%) diff --git a/PIDFunction.py b/PIDFunction.py new file mode 100644 index 0000000..64e9147 --- /dev/null +++ b/PIDFunction.py @@ -0,0 +1,108 @@ +import pybullet as p +import math +import time +import pybullet_data +import math +import matplotlib.pyplot as plt +import numpy as np +physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally +p.setGravity(0,0,-10) +planeId = p.loadURDF("plane.urdf") +startPos = [0,0,0] +startOrientation = p.getQuaternionFromEuler([0,0,0]) +robotId = p.loadURDF("olympian.urdf",startPos, startOrientation, + # useMaximalCoordinates=1, ## New feature in Pybullet + flags=p.URDF_USE_INERTIA_FROM_FILE) + +xValues = np.linspace(0, 20, 200) +yValues = [math.sin(i) for i in xValues] +yValues = [i * 0.2 for i in yValues] +plt.plot(xValues, yValues) +plt.show() +print(yValues) + +def calcCOM(): + + #CALCULATE COM + masstimesxpossum = 0.0 + masstimesypossum = 0.0 + masstimeszpossum = 0.0 + masssum = 0.0 + for i in range(0, p.getNumJoints(robotId) -1): + + # if(i >= 0): + # print(p.getJointInfo(robotId, i)[0:13]) + + wheight = p.getDynamicsInfo(robotId, i)[0] + xpos = p.getLinkState(robotId, i)[0][0] + ypos = p.getLinkState(robotId, i)[0][1] + zpos = p.getLinkState(robotId, i)[0][2] + + masstimesxpossum += (wheight * xpos) + masstimesypossum += (wheight * ypos) + masstimeszpossum += (wheight * zpos) + masssum += wheight + + # print(wheight) + # print(xpos) + # print(ypos) + # print(zpos) + # print("\n") + p.stepSimulation() + com = (masstimesxpossum/masssum, masstimesypossum/masssum, masstimeszpossum/masssum) + + return com + +def footError(): + robotCOM = calcCOM() + realRobotCOM = robotCOM[1] + footCOM = p.getLinkState(robotId, 15) + realFootCOM = footCOM[0][1] + error = realRobotCOM - realFootCOM + print(realFootCOM, realRobotCOM) + return error + +def error(targetValue): + robotCOM = calcCOM() + realRobotCOM = robotCOM[1] + error = realRobotCOM - targetValue + print(targetValue, realRobotCOM, error) + return error + +Array = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] +positions = [0]*23 +forces = [1000]*23 +indexes = list(range(0,23)) + +# p.setJointMotorControl2(robotId, 20, p.POSITION_CONTROL, targetPosition = 0, force = 10000) +# p.setJointMotorControl2(robotId, 14, p.POSITION_CONTROL, targetPosition = 0, force = 10000) +doneYs = [] +initial_error = 50 + +ka = 0 + +def PIDsinCurve(i): + + doneYs.append(i) + # print(doneYs) + treshold = 0.01 + initial_error = 50 + while(abs(initial_error) > treshold): + print("Inside the loop, current y value is " + str(i)) + Kc = -0.5 + bias = 0 + initial_error = error(i) + global ka + ka += Kc*initial_error + bias + print("error" + str(initial_error)) + positions[21] = ka + positions[15] = ka + positions[18] = ka + positions[12] = ka + p.setJointMotorControlArray(robotId, indexes, p.POSITION_CONTROL, targetPositions = positions, forces = forces) + p.stepSimulation() + +for i in yValues: + PIDsinCurve(i) \ No newline at end of file diff --git a/PID.py b/PIDsinCurve.py similarity index 66% rename from PID.py rename to PIDsinCurve.py index 86891f8..a0bac93 100644 --- a/PID.py +++ b/PIDsinCurve.py @@ -1,8 +1,10 @@ import pybullet as p +import math import time import pybullet_data import math import matplotlib.pyplot as plt +import numpy as np physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally @@ -14,6 +16,12 @@ robotId = p.loadURDF("olympian.urdf",startPos, startOrientation, # useMaximalCoordinates=1, ## New feature in Pybullet flags=p.URDF_USE_INERTIA_FROM_FILE) +xValues = np.linspace(0, 20, 200) +yValues = [math.sin(i) for i in xValues] +yValues = [i * 0.2 for i in yValues] +plt.plot(xValues, yValues) +plt.show() +print(yValues) def calcCOM(): @@ -45,10 +53,9 @@ def calcCOM(): p.stepSimulation() com = (masstimesxpossum/masssum, masstimesypossum/masssum, masstimeszpossum/masssum) - return com -def error(): +def footError(): robotCOM = calcCOM() realRobotCOM = robotCOM[1] footCOM = p.getLinkState(robotId, 15) @@ -57,22 +64,38 @@ def error(): print(realFootCOM, realRobotCOM) return error +def error(targetValue): + robotCOM = calcCOM() + realRobotCOM = robotCOM[1] + error = realRobotCOM - targetValue + print(targetValue, realRobotCOM, error) + return error + +ka = 0 Array = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] positions = [0]*23 forces = [1000]*23 indexes = list(range(0,23)) -ka = 0 + # p.setJointMotorControl2(robotId, 20, p.POSITION_CONTROL, targetPosition = 0, force = 10000) # p.setJointMotorControl2(robotId, 14, p.POSITION_CONTROL, targetPosition = 0, force = 10000) +doneYs = [] +initial_error = 50 -while(True): - Kc = -0.05 - bias = 0 - initial_error = error() - ka += Kc*initial_error + bias - print(initial_error) - positions[21] = ka - positions[15] = ka - positions[18] = ka - positions[12] = ka - p.setJointMotorControlArray(robotId, indexes, p.POSITION_CONTROL, targetPositions = positions, forces = forces) \ No newline at end of file +for i in yValues: + doneYs.append(i) + # print(doneYs) + treshold = 0.01 + initial_error = 50 + while(abs(initial_error) > treshold): + print("Inside the loop, current y value is " + str(i)) + Kc = -0.5 + bias = 0 + initial_error = error(i) + ka += Kc*initial_error + bias + print("error" + str(initial_error)) + positions[21] = ka + positions[15] = ka + positions[18] = ka + positions[12] = ka + p.setJointMotorControlArray(robotId, indexes, p.POSITION_CONTROL, targetPositions = positions, forces = forces) \ No newline at end of file