mirror of
https://github.com/PotentiaRobotics/engine-software.git
synced 2025-04-09 23:00:21 -04:00
Add files via upload
This commit is contained in:
TheRulerOfNerds
GitHub
parent
b11694bf1f
commit
12c5cc15ee
|
|
@ -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.
|
||||
|
|
@ -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.
|
||||
249
simulation/atlas/controllers/DynamicBalance/DynamicBalance.py
Normal file
249
simulation/atlas/controllers/DynamicBalance/DynamicBalance.py
Normal file
|
|
@ -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.
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user