From df97475a651f7c4b53fb680815489439d4227fe8 Mon Sep 17 00:00:00 2001 From: ramnreddy15 Date: Sun, 28 Mar 2021 11:08:42 -0700 Subject: [PATCH] PID test --- .../controllers/olympian/standingBalance.py | 143 ++++++++++++------ simulation/worlds/.olympian.wbproj | 7 +- simulation/worlds/olympian.wbt | 9 +- 3 files changed, 104 insertions(+), 55 deletions(-) diff --git a/simulation/controllers/olympian/standingBalance.py b/simulation/controllers/olympian/standingBalance.py index fb51131..3ba4694 100644 --- a/simulation/controllers/olympian/standingBalance.py +++ b/simulation/controllers/olympian/standingBalance.py @@ -2,7 +2,8 @@ # You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor -from controller import Robot, Accelerometer, Gyro +from controller import Robot, Accelerometer, Gyro, TouchSensor, Supervisor +import time def integral(error, priorIntegral): return priorIntegral + error * (TIMESTEP) @@ -12,6 +13,26 @@ def derivative(error, priorError): def actuatorMovement(robot, pidOutput): #Inverse Kinematic Equation + torso_pitch = robot.getDevice("torso_pitch") + right_knee_pitch = robot.getDevice("left_hip_pitch") + left_knee_pitch = robot.getDevice("right_hip_pitch") + + if pidOutput[0] > 0.5: + torso_pitch.setVelocity(1000) + right_knee_pitch.setVelocity(1000) + left_knee_pitch.setVelocity(1000) + torso_pitch.setPosition(-1.3) + right_knee_pitch.setPosition(-1.3) + left_knee_pitch.setPosition(-1.3) + print("here1") + elif pidOutput[0] < -0.5: + torso_pitch.setVelocity(1000) + right_knee_pitch.setVelocity(1000) + left_knee_pitch.setVelocity(1000) + torso_pitch.setPosition(1.3) + right_knee_pitch.setPosition(1.3) + left_knee_pitch.setPosition(1.3) + print("here2") return def controllerPID(robot, error, priorError, priorIntegral): @@ -28,9 +49,9 @@ def controllerPID(robot, error, priorError, priorIntegral): nominalValue = 0.0 Kc = 1.0 # Kc is the controller gain - tauI = 0.0 # tauI is the reset time, which is a tuning param for integral + tauI = 1.0 # tauI is the reset time, which is a tuning param for integral tauD = 0.0 # tauD is derivative time. Tuning param for derivative - maxMotor = 1.0 # How big the signal that goes to the actuator is + maxMotor = 1000.0 # How big the signal that goes to the actuator is minMotor = 0.0 # How small the signal that goes to the actuator is integralX = integral(error[0], priorIntegral[0]) @@ -65,6 +86,9 @@ def controllerPID(robot, error, priorError, priorIntegral): priorError = error priorIntegral = [integralX, integralY] + print("ux " + str(ux)) + print("uy " + str(uy)) + actuatorMovement(robot, [ux, uy]) @@ -77,52 +101,22 @@ def calculateZMP(gyro, accel): gravity = 9.81 xObs = -CoM_height/gravity * aData[0] - yObs = -CoM_height/gravity * aData[1] + yObs = -CoM_height/gravity * aData[2] return xObs, yObs def calculateCOM(robot): - # links needed to calculate COM - torso = robot.getDevice("torsoMotor") - upperLeftLeg= robot.getDevice("upperLeftLegMotor") - upperRightLeg = robot.getDevice("upperRightLegMotor") - - links = [torso, upperLeftLeg, upperRightLeg] # additional links will be added, when they're made - - totalMass = len(links) # assumption that all links have mass of 1 - centerOfMass = [0, 0, 0, 0] - - for i in links: - # instantiation of values - # need to add code to get joint angles - if i == torso: - r = 0.25 - theta0 = 0 - theta1 = 0 - else: - r = 0.225 - theta0 = 0 - theta1 = 0 + links = ["left_foot", "left_shin", "left_thigh", "left_lower_arm", "left_upper_arm", "right_foot", "right_shin", + "right_thigh", "right_lower_arm", "right_upper_arm", "torso", "head", "pelvis"] - # instantiation of matrices - # assumption that all angle values are in degrees - transformationMatrixToLink = [[np.cos(theta0*np.pi/180), -np.sin(theta0*np.pi/180), - 0, 2r*np.cos(theta0*np.pi/180)], [np.sin(theta0*np.pi/180), - np.cos(theta0*np.pi/180), 0, 2r*np.sin(theta0*np.pi/180)], - [0, 0, 1, 0], [0, 0, 0, 1]] - vectorOfJointAngles = [2r*np.cos(theta1*np.pi/180), 2r*np.sin(theta1*np.pi/180), 0, 1] - transformationMatrixToLinkCOM = [[np.cos(theta1*np.pi/180), 0, -np.sin(theta1*np.pi/180), - 2r*np.cos(theta0*np.pi/180)], [0, 1, 0, 0], - [np.sin(theta1*np.pi/180), 0, np.cos(theta1*np.pi/180), - 2r*np.sin(theta0*np.pi/180)], [0, 0, 0, 1]] + centerOfMass = [0, 0, 0] - # COM of each link is calculated forward kinematics equations - calc = transformationMatrixToLink.dot(vectorOfJointAngles).dot(transformationMatrixToLinkCOM) + for i in links: + temp = Supervisor.getFromDef(i) + tempCOM = temp.getCenterOfMass() + centerOfMass += tempCOM - # COM of the entire rigid body is calculated using a weighted average - centerOfMass += (1/totalMass)*calc - - return centerOfMass[0], centerOfMass[1], centerOfMass[2] + return centerOfMass/8.18 def main(): print("Initializing Olympiad...") @@ -131,14 +125,14 @@ def main(): # get the time step of the current world. global TIMESTEP TIMESTEP = int(robot.getBasicTimeStep()) - + # gyroscope, accelorometer - gyro = robot.getDevice("gyro.wbt name") - accel = robot.getDevice("accel .wbt name") + gyro = robot.getDevice("torso_gyro") + accel = robot.getDevice("torso_accelerometer") gyro.enable(TIMESTEP) accel.enable(TIMESTEP) - + # If the ZMP is stable then there will be no trajectory # Assumes that the base-frame-origin is in between the two feet since it's standing # This stays under the x and y coor of the COM (assuming standing straight) @@ -155,4 +149,59 @@ def main(): if __name__ == '__main__': - main() \ No newline at end of file + main() + + +# def main(): + #create the Robot instance. + # print("Initializing world...") + # robot = Robot() + # timestep = 16 + # curr_time = 0 + + # lfootsensor = robot.getDevice("torso_gyro") + + + # while robot.step(timestep) != -1: + #print(calculateCOM(robot)) + # curr_time += timestep/1000.0 + #head_motor = robot.getDevice("torso_yaw") + # motor2 = robot.getDevice("neck_roll") + # lknee = robot.getDevice("left_knee_pitch") + # rknee = robot.getDevice("right_knee_pitch") + # lhip = robot.getDevice("left_hip_pitch") + # rhip = robot.getDevice("right_hip_pitch") + # lfootsensor.enable(10) + + + #print sensor data every second + + # if curr_time % 1 < 0.01: + + # print(lfootsensor.getValues()) + + #lfoot = robot.getDevice("left_ankle_pitch") + #rfoot = robot.getDevice("right_ankle_pitch") + +def func(): + + torso_pitch = robot.getDevice("torso_pitch") + + lknee.setVelocity(2) + lknee.setPosition(1) + torso_pitch.setVelocity(2) + + + lhip.setVelocity(2) + rhip.setVelocity(2) + lhip.setPosition(-1) + torso_pitch.setPosition(0.2) + + + if curr_time > 1: + lknee.setPosition(0.1) + lhip.setPosition(0.5) + torso_pitch.setPosition(0.7) + + +main() \ No newline at end of file diff --git a/simulation/worlds/.olympian.wbproj b/simulation/worlds/.olympian.wbproj index 36e2b80..4e9b267 100644 --- a/simulation/worlds/.olympian.wbproj +++ b/simulation/worlds/.olympian.wbproj @@ -1,11 +1,10 @@ Webots Project File version R2021a -perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000023a000003b6fc0200000002fb0000001400540065007800740045006400690074006f0072010000001a000002e30000008700fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000002ff000000d10000003900ffffff0000000300000780000001e8fc0100000001fb0000000e0043006f006e0073006f006c0065010000000000000740000000000000000000000544000003b600000004000000040000000100000008fc00000000 -simulationViewPerspectives: 000000ff0000000100000002000001dd000003650100000002010000000101 +perspectives: 000000ff00000000fd000000030000000000000255000003ccfc0100000003fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900fffffffb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000002550000006900ffffff00000001000003ae000003ccfc0200000002fb0000001400540065007800740045006400690074006f00720000000016000002f90000008900fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000016000003cc00000000000000000000000300000780000001e8fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007400000000000000000000000000000000000000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff0000000100000002000001e6000000bd0100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201 maximizedDockId: -1 -centralWidgetVisible: 1 +centralWidgetVisible: 0 orthographicViewHeight: 1 textFiles: 1 "controllers/olympian/standingBalance.py" "controllers/olympian/olympian.py" globalOptionalRendering: CoordinateSystem::AllBoundingObjects::ContactPoints::ConnectorAxes::JointAxes::Skeleton centerOfMass: robot -consoles: Console:All:All diff --git a/simulation/worlds/olympian.wbt b/simulation/worlds/olympian.wbt index 5c156eb..80e5847 100644 --- a/simulation/worlds/olympian.wbt +++ b/simulation/worlds/olympian.wbt @@ -130,7 +130,7 @@ Robot { } RotationalMotor { name "left_ankle_yaw" - maxTorque 100 + maxTorque 10000 } ] device2 [ @@ -149,7 +149,7 @@ Robot { } RotationalMotor { name "left_ankle_pitch" - maxTorque 100 + maxTorque 10000 } ] endPoint Solid { @@ -344,7 +344,7 @@ Robot { } RotationalMotor { name "right_ankle_yaw" - maxTorque 100 + maxTorque 10000 } ] device2 [ @@ -363,7 +363,7 @@ Robot { } RotationalMotor { name "right_ankle_pitch" - maxTorque 100 + maxTorque 10000 } ] endPoint Solid { @@ -484,6 +484,7 @@ Robot { } RotationalMotor { name "torso_pitch" + maxVelocity 100000 } ] device2 [