From 12c5cc15ee01169f8568c6a31f0b5c5cfd28af3f Mon Sep 17 00:00:00 2001 From: TheRulerOfNerds <63425932+TheRulerOfNerds@users.noreply.github.com> Date: Sun, 18 Jul 2021 16:01:13 -0400 Subject: [PATCH] Add files via upload --- .../Backups/DynamicBalanceBackup1.py | 233 ++++++++++++++++ .../Backups/DynamicBalanceBackup2.py | 237 +++++++++++++++++ .../DynamicBalance/DynamicBalance.py | 249 ++++++++++++++++++ simulation/atlas/protos/Atlas.proto | 10 + simulation/atlas/worlds/atlas.wbt | 16 +- 5 files changed, 743 insertions(+), 2 deletions(-) create mode 100644 simulation/atlas/controllers/DynamicBalance/Backups/DynamicBalanceBackup1.py create mode 100644 simulation/atlas/controllers/DynamicBalance/Backups/DynamicBalanceBackup2.py create mode 100644 simulation/atlas/controllers/DynamicBalance/DynamicBalance.py diff --git a/simulation/atlas/controllers/DynamicBalance/Backups/DynamicBalanceBackup1.py b/simulation/atlas/controllers/DynamicBalance/Backups/DynamicBalanceBackup1.py new file mode 100644 index 0000000..6477c0f --- /dev/null +++ b/simulation/atlas/controllers/DynamicBalance/Backups/DynamicBalanceBackup1.py @@ -0,0 +1,233 @@ +import time +from shapely.geometry.polygon import Polygon +from shapely.geometry.point import Point +from controller import Robot, Supervisor, Display + +#robot - wb_supervisor_node +#supervisor - wb_robot +#so basically supervisor is the robot and robot is the supervisor +#hooray for poor nomenclature +supervisor = Supervisor() + +global TIMESTEP +TIMESTEP = int(supervisor.getBasicTimeStep()) +startTime = time.time() + +robotNode = supervisor.getRoot() +children = robotNode.getField("children") +robot = children.getMFNode(5) + + +right_arm_uzi = supervisor.getDevice("RArmUsy") +left_arm_uzi = supervisor.getDevice("LArmUsy") + +torso_pitch = supervisor.getDevice("BackMby") +torso_roll = supervisor.getDevice("BackUbx") + +right_torso_pitch = supervisor.getDevice("RLegLhy") +left_torso_pitch = supervisor.getDevice("LLegLhy") + + +right_ankle_pitch = supervisor.getDevice("RLegUay") +left_ankle_pitch = supervisor.getDevice("LLegUay") + +rightFootSlot = robot.getField("rightFootSlot") +rightFootSlotPosition = rightFootSlot.getMFNode(0).getPosition() + + +leftFootSlot = robot.getField("leftFootSlot") +leftFootSlotPosition = leftFootSlot.getMFNode(0).getPosition() + +# print(rightFootSlotPosition,leftFootSlotPosition) + + + + +# def isCOMInSupportPlane(wantDistance): +# coordinate1 = ((rightFootSlotPosition[0] + 0.17), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge +# coordinate2 = ((rightFootSlotPosition[0] - 0.08), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + +# coordinate3 = ((leftFootSlotPosition[0] - 0.08), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge +# coordinate4 = ((leftFootSlotPosition[0] + 0.17), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + +# coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + +# supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + +# COMPoint = Point(coordinate5) + + +# if(wantDistance== False): +# return supportRectangle.contains(COMPoint) +# elif(wantDistance == True): +# return supportRectangle.distance(COMPoint) + + +def distanceToCOMinSupportPlaneCenter(): + coordinate1 = ((rightFootSlotPosition[0] + 0.17), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge + coordinate2 = ((rightFootSlotPosition[0] - 0.08), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + + coordinate3 = ((leftFootSlotPosition[0] - 0.08), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge + coordinate4 = ((leftFootSlotPosition[0] + 0.17), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + + coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + + supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + + COMPoint = Point(coordinate5) + + supportRectangleCenter = supportRectangle.centroid + + return [(COMPoint.x - supportRectangleCenter.x), (COMPoint.y - supportRectangleCenter.y)] + + + + +# def isCOMInSupportPlaneTight(wantDistance): +# coordinate1 = ((rightFootSlotPosition[0] + 0.025), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge +# coordinate2 = ((rightFootSlotPosition[0] - 0.05), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + +# coordinate3 = ((leftFootSlotPosition[0] - 0.05), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge +# coordinate4 = ((leftFootSlotPosition[0] + 0.025), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + +# coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + +# supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + +# COMPoint = Point(coordinate5) + + +# if(wantDistance== False): +# return supportRectangle.contains(COMPoint) +# elif(wantDistance == True): +# return supportRectangle.distance(COMPoint) + +# def isComInSupportPlaneCenter(wantDistance): +# coordinate1 = ((rightFootSlotPosition[0] + 0.075), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge +# coordinate2 = ((rightFootSlotPosition[0] - 0.025), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + +# coordinate3 = ((leftFootSlotPosition[0] - 0.025), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge +# coordinate4 = ((leftFootSlotPosition[0] + 0.075), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + +# coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + +# supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + +# COMPoint = Point(coordinate5) + +# print("COM is at " + str(COMPoint)) +# print("COM should be at" + str(supportRectangle.centroid)) + +# if (wantDistance == False): +# if((COMPoint.x > coordinate2[0]) and (COMPoint.x < (coordinate2[0] + 0.03))): +# return True +# elif(supportRectangle.centroid != COMPoint): +# return False +# elif (wantDistance == True): +# return ((coordinate2[0] + 0.015) - COMPoint.x) + + + + + + + +shoulderForce = 1.5 + + +# right_shoulder_pitch.setPosition(shoulderForce) +# left_shoulder_pitch.setPosition(shoulderForce) + + + + +firstTimeMovingTest = True + +fallingForward = True + +# isComInSupportPlaneCenter(False) + + +while supervisor.step(TIMESTEP) != -1: + + + if(time.time() - startTime >= 3 and firstTimeMovingTest == True ): + # right_torso_pitch.setPosition(testForce) + # left_torso_pitch.setPosition(testForce) + # forcex = -4550 + forcex = -4500 + robot.addForce([forcex,0,0], False) + firstTimeMovingTest = False + + # if(time.time() - startTime >= 6 ): + # # isComInSupportPlaneCenter() + # # print(robot.getCenterOfMass()) + # print(isComInSupportPlaneCenter(True)) + + wantActuation = True + + if( firstTimeMovingTest == False and wantActuation == True): + + if(robot.getCenterOfMass()[0] > 0): + fallingForward = True #com is forward so apply backwards motor values (positive motor values) + elif (robot.getCenterOfMass()[0] < 0): + fallingForward = False #com is backward so apply forwards motor values (negative motor values) + + # print(fallingForward) + + + + # print(error) + fallModifier = 1 + + if(fallingForward == True): + fallModifier = 1 + elif(fallingForward == False): + fallModifier = -1 + + + # print(distanceToCOMinSupportPlaneCenter()) + + #PID CONTROL (ADD DERIVITIVE) + #CO = CObias + KC * e(t) + ankleerrorx = distanceToCOMinSupportPlaneCenter()[0] #find error/e(t) + anklekc = -0.5 * fallModifier + ankleCObias = 0 + ankleCOx = ankleCObias + (anklekc * ankleerrorx) + + torsoerrorx = distanceToCOMinSupportPlaneCenter()[0] #find error/e(t) + torsokc = -1 * fallModifier + torsoCObias = 0 + torsoCOx = torsoCObias + (torsokc * torsoerrorx) + + torsoerrory = distanceToCOMinSupportPlaneCenter()[1] #find error/e(t) + torsokc = -1 * fallModifier + torsoCObias = 0 + torsoCOy = torsoCObias + (torsokc * torsoerrorx) + + shouldererrorx = distanceToCOMinSupportPlaneCenter()[0] #find error/e(t) + shoulderkc = -0.5 * fallModifier + shoulderCObias = 0 + shoulderCOx = shoulderCObias + (shoulderkc * shouldererrorx) + + right_ankle_pitch.setPosition(ankleCOx) + left_ankle_pitch.setPosition(ankleCOx) + + + torso_pitch.setPosition(torsoCOx) + torso_roll.setPosition(torsoCOy) + + right_torso_pitch.setPosition(torsoCOx*-1) + left_torso_pitch.setPosition(torsoCOx*-1) + + + right_arm_uzi.setPosition(shoulderCOx) + left_arm_uzi.setPosition(shoulderCOx) + + + # print("CO " + str(shoulderCOx)) + + + + +#TODO: make it fix y errors as well as x errors. \ No newline at end of file diff --git a/simulation/atlas/controllers/DynamicBalance/Backups/DynamicBalanceBackup2.py b/simulation/atlas/controllers/DynamicBalance/Backups/DynamicBalanceBackup2.py new file mode 100644 index 0000000..bf0c6cf --- /dev/null +++ b/simulation/atlas/controllers/DynamicBalance/Backups/DynamicBalanceBackup2.py @@ -0,0 +1,237 @@ +import time +from shapely.geometry.polygon import Polygon +from shapely.geometry.point import Point +from controller import Robot, Supervisor, Display + +#robot - wb_supervisor_node +#supervisor - wb_robot +#so basically supervisor is the robot and robot is the supervisor +#hooray for poor nomenclature +supervisor = Supervisor() + +global TIMESTEP +TIMESTEP = int(supervisor.getBasicTimeStep()) +startTime = time.time() + +robotNode = supervisor.getRoot() +children = robotNode.getField("children") +robot = children.getMFNode(5) + + +right_arm_uzi = supervisor.getDevice("RArmUsy") +left_arm_uzi = supervisor.getDevice("LArmUsy") + +torso_pitch = supervisor.getDevice("BackMby") +torso_roll = supervisor.getDevice("BackUbx") + +right_torso_pitch = supervisor.getDevice("RLegLhy") +left_torso_pitch = supervisor.getDevice("LLegLhy") + + +right_ankle_pitch = supervisor.getDevice("RLegUay") +left_ankle_pitch = supervisor.getDevice("LLegUay") + + +# print(rightFootSlotPosition,leftFootSlotPosition) + + + + +# def isCOMInSupportPlane(wantDistance): +# coordinate1 = ((rightFootSlotPosition[0] + 0.17), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge +# coordinate2 = ((rightFootSlotPosition[0] - 0.08), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + +# coordinate3 = ((leftFootSlotPosition[0] - 0.08), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge +# coordinate4 = ((leftFootSlotPosition[0] + 0.17), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + +# coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + +# supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + +# COMPoint = Point(coordinate5) + + +# if(wantDistance== False): +# return supportRectangle.contains(COMPoint) +# elif(wantDistance == True): +# return supportRectangle.distance(COMPoint) + + +def distanceToCOMinSupportPlaneCenter(): + + rightFootSlot = robot.getField("rightFootSlot") + rightFootSlotPosition = rightFootSlot.getMFNode(0).getPosition() + + + leftFootSlot = robot.getField("leftFootSlot") + leftFootSlotPosition = leftFootSlot.getMFNode(0).getPosition() + + + + coordinate1 = ((rightFootSlotPosition[0] + 0.17), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge + coordinate2 = ((rightFootSlotPosition[0] - 0.08), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + + coordinate3 = ((leftFootSlotPosition[0] - 0.08), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge + coordinate4 = ((leftFootSlotPosition[0] + 0.17), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + + coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + + supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + + COMPoint = Point(coordinate5) + + supportRectangleCenter = supportRectangle.centroid + + return [(COMPoint.x - supportRectangleCenter.x), (COMPoint.y - supportRectangleCenter.y)] + + + + +# def isCOMInSupportPlaneTight(wantDistance): +# coordinate1 = ((rightFootSlotPosition[0] + 0.025), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge +# coordinate2 = ((rightFootSlotPosition[0] - 0.05), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + +# coordinate3 = ((leftFootSlotPosition[0] - 0.05), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge +# coordinate4 = ((leftFootSlotPosition[0] + 0.025), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + +# coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + +# supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + +# COMPoint = Point(coordinate5) + + +# if(wantDistance== False): +# return supportRectangle.contains(COMPoint) +# elif(wantDistance == True): +# return supportRectangle.distance(COMPoint) + +# def isComInSupportPlaneCenter(wantDistance): +# coordinate1 = ((rightFootSlotPosition[0] + 0.075), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge +# coordinate2 = ((rightFootSlotPosition[0] - 0.025), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + +# coordinate3 = ((leftFootSlotPosition[0] - 0.025), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge +# coordinate4 = ((leftFootSlotPosition[0] + 0.075), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + +# coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + +# supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + +# COMPoint = Point(coordinate5) + +# print("COM is at " + str(COMPoint)) +# print("COM should be at" + str(supportRectangle.centroid)) + +# if (wantDistance == False): +# if((COMPoint.x > coordinate2[0]) and (COMPoint.x < (coordinate2[0] + 0.03))): +# return True +# elif(supportRectangle.centroid != COMPoint): +# return False +# elif (wantDistance == True): +# return ((coordinate2[0] + 0.015) - COMPoint.x) + + + + + + + +shoulderForce = 1.5 + + +# right_shoulder_pitch.setPosition(shoulderForce) +# left_shoulder_pitch.setPosition(shoulderForce) + + + + +firstTimeMovingTest = True + +fallingForward = True + +# isComInSupportPlaneCenter(False) + + +while supervisor.step(TIMESTEP) != -1: + + + if(time.time() - startTime >= 3 and firstTimeMovingTest == True ): + # right_torso_pitch.setPosition(testForce) + # left_torso_pitch.setPosition(testForce) + # forcex = -4550,6000 + forcex = -4500 + robot.addForce([forcex,0,0], False) + firstTimeMovingTest = False + + # if(time.time() - startTime >= 6 ): + # # isComInSupportPlaneCenter() + # # print(robot.getCenterOfMass()) + # print(isComInSupportPlaneCenter(True)) + + wantActuation = True + + if( firstTimeMovingTest == False and wantActuation == True): + + if(robot.getCenterOfMass()[0] > 0): + fallingForward = True #com is forward so apply backwards motor values (positive motor values) + elif (robot.getCenterOfMass()[0] < 0): + fallingForward = False #com is backward so apply forwards motor values (negative motor values) + + # print(fallingForward) + + + + # print(error) + fallModifier = 1 + + if(fallingForward == True): + fallModifier = 1 + elif(fallingForward == False): + fallModifier = -1 + + + # print(distanceToCOMinSupportPlaneCenter()) + + #PID CONTROL (ADD DERIVITIVE) + #CO = CObias + KC * e(t) + ankleerrorx = distanceToCOMinSupportPlaneCenter()[0] #find error/e(t) + anklekc = -0.5 * fallModifier + ankleCObias = 0 + ankleCOx = ankleCObias + (anklekc * ankleerrorx) + + torsoerrorx = distanceToCOMinSupportPlaneCenter()[0] #find error/e(t) + torsokc = -1 * fallModifier + torsoCObias = 0 + torsoCOx = torsoCObias + (torsokc * torsoerrorx) + + torsoerrory = distanceToCOMinSupportPlaneCenter()[1] #find error/e(t) + torsokc = -1 * fallModifier + torsoCObias = 0 + torsoCOy = torsoCObias + (torsokc * torsoerrorx) + + shouldererrorx = distanceToCOMinSupportPlaneCenter()[0] #find error/e(t) + shoulderkc = -0.5 * fallModifier + shoulderCObias = 0 + shoulderCOx = shoulderCObias + (shoulderkc * shouldererrorx) + + right_ankle_pitch.setPosition(ankleCOx) + left_ankle_pitch.setPosition(ankleCOx) + + + torso_pitch.setPosition(torsoCOx) + torso_roll.setPosition(torsoCOy) + + right_torso_pitch.setPosition(torsoCOx*-1) + left_torso_pitch.setPosition(torsoCOx*-1) + + + right_arm_uzi.setPosition(shoulderCOx) + left_arm_uzi.setPosition(shoulderCOx) + + + # print("CO " + str(shoulderCOx)) + + + + +#TODO: make it fix y errors as well as x errors. \ No newline at end of file diff --git a/simulation/atlas/controllers/DynamicBalance/DynamicBalance.py b/simulation/atlas/controllers/DynamicBalance/DynamicBalance.py new file mode 100644 index 0000000..bc59ef6 --- /dev/null +++ b/simulation/atlas/controllers/DynamicBalance/DynamicBalance.py @@ -0,0 +1,249 @@ +import time +from shapely.geometry.polygon import Polygon +from shapely.geometry.point import Point +from controller import Robot, Supervisor, Display + +#robot - wb_supervisor_node +#supervisor - wb_robot +#so basically supervisor is the robot and robot is the supervisor +#hooray for poor nomenclature +supervisor = Supervisor() + +global TIMESTEP +TIMESTEP = int(supervisor.getBasicTimeStep()) +startTime = time.time() + +robotNode = supervisor.getRoot() +children = robotNode.getField("children") +robot = children.getMFNode(5) + + +right_arm_uzi = supervisor.getDevice("RArmUsy") +left_arm_uzi = supervisor.getDevice("LArmUsy") + +torso_pitch = supervisor.getDevice("BackMby") +torso_roll = supervisor.getDevice("BackUbx") + +right_torso_pitch = supervisor.getDevice("RLegLhy") +left_torso_pitch = supervisor.getDevice("LLegLhy") + + +right_ankle_pitch = supervisor.getDevice("RLegUay") +left_ankle_pitch = supervisor.getDevice("LLegUay") + + +# print(rightFootSlotPosition,leftFootSlotPosition) + + + + +# def isCOMInSupportPlane(wantDistance): +# coordinate1 = ((rightFootSlotPosition[0] + 0.17), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge +# coordinate2 = ((rightFootSlotPosition[0] - 0.08), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + +# coordinate3 = ((leftFootSlotPosition[0] - 0.08), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge +# coordinate4 = ((leftFootSlotPosition[0] + 0.17), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + +# coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + +# supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + +# COMPoint = Point(coordinate5) + + +# if(wantDistance== False): +# return supportRectangle.contains(COMPoint) +# elif(wantDistance == True): +# return supportRectangle.distance(COMPoint) + + +def distanceToCOMinSupportPlaneCenter(): + + rightFootSlot = robot.getField("rightFootSlot") + rightFootSlotPosition = rightFootSlot.getMFNode(0).getPosition() + + + leftFootSlot = robot.getField("leftFootSlot") + leftFootSlotPosition = leftFootSlot.getMFNode(0).getPosition() + + + + coordinate1 = ((rightFootSlotPosition[0] + 0.17), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge + coordinate2 = ((rightFootSlotPosition[0] - 0.08), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + + coordinate3 = ((leftFootSlotPosition[0] - 0.08), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge + coordinate4 = ((leftFootSlotPosition[0] + 0.17), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + + coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + + supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + + COMPoint = Point(coordinate5) + + supportRectangleCenter = supportRectangle.centroid + + return [(COMPoint.x - supportRectangleCenter.x), (COMPoint.y - supportRectangleCenter.y)] + + + + +# def isCOMInSupportPlaneTight(wantDistance): +# coordinate1 = ((rightFootSlotPosition[0] + 0.025), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge +# coordinate2 = ((rightFootSlotPosition[0] - 0.05), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + +# coordinate3 = ((leftFootSlotPosition[0] - 0.05), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge +# coordinate4 = ((leftFootSlotPosition[0] + 0.025), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + +# coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + +# supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + +# COMPoint = Point(coordinate5) + + +# if(wantDistance== False): +# return supportRectangle.contains(COMPoint) +# elif(wantDistance == True): +# return supportRectangle.distance(COMPoint) + +# def isComInSupportPlaneCenter(wantDistance): +# coordinate1 = ((rightFootSlotPosition[0] + 0.075), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Front Edge +# coordinate2 = ((rightFootSlotPosition[0] - 0.025), (rightFootSlotPosition[2] + 0.05)) #Right Foot, Back Edge + +# coordinate3 = ((leftFootSlotPosition[0] - 0.025), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Back Edge +# coordinate4 = ((leftFootSlotPosition[0] + 0.075), (leftFootSlotPosition[2] - 0.05)) #Left Foot, Front Edge + +# coordinate5 = ((robot.getCenterOfMass()[0]), (robot.getCenterOfMass()[2])) + +# supportRectangle = Polygon([coordinate1,coordinate2,coordinate3,coordinate4]) + +# COMPoint = Point(coordinate5) + +# print("COM is at " + str(COMPoint)) +# print("COM should be at" + str(supportRectangle.centroid)) + +# if (wantDistance == False): +# if((COMPoint.x > coordinate2[0]) and (COMPoint.x < (coordinate2[0] + 0.03))): +# return True +# elif(supportRectangle.centroid != COMPoint): +# return False +# elif (wantDistance == True): +# return ((coordinate2[0] + 0.015) - COMPoint.x) + + + + + + + +shoulderForce = 1.5 + + +# right_shoulder_pitch.setPosition(shoulderForce) +# left_shoulder_pitch.setPosition(shoulderForce) + + + + +firstTimeMovingTest = True + +fallingForward = True + +# isComInSupportPlaneCenter(False) +# supervisor.getDevice("LArmShx").setPosition(-1) +# supervisor.getDevice("RArmShx").setPosition(1) + +# supervisor.getDevice("LLegLhy").setPosition(-0.5) +# torso_roll.setPosition(0.1) + +while supervisor.step(TIMESTEP) != -1: + + # if(time.time() - startTime >=2 and firstTimeMovingTest == True): + + # # supervisor.getDevice("LArmShx").setPosition(-1) + # # supervisor.getDevice("RArmShx").setPosition(1) + + # # supervisor.getDevice("LLegLhy").setPosition(-0.5) + # # torso_roll.setPosition(0.2) + + + if(time.time() - startTime >= 3 and firstTimeMovingTest == True ): + # right_torso_pitch.setPosition(testForce) + # left_torso_pitch.setPosition(testForce) + # forcex = -4550,6000 + forcex = -4500 + robot.addForce([forcex,0,0], False) + firstTimeMovingTest = False + + # if(time.time() - startTime >= 6 ): + # # isComInSupportPlaneCenter() + # # print(robot.getCenterOfMass()) + # print(isComInSupportPlaneCenter(True)) + + wantActuation = True + + if( firstTimeMovingTest == False and wantActuation == True): + + if(robot.getCenterOfMass()[0] > 0): + fallingForward = True #com is forward so apply backwards motor values (positive motor values) + elif (robot.getCenterOfMass()[0] < 0): + fallingForward = False #com is backward so apply forwards motor values (negative motor values) + + # print(fallingForward) + + + + # print(error) + fallModifier = 1 + + if(fallingForward == True): + fallModifier = 1 + elif(fallingForward == False): + fallModifier = -1 + + + # print(distanceToCOMinSupportPlaneCenter()) + + #PID CONTROL (ADD DERIVITIVE) + #CO = CObias + KC * e(t) + ankleerrorx = distanceToCOMinSupportPlaneCenter()[0] #find error/e(t) + anklekc = -0.5 * fallModifier + ankleCObias = 0 + ankleCOx = ankleCObias + (anklekc * ankleerrorx) + + torsoerrorx = distanceToCOMinSupportPlaneCenter()[0] #find error/e(t) + torsokc = -1 * fallModifier + torsoCObias = 0 + torsoCOx = torsoCObias + (torsokc * torsoerrorx) + + torsoerrory = distanceToCOMinSupportPlaneCenter()[1] #find error/e(t) + torsokc = -1 * fallModifier + torsoCObias = 0 + torsoCOy = torsoCObias + (torsokc * torsoerrorx) + + shouldererrorx = distanceToCOMinSupportPlaneCenter()[0] #find error/e(t) + shoulderkc = -0.5 * fallModifier + shoulderCObias = 0 + shoulderCOx = shoulderCObias + (shoulderkc * shouldererrorx) + + right_ankle_pitch.setPosition(ankleCOx) + left_ankle_pitch.setPosition(ankleCOx) + + + torso_pitch.setPosition(torsoCOx) + torso_roll.setPosition(torsoCOy) + + right_torso_pitch.setPosition(torsoCOx*-1) + left_torso_pitch.setPosition(torsoCOx*-1) + + + right_arm_uzi.setPosition(shoulderCOx) + left_arm_uzi.setPosition(shoulderCOx) + + + # print("CO " + str(shoulderCOx)) + + + + +#TODO: make it fix y errors as well as x errors. \ No newline at end of file diff --git a/simulation/atlas/protos/Atlas.proto b/simulation/atlas/protos/Atlas.proto index b2c1094..ed4f1b9 100644 --- a/simulation/atlas/protos/Atlas.proto +++ b/simulation/atlas/protos/Atlas.proto @@ -15,6 +15,8 @@ PROTO Atlas [ field SFBool supervisor FALSE # Is `Robot.supervisor`. field SFBool synchronization TRUE # Is `Robot.synchronization`. field MFNode pelvisSlot [] # Extends the robot with new nodes in the pelvis slot. + field MFNode rightFootSlot [] # Extends the robot with new nodes in the right foot slot. + field MFNode leftFootSlot [] # Extends the robot with new nodes in the left foot slot. ] { Robot { @@ -517,6 +519,10 @@ PROTO Atlas [ children [ LFootSolid { } + DEF LEFT_FOOT_SLOT Transform { + translation 0 0 -0.075 + children IS leftFootSlot + } ] physics USE DEFAULT_PHYSICS } @@ -656,6 +662,10 @@ PROTO Atlas [ children [ RFootSolid { } + DEF RIGHT_FOOT_SLOT Transform { + translation 0 0 -0.075 + children IS rightFootSlot + } ] physics USE DEFAULT_PHYSICS } diff --git a/simulation/atlas/worlds/atlas.wbt b/simulation/atlas/worlds/atlas.wbt index 5f94414..3e12cf8 100644 --- a/simulation/atlas/worlds/atlas.wbt +++ b/simulation/atlas/worlds/atlas.wbt @@ -10,15 +10,27 @@ WorldInfo { lineScale 0.25 } Viewpoint { - orientation 0.07361487165598588 -0.9880371064661203 -0.13551209140563525 4.208435019952087 - position 4.007193926491309 2.1969755916188816 -2.1227761648514134 + orientation -0.01946869317373936 0.9993605860641073 0.029989815066301183 1.9905841599583447 + position 5.8791583117842645 1.2945375732822115 -2.7941691397149198 } TexturedBackground { + texture "dusk" } TexturedBackgroundLight { + texture "dusk" } Floor { size 1000 1000 } Atlas { + controller "DynamicBalance" + supervisor TRUE + rightFootSlot [ + Solid { + } + ] + leftFootSlot [ + Solid { + } + ] }