From d8d911ab5afb643f54f11509950f17c579dbceee Mon Sep 17 00:00:00 2001 From: ramnreddy15 <> Date: Sat, 28 May 2022 18:21:41 +0000 Subject: [PATCH] Add gait gen --- Control System/bootup.py | 2 +- Control System/gaitGen/footProjection.py | 52 ++ Control System/gaitGen/olympian.urdf | 927 +++++++++++++++++++++++ Control System/gaitGen/step.py | 299 ++++++++ Control System/gaitGen/timeX.txt | 301 ++++++++ Control System/gaitGen/timeZ.txt | 301 ++++++++ Control System/gaitGen/urdf.py | 8 + Control System/gaitGen/xValues.txt | 301 ++++++++ Control System/gaitGen/zValues.txt | 301 ++++++++ 9 files changed, 2491 insertions(+), 1 deletion(-) create mode 100644 Control System/gaitGen/footProjection.py create mode 100644 Control System/gaitGen/olympian.urdf create mode 100644 Control System/gaitGen/step.py create mode 100644 Control System/gaitGen/timeX.txt create mode 100644 Control System/gaitGen/timeZ.txt create mode 100644 Control System/gaitGen/urdf.py create mode 100644 Control System/gaitGen/xValues.txt create mode 100644 Control System/gaitGen/zValues.txt diff --git a/Control System/bootup.py b/Control System/bootup.py index 3bbfb51..7095d70 100644 --- a/Control System/bootup.py +++ b/Control System/bootup.py @@ -127,7 +127,7 @@ class Receiver: # threading.Thread(target=self.sensorData).start() # threading.Thread(target=self.balance).start() - # threading.Thread(target=self.gaitGen).start() + threading.Thread(target=self.gaitGen).start() # threading.Thread(target=self.comuterVision).start() def startBoot(): diff --git a/Control System/gaitGen/footProjection.py b/Control System/gaitGen/footProjection.py new file mode 100644 index 0000000..b20575a --- /dev/null +++ b/Control System/gaitGen/footProjection.py @@ -0,0 +1,52 @@ +import math as m +def createXProjection(length, duration): + t = 0; + f = open("xValues.txt", "w") + f2 = open("timeX.txt", "w") + xLambda = length/duration + w = m.pi * 2 + w = w/duration + projectedPoints = [] + deltaT = 0.01 #this is an arbitrary delta t, which can be changed later on + while(t < duration+deltaT): + value = xLambda*t - (xLambda/w)*m.sin(w*t) + projectedPoints.append(value) + f.write(str(value) + "\n") + f2.write(str(t) + "\n") + t += deltaT + t = round(t, 2) + f.close() + f2.close() + return projectedPoints +def createZProjection(height, duration): + t = 0; + f = open("zValues.txt", "w") + f2 = open("timeZ.txt", "w") + duration = duration/2 + zLambda = height/(duration) + w = m.pi * 2 + w = w/duration + projectedPoints = [] + deltaT = 0.01 #this is an arbitrary delta t, which can be changed later on + while(t < duration+deltaT): + value = zLambda*t - (zLambda/w)*m.sin(w*t) + projectedPoints.append(value) + f.write(str(value) + "\n") + f2.write(str(t) + "\n") + t += deltaT + t = round(t, 2) + t = duration+deltaT + while(t < (2 * duration)+deltaT): + value = -(zLambda*t - (zLambda/w)*m.sin(w*t)) + (height*2) + projectedPoints.append(value) + f.write(str(value) +"\n") + f2.write(str(t)+"\n") + t += deltaT + t = round(t, 2) + f.close() + f2.close() + return projectedPoints + +#change the values based on the length/height of the step and how long the duration is +createXProjection(0.5,3) +createZProjection(0.4,3) \ No newline at end of file diff --git a/Control System/gaitGen/olympian.urdf b/Control System/gaitGen/olympian.urdf new file mode 100644 index 0000000..ca8c72e --- /dev/null +++ b/Control System/gaitGen/olympian.urdf @@ -0,0 +1,927 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + diff --git a/Control System/gaitGen/step.py b/Control System/gaitGen/step.py new file mode 100644 index 0000000..8e3bb37 --- /dev/null +++ b/Control System/gaitGen/step.py @@ -0,0 +1,299 @@ +import pybullet as p +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 + +print("hi") + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally +p.setGravity(0,0,-9.8) +planeId = p.loadURDF("plane.urdf") #where is plane.urdf +startPos = [0,0,0] +#sphereId = p.loadURDF("sphere.urdf") +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) + +print("==========SIMULATION ENABLED================") + +#GET JOINT INFO + +print(p.getNumJoints(robotId)) +for i in range(0, p.getNumJoints(robotId)-1): + print(p.getJointInfo(robotId, i)[0:13]) + + +listOfJointIndeces = [] +for i in range(0, p.getNumJoints(robotId)): + listOfJointIndeces.append(i) +#why not use list(range())? +#also why have 0 + +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) + + #print("mass: " + str(masssum)) + #print("center of mass: " + str(com)) + #print("\n") + return com + +#INVERSE KINEMATICS +#GET JOINT ANGLES NEEDED TO MOVE LINK 10 to (0. 0.6, 2) +#FORCE IS 25NM for every link bc im too lazy to custom set it for every link +#IT FALLS BECAUSE its a lot of torque exerted in a small amount of time + +torque = 100 +newi = [] +comPos = [] +comPos2 = [] + + + +holdingTorque = 100 +actuationTorque = 10 +torqueList = [holdingTorque]*23 + +file = open("xValues.txt", "r") +xValues = [] +for word in file.readlines(): + xValues.append(float(word.rstrip('\n'))) +file = open("zValues.txt", "r") +zValues = [] +for word in file.readlines(): + zValues.append(float(word.rstrip('\n'))) + +def shiftFoot(): + + leftAnkle = p.getJointState(robotId, 21)[0] + leftThigh = p.getJointState(robotId, 18)[0] + print(leftAnkle) + print(leftThigh) + + for i in range(0, 50): + positionsList = [0]*23 + angleNeeded = calculateAngleNeeded() + comPos.append(calcCOM()) + newi.insert(0, i) + positionsList[21] = angleNeeded/50 * i #left ankle + positionsList[15] = angleNeeded/50 * i #right ankle + positionsList[18] = angleNeeded/50 * i#left thigh + positionsList[12] = angleNeeded/50 * i#right thigh + + #positionsList[14] = 0.3/50 * i + forceArray = [torque]*23 + p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray) + p.stepSimulation() + + '' + for i in range(0, 50): + comPos.append(calcCOM()) + newi.insert(0, i) + angleNeeded = 0.5 + + positionsList[11] = 0.2598371070140097/50 * i #thigh pitch + positionsList[14] = 0.8123194567657278/50 * i #knee + positionsList[16] = 0.4891906071331436/50 * i#ankle + + #positionsList[14] = 0.3/50 * i + forceArray = [torque]*23 + p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray) + p.stepSimulation() + + + positionsList = [0]*23 + + firstY = p.getLinkState(robotId, 15)[0][1] + firstZ = p.getLinkState(robotId, 15)[0][2] + + + for i in range(0, len(xValues)//20): + """ + comPos.append(calcCOM()) + newi.insert(0, i) + angleNeeded = calculateAngleNeeded() + positionsList[21] = angleNeeded/len(xValues) * i #left ankle + #positionsList[15] = angleNeeded/len(xValues) * i #right ankle + positionsList[18] = angleNeeded/len(xValues) * i#left thigh + #positionsList[12] = angleNeeded/len(xValues) * i#right thigh + forceArray = [torque]*23 + p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray) + p.stepSimulation() + """ + #thighs are 12 and 18 + angleNeeded = calculateAngleNeeded() + ikList = p.calculateInverseKinematics(robotId, 15, [xValues[20*i], firstY, zValues[20*i]+(len(xValues)-20*i)/len(xValues)*firstZ]) + + positionsList = list(ikList) + #print("uwu") + #print(positionsList[11]) + #print(positionsList[14]) + #print(positionsList[16]) + #positionsList[11] = 0.5*(len(xValues)-i)/len(xValues)+positionsList[11]*(i/len(xValues)) + #positionsList[14] = 1*(len(xValues)-i)/len(xValues)+positionsList[14]*(i/len(xValues)) + #positionsList[16] = 0.5*(len(xValues)-i)/len(xValues)+positionsList[16]*(i/len(xValues)) + positionsList[21] = angleNeeded*(len(xValues)-8*i)/len(xValues) + positionsList[18] = angleNeeded*(len(xValues)-8*i)/len(xValues) + ankleAngle = calculateAnkleAngle() + + positionsList[15] = (math.pi/2-ankleAngle*np.sign(ankleAngle))*np.sign(ankleAngle)/len(xValues)*i*20 + ankleAngle = calculateOtherAnkleAngle() + positionsList[16] = 0.4891906071331436*(len(xValues)-(i*20))/len(xValues)+ankleAngle/len(xValues)*i*20 + pelvisAngle = calculatePelvisAngle() + print(pelvisAngle) + positionsList[0] = -1*pelvisAngle + #positionsList[15] = (math.pi-ankleAngle*np.sign(ankleAngle))*np.sign(ankleAngle)/len(xValues)*i + ''' + if((ankleAngle)<0): + positionsList[15] = -1*(math.pi-(abs(ankleAngle)))/len(xValues)*i + else: + positionsList[15] = math.pi-(abs(ankleAngle))/len(xValues)*i + ''' + #positionsList[16] = + #positionsList[15] = (math.pi-calculateAnkleAngle())/len(xValues)*i + # positionsList[0] = math.pi / 4 + forceArray = [torque]*23 + p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray) + p.stepSimulation() + + for i in range(50000): + p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, targetPositions = positionsList, forces = forceArray) + p.stepSimulation() + +def calculateAngleNeeded(): + anklePos = p.getLinkState(robotId, 21)[0] + comPos = calcCOM() + hipPos = p.getLinkState(robotId, 18)[0] + ankleCom = (comPos[1]-anklePos[1], comPos[2]-anklePos[2]) + ankleHip = (hipPos[1]-anklePos[1], hipPos[2]-anklePos[2]) + cosAngle = (ankleCom[0]*ankleHip[0] + ankleCom[1]*ankleHip[1])/(np.sqrt((ankleCom[0]**2+ankleCom[1]**2))*np.sqrt((ankleHip[0]**2 + ankleHip[1]**2))) + angle = np.arccos(cosAngle) + #print(angle) + return angle + +def calculateAnkleAngle(): + anklePos = p.getLinkState(robotId, 15)[0] + kneePos = p.getLinkState(robotId, 14)[0] + angle = np.arctan((kneePos[2]-anklePos[2])/(kneePos[1]-anklePos[1])) + print("thing " + str(angle)) + return angle + +def calculateOtherAnkleAngle(): + anklePos = p.getLinkState(robotId, 15)[0] + kneePos = p.getLinkState(robotId, 14)[0] + angle = np.arctan((kneePos[0]-anklePos[0])/(kneePos[2]-anklePos[2])) + print("thing " + str(angle)) + return angle + +def calculatePelvisAngle(): + pelvisPos = p.getLinkState(robotId, 0)[0] + chestPos = p.getLinkState(robotId, 1)[0] + print(chestPos) + print(pelvisPos) + angle = np.arctan((chestPos[0]-pelvisPos[0])/(chestPos[2]-pelvisPos[2])) + return angle + +shiftFoot() + +print(p.getLinkState(robotId, 21)[0][1]-calcCOM()[1]) + +print(str(p.getLinkState(robotId, 21)) + "HIIII") + +for i in range(1000000): + p.stepSimulation() + +""" +for i in range(0, len(xValues), 1): + ikList = p.calculateInverseKinematics(robotId, 15, [xValues[i], 0, zValues[i]]) + p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, + targetPositions = ikList, forces = torqueList) + p.stepSimulation() + time.sleep(1./240.) + +for i in range(10000): + p.stepSimulation() + time.sleep(1./240.) +""" + +z = list(range(len(xValues))) +area = 3 # 0 to 15 point radii + +plt.scatter(z, zValues, s=area, alpha=0.5) +#plt.show() + +#ikList = p.calculateInverseKinematics(robotId, 10, [0,0.6,2] ) + +""" +p.setJointMotorControlArray(robotId, listOfJointIndeces, p.POSITION_CONTROL, + targetPositions = ikList, forces = torqueList) + + +#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) #what is wheight + print(xpos) + print(ypos) + print(zpos) + print("\n") + +com = (masstimesxpossum/masssum, masstimesypossum/masssum, masstimeszpossum/masssum) +print("==========COM APROX EQUALS===========") +print(com) +print("\n") +#STEP SIMULATION +for i in range(0,10000): + p.stepSimulation() + time.sleep(1./240.) + + +print("========REALTIME SIMULATION DISABLED===============") +# p.calculateInverseKinematics(robotId, ) +""" \ No newline at end of file diff --git a/Control System/gaitGen/timeX.txt b/Control System/gaitGen/timeX.txt new file mode 100644 index 0000000..e5a6cce --- /dev/null +++ b/Control System/gaitGen/timeX.txt @@ -0,0 +1,301 @@ +0 +0.01 +0.02 +0.03 +0.04 +0.05 +0.06 +0.07 +0.08 +0.09 +0.1 +0.11 +0.12 +0.13 +0.14 +0.15 +0.16 +0.17 +0.18 +0.19 +0.2 +0.21 +0.22 +0.23 +0.24 +0.25 +0.26 +0.27 +0.28 +0.29 +0.3 +0.31 +0.32 +0.33 +0.34 +0.35 +0.36 +0.37 +0.38 +0.39 +0.4 +0.41 +0.42 +0.43 +0.44 +0.45 +0.46 +0.47 +0.48 +0.49 +0.5 +0.51 +0.52 +0.53 +0.54 +0.55 +0.56 +0.57 +0.58 +0.59 +0.6 +0.61 +0.62 +0.63 +0.64 +0.65 +0.66 +0.67 +0.68 +0.69 +0.7 +0.71 +0.72 +0.73 +0.74 +0.75 +0.76 +0.77 +0.78 +0.79 +0.8 +0.81 +0.82 +0.83 +0.84 +0.85 +0.86 +0.87 +0.88 +0.89 +0.9 +0.91 +0.92 +0.93 +0.94 +0.95 +0.96 +0.97 +0.98 +0.99 +1.0 +1.01 +1.02 +1.03 +1.04 +1.05 +1.06 +1.07 +1.08 +1.09 +1.1 +1.11 +1.12 +1.13 +1.14 +1.15 +1.16 +1.17 +1.18 +1.19 +1.2 +1.21 +1.22 +1.23 +1.24 +1.25 +1.26 +1.27 +1.28 +1.29 +1.3 +1.31 +1.32 +1.33 +1.34 +1.35 +1.36 +1.37 +1.38 +1.39 +1.4 +1.41 +1.42 +1.43 +1.44 +1.45 +1.46 +1.47 +1.48 +1.49 +1.5 +1.51 +1.52 +1.53 +1.54 +1.55 +1.56 +1.57 +1.58 +1.59 +1.6 +1.61 +1.62 +1.63 +1.64 +1.65 +1.66 +1.67 +1.68 +1.69 +1.7 +1.71 +1.72 +1.73 +1.74 +1.75 +1.76 +1.77 +1.78 +1.79 +1.8 +1.81 +1.82 +1.83 +1.84 +1.85 +1.86 +1.87 +1.88 +1.89 +1.9 +1.91 +1.92 +1.93 +1.94 +1.95 +1.96 +1.97 +1.98 +1.99 +2.0 +2.01 +2.02 +2.03 +2.04 +2.05 +2.06 +2.07 +2.08 +2.09 +2.1 +2.11 +2.12 +2.13 +2.14 +2.15 +2.16 +2.17 +2.18 +2.19 +2.2 +2.21 +2.22 +2.23 +2.24 +2.25 +2.26 +2.27 +2.28 +2.29 +2.3 +2.31 +2.32 +2.33 +2.34 +2.35 +2.36 +2.37 +2.38 +2.39 +2.4 +2.41 +2.42 +2.43 +2.44 +2.45 +2.46 +2.47 +2.48 +2.49 +2.5 +2.51 +2.52 +2.53 +2.54 +2.55 +2.56 +2.57 +2.58 +2.59 +2.6 +2.61 +2.62 +2.63 +2.64 +2.65 +2.66 +2.67 +2.68 +2.69 +2.7 +2.71 +2.72 +2.73 +2.74 +2.75 +2.76 +2.77 +2.78 +2.79 +2.8 +2.81 +2.82 +2.83 +2.84 +2.85 +2.86 +2.87 +2.88 +2.89 +2.9 +2.91 +2.92 +2.93 +2.94 +2.95 +2.96 +2.97 +2.98 +2.99 +3.0 diff --git a/Control System/gaitGen/timeZ.txt b/Control System/gaitGen/timeZ.txt new file mode 100644 index 0000000..e5a6cce --- /dev/null +++ b/Control System/gaitGen/timeZ.txt @@ -0,0 +1,301 @@ +0 +0.01 +0.02 +0.03 +0.04 +0.05 +0.06 +0.07 +0.08 +0.09 +0.1 +0.11 +0.12 +0.13 +0.14 +0.15 +0.16 +0.17 +0.18 +0.19 +0.2 +0.21 +0.22 +0.23 +0.24 +0.25 +0.26 +0.27 +0.28 +0.29 +0.3 +0.31 +0.32 +0.33 +0.34 +0.35 +0.36 +0.37 +0.38 +0.39 +0.4 +0.41 +0.42 +0.43 +0.44 +0.45 +0.46 +0.47 +0.48 +0.49 +0.5 +0.51 +0.52 +0.53 +0.54 +0.55 +0.56 +0.57 +0.58 +0.59 +0.6 +0.61 +0.62 +0.63 +0.64 +0.65 +0.66 +0.67 +0.68 +0.69 +0.7 +0.71 +0.72 +0.73 +0.74 +0.75 +0.76 +0.77 +0.78 +0.79 +0.8 +0.81 +0.82 +0.83 +0.84 +0.85 +0.86 +0.87 +0.88 +0.89 +0.9 +0.91 +0.92 +0.93 +0.94 +0.95 +0.96 +0.97 +0.98 +0.99 +1.0 +1.01 +1.02 +1.03 +1.04 +1.05 +1.06 +1.07 +1.08 +1.09 +1.1 +1.11 +1.12 +1.13 +1.14 +1.15 +1.16 +1.17 +1.18 +1.19 +1.2 +1.21 +1.22 +1.23 +1.24 +1.25 +1.26 +1.27 +1.28 +1.29 +1.3 +1.31 +1.32 +1.33 +1.34 +1.35 +1.36 +1.37 +1.38 +1.39 +1.4 +1.41 +1.42 +1.43 +1.44 +1.45 +1.46 +1.47 +1.48 +1.49 +1.5 +1.51 +1.52 +1.53 +1.54 +1.55 +1.56 +1.57 +1.58 +1.59 +1.6 +1.61 +1.62 +1.63 +1.64 +1.65 +1.66 +1.67 +1.68 +1.69 +1.7 +1.71 +1.72 +1.73 +1.74 +1.75 +1.76 +1.77 +1.78 +1.79 +1.8 +1.81 +1.82 +1.83 +1.84 +1.85 +1.86 +1.87 +1.88 +1.89 +1.9 +1.91 +1.92 +1.93 +1.94 +1.95 +1.96 +1.97 +1.98 +1.99 +2.0 +2.01 +2.02 +2.03 +2.04 +2.05 +2.06 +2.07 +2.08 +2.09 +2.1 +2.11 +2.12 +2.13 +2.14 +2.15 +2.16 +2.17 +2.18 +2.19 +2.2 +2.21 +2.22 +2.23 +2.24 +2.25 +2.26 +2.27 +2.28 +2.29 +2.3 +2.31 +2.32 +2.33 +2.34 +2.35 +2.36 +2.37 +2.38 +2.39 +2.4 +2.41 +2.42 +2.43 +2.44 +2.45 +2.46 +2.47 +2.48 +2.49 +2.5 +2.51 +2.52 +2.53 +2.54 +2.55 +2.56 +2.57 +2.58 +2.59 +2.6 +2.61 +2.62 +2.63 +2.64 +2.65 +2.66 +2.67 +2.68 +2.69 +2.7 +2.71 +2.72 +2.73 +2.74 +2.75 +2.76 +2.77 +2.78 +2.79 +2.8 +2.81 +2.82 +2.83 +2.84 +2.85 +2.86 +2.87 +2.88 +2.89 +2.9 +2.91 +2.92 +2.93 +2.94 +2.95 +2.96 +2.97 +2.98 +2.99 +3.0 diff --git a/Control System/gaitGen/urdf.py b/Control System/gaitGen/urdf.py new file mode 100644 index 0000000..ceb1fa6 --- /dev/null +++ b/Control System/gaitGen/urdf.py @@ -0,0 +1,8 @@ +from urdfpy import URDF + +robot = URDF.load('olympian.urdf') + +for link in robot.links: + print(link.name) + +print(robot.base_link.name) \ No newline at end of file diff --git a/Control System/gaitGen/xValues.txt b/Control System/gaitGen/xValues.txt new file mode 100644 index 0000000..93eefbc --- /dev/null +++ b/Control System/gaitGen/xValues.txt @@ -0,0 +1,301 @@ +0.0 +1.4621315464749193e-07 +1.1696282759916451e-06 +3.947062560952751e-06 +9.354563835677412e-06 +1.8267026292003927e-05 +3.155780672995691e-05 +5.009834147459355e-05 +7.475776413522206e-05 +0.00010640252437467412 +0.0001458960078558648 +0.00019409815753227908 +0.00025186509644859953 +0.00032004875221680607 +0.00039949648333261964 +0.0004910507074960234 +0.0005955485320989981 +0.0007138213870426305 +0.0008466946600443892 +0.0009949873345958607 +0.0011595116307294845 +0.0013410726487520092 +0.0015404680161009016 +0.0017584875374785994 +0.001995912848418005 +0.0022535170724313977 +0.0025320644818928317 +0.0028323101628030672 +0.0031549996835838617 +0.003500868768047026 +0.003870642972681651 +0.004265037368401052 +0.0046847562268890475 +0.005130492711683053 +0.005602928574129737 +0.006102733854346301 +0.00663056658731874 +0.007187072514265952 +0.007772884799396335 +0.008388623752180926 +0.009034896555265307 +0.009712296998139136 +0.010421405216680515 +0.011162787438689148 +0.011936995735520178 +0.012744567779927413 +0.01358602661022229 +0.014461880400851945 +0.015372622239497244 +0.016318729910788046 +0.01730066568673118 +0.018318876123942687 +0.01937379186777323 +0.020465827463412897 +0.021595381174057945 +0.02276283480621942 +0.02396855354225015 +0.02521288578016373 +0.02649616298081571 +0.027818699522513785 +0.029180792563121144 +0.030582721909713173 +0.0320247498958449 +0.03350712126648277 +0.035030063070651374 +0.03659378456184233 +0.03819847710622874 +0.039844314098725686 +0.04153145088693344 +0.043260024702996905 +0.04503015460341113 +0.04684194141679901 +0.04869546769968476 +0.050590797700281645 +0.052527977330310824 +0.054507034144862784 +0.05652797733031083 +0.05859079770028165 +0.06069546769968477 +0.06284194141679902 +0.06503015460341113 +0.06726002470299691 +0.06953145088693341 +0.07184431409872565 +0.0741984771062287 +0.07659378456184229 +0.07903006307065134 +0.08150712126648274 +0.08402474989584491 +0.08658272190971318 +0.08918079256312114 +0.0918186995225138 +0.09449616298081573 +0.09721288578016374 +0.0999685535422501 +0.10276283480621937 +0.10559538117405791 +0.10846582746341286 +0.1113737918677732 +0.11431887612394266 +0.11730066568673116 +0.12031872991078801 +0.12337262223949721 +0.12646188040085193 +0.12958602661022225 +0.1327445677799274 +0.13593699573552015 +0.13916278743868915 +0.1424214052166805 +0.14571229699813915 +0.1490348965552653 +0.15238862375218093 +0.15577288479939633 +0.1591870725142659 +0.16263056658731867 +0.16610273385434624 +0.16960292857412967 +0.17313049271168296 +0.17668475622688895 +0.180265037368401 +0.18387064297268157 +0.18750086876804695 +0.1911549996835838 +0.194832310162803 +0.19853206448189276 +0.20225351707243133 +0.20599591284841798 +0.20975848753747856 +0.21354046801610088 +0.217341072648752 +0.22115951163072947 +0.22499498733459586 +0.22884669466004437 +0.23271382138704264 +0.23659554853209902 +0.24049105070749602 +0.24439949648333265 +0.24832004875221683 +0.25225186509644854 +0.2561940981575322 +0.2601458960078558 +0.2641064025243746 +0.26807475776413514 +0.27205009834147453 +0.2760315578067299 +0.28001826702629196 +0.28400935456383564 +0.28800394706256094 +0.29200116962827594 +0.2960001462131546 +0.3 +0.3039998537868453 +0.30799883037172393 +0.311996052937439 +0.3159906454361643 +0.31998173297370797 +0.32396844219327003 +0.3279499016585254 +0.3319252422358647 +0.3358935974756253 +0.3398541039921441 +0.34380590184246773 +0.3477481349035514 +0.35167995124778306 +0.3556005035166673 +0.3595089492925039 +0.3634044514679009 +0.3672861786129573 +0.37115330533995555 +0.3750050126654041 +0.3788404883692704 +0.38265892735124796 +0.38645953198389904 +0.39024151246252137 +0.39400408715158197 +0.39774648292756853 +0.4014679355181071 +0.40516768983719686 +0.40884500031641613 +0.4124991312319529 +0.41612935702731835 +0.41973496263159893 +0.4233152437731109 +0.42686950728831696 +0.43039707142587025 +0.43389726614565366 +0.43736943341268125 +0.44081292748573403 +0.44422711520060354 +0.447611376247819 +0.4509651034447346 +0.4542877030018608 +0.4575785947833194 +0.46083721256131077 +0.4640630042644797 +0.46725543222007254 +0.47041397338977764 +0.473538119599148 +0.4766273777605027 +0.4796812700892119 +0.4826993343132687 +0.48568112387605716 +0.48862620813222674 +0.491534172536587 +0.494404618825942 +0.4972371651937805 +0.5000314464577498 +0.5027871142198361 +0.5055038370191842 +0.5081813004774861 +0.5108192074368788 +0.5134172780902867 +0.515975250104155 +0.5184928787335171 +0.5209699369293487 +0.5234062154381576 +0.5258015228937712 +0.5281556859012743 +0.5304685491130665 +0.532739975297003 +0.5349698453965889 +0.5371580585832009 +0.5393045323003153 +0.5414092022997183 +0.5434720226696892 +0.5454929658551372 +0.5474720226696891 +0.5494092022997183 +0.5513045323003152 +0.553158058583201 +0.5549698453965888 +0.556739975297003 +0.5584685491130665 +0.5601556859012743 +0.5618015228937712 +0.5634062154381577 +0.5649699369293486 +0.5664928787335173 +0.5679752501041551 +0.5694172780902869 +0.5708192074368788 +0.5721813004774862 +0.5735038370191843 +0.5747871142198363 +0.5760314464577498 +0.5772371651937807 +0.578404618825942 +0.5795341725365871 +0.5806262081322268 +0.5816811238760573 +0.5826993343132688 +0.5836812700892119 +0.5846273777605028 +0.585538119599148 +0.5864139733897777 +0.5872554322200726 +0.5880630042644799 +0.5888372125613108 +0.5895785947833195 +0.5902877030018608 +0.5909651034447347 +0.591611376247819 +0.5922271152006038 +0.592812927485734 +0.5933694334126813 +0.5938972661456536 +0.5943970714258703 +0.5948695072883169 +0.595315243773111 +0.5957349626315989 +0.5961293570273184 +0.596499131231953 +0.5968450003164162 +0.5971676898371969 +0.5974679355181072 +0.5977464829275686 +0.598004087151582 +0.5982415124625213 +0.5984595319838991 +0.5986589273512479 +0.5988404883692705 +0.5990050126654041 +0.5991533053399556 +0.5992861786129573 +0.599404451467901 +0.599508949292504 +0.5996005035166674 +0.5996799512477832 +0.5997481349035514 +0.5998059018424677 +0.5998541039921441 +0.5998935974756253 +0.5999252422358647 +0.5999499016585254 +0.5999684421932701 +0.5999817329737079 +0.5999906454361643 +0.599996052937439 +0.599998830371724 +0.5999998537868453 +0.6 diff --git a/Control System/gaitGen/zValues.txt b/Control System/gaitGen/zValues.txt new file mode 100644 index 0000000..99405c3 --- /dev/null +++ b/Control System/gaitGen/zValues.txt @@ -0,0 +1,301 @@ +0.0 +7.79752183994141e-07 +6.236375890451319e-06 +2.1038537819973585e-05 +4.9838509423480795e-05 +9.726400523724608e-05 +0.00016791006429906982 +0.0002663309888884177 +0.0003970323547326689 +0.0005644631066962641 +0.0007730077538196621 +0.0010269786774006057 +0.001330608565612009 +0.0016880429879285591 +0.0021033331223892435 +0.0025804286484544456 +0.003123170817926034 +0.0037352857160864936 +0.004420377724879167 +0.005181923199597559 +0.006023264370176876 +0.006947603477787008 +0.00795799715701346 +0.009057351073481532 +0.010248414826331508 +0.011533777124487464 +0.012915861245182153 +0.01439692078270531 +0.015979035694833436 +0.01766410865387715 +0.019453861708747446 +0.02134983326389662 +0.02335337538043425 +0.02546565140415251 +0.0276876339246223 +0.03002010306894075 +0.03246364513312318 +0.03501865155354056 +0.03768531822020722 +0.04046364513312317 +0.0433534364022741 +0.04635430059128895 +0.049465651404152475 +0.05268670871376758 +0.05601649993056328 +0.05945386170874743 +0.0629974419872105 +0.06664570236150008 +0.07039692078270529 +0.07424919457851548 +0.07820044379115412 +0.0822484148263315 +0.08639068440681484 +0.09062466382368012 +0.09494760347778702 +0.0993565977035102 +0.10384858986626423 +0.10842037772487914 +0.11306861904941978 +0.11778983748459265 +0.12258042864845442 +0.12743666645572255 +0.1323547096545952 +0.137330608565612 +0.1423603120107339 +0.1474396744204863 +0.15256446310669627 +0.157730365688066 +0.16293299765555508 +0.16816791006429904 +0.17343059733857052 +0.1787165051760901 +0.18402103853781995 +0.18933956970922375 +0.19466744641885064 +0.2 +0.2053325535811493 +0.2106604302907762 +0.21597896146217999 +0.22128349482390983 +0.22656940266142941 +0.23183208993570092 +0.23706700234444486 +0.24226963431193396 +0.2474355368933037 +0.25256032557951363 +0.25763968798926606 +0.26266939143438794 +0.26764529034540474 +0.2725633335442774 +0.27741957135154555 +0.2822101625154073 +0.2869313809505802 +0.2915796222751208 +0.29615141013373575 +0.3006434022964897 +0.30505239652221294 +0.30937533617631985 +0.3136093155931851 +0.31775158517366847 +0.32179955620884587 +0.3257508054214845 +0.3296030792172947 +0.3333542976384999 +0.3370025580127895 +0.3405461382912526 +0.34398350006943673 +0.3473132912862324 +0.35053434859584753 +0.35364569940871105 +0.3566465635977259 +0.35953635486687685 +0.3623146817797928 +0.3649813484464594 +0.3675363548668768 +0.3699798969310592 +0.37231236607537765 +0.37453434859584755 +0.3766466246195658 +0.37865016673610336 +0.3805461382912526 +0.38233589134612284 +0.3840209643051666 +0.38560307921729475 +0.38708413875481784 +0.3884662228755125 +0.38975158517366854 +0.39094264892651853 +0.39204200284298657 +0.393052396522213 +0.39397673562982316 +0.3948180768004025 +0.3955796222751209 +0.3962647142839135 +0.396876829182074 +0.3974195713515456 +0.39789666687761077 +0.39831195701207145 +0.39866939143438807 +0.39897302132259943 +0.39922699224618036 +0.3994355368933038 +0.39960296764526737 +0.39973366901111157 +0.39983208993570096 +0.3999027359947628 +0.39995016149057655 +0.3999789614621801 +0.39999376362410954 +0.39999922024781603 +0.4 +0.399999220247816 +0.39999376362410954 +0.39997896146218004 +0.3999501614905765 +0.3999027359947627 +0.39983208993570096 +0.39973366901111157 +0.3996029676452673 +0.39943553689330374 +0.3992269922461803 +0.3989730213225994 +0.398669391434388 +0.39831195701207145 +0.39789666687761077 +0.3974195713515456 +0.39687682918207395 +0.3962647142839135 +0.3955796222751209 +0.3948180768004025 +0.39397673562982316 +0.393052396522213 +0.39204200284298657 +0.39094264892651853 +0.38975158517366854 +0.3884662228755126 +0.3870841387548179 +0.38560307921729475 +0.3840209643051666 +0.3823358913461229 +0.3805461382912526 +0.3786501667361034 +0.3766466246195658 +0.37453434859584755 +0.3723123660753777 +0.36997989693105926 +0.36753635486687686 +0.36498134844645946 +0.36231468177979287 +0.35953635486687685 +0.35664656359772606 +0.35364569940871116 +0.35053434859584753 +0.34731329128623256 +0.3439835000694368 +0.3405461382912526 +0.3370025580127896 +0.3333542976385 +0.32960307921729476 +0.3257508054214846 +0.3217995562088459 +0.3177515851736687 +0.3136093155931853 +0.30937533617632007 +0.30505239652221305 +0.30064340229648995 +0.2961514101337358 +0.291579622275121 +0.28693138095058024 +0.28221016251540754 +0.2774195713515456 +0.27256333354427753 +0.26764529034540485 +0.26266939143438817 +0.2576396879892662 +0.2525603255795139 +0.24743553689330366 +0.24226963431193416 +0.2370670023444449 +0.23183208993570104 +0.22656940266142944 +0.22128349482390997 +0.21597896146217999 +0.2106604302907763 +0.20533255358114932 +0.20000000000000007 +0.19466744641885092 +0.18933956970922394 +0.18402103853782015 +0.17871650517609017 +0.1734305973385707 +0.1681679100642991 +0.16293299765555525 +0.1577303656880661 +0.15256446310669647 +0.14743967442048644 +0.14236031201073418 +0.13733060856561208 +0.1323547096545955 +0.1274366664557227 +0.12258042864845453 +0.11778983748459271 +0.11306861904942 +0.10842037772487922 +0.10384858986626444 +0.09935659770351024 +0.09494760347778708 +0.09062466382368017 +0.08639068440681497 +0.0822484148263315 +0.07820044379115432 +0.07424919457851575 +0.07039692078270532 +0.06664570236150025 +0.06299744198721058 +0.05945386170874756 +0.056016499930563346 +0.052686708713767794 +0.04946565140415249 +0.04635430059128898 +0.04335343640227407 +0.04046364513312328 +0.037685318220207265 +0.03501865155354067 +0.032463645133123165 +0.030020103068940762 +0.027687633924622257 +0.02546565140415258 +0.02335337538043425 +0.021349833263896723 +0.01945386170874741 +0.01766410865387713 +0.015979035694833388 +0.01439692078270538 +0.01291586124518218 +0.011533777124487554 +0.010248414826331542 +0.00905735107348149 +0.007957997157013508 +0.006947603477787001 +0.006023264370176973 +0.005181923199597649 +0.004420377724879132 +0.0037352857160865005 +0.003123170817926013 +0.0025804286484544248 +0.002103333122389306 +0.001688042987928573 +0.001330608565611957 +0.0010269786774005363 +0.0007730077538196101 +0.0005644631066962225 +0.0003970323547326515 +0.0002663309888885079 +0.00016791006429905941 +9.726400523724088e-05 +4.983850942352763e-05 +2.1038537819983993e-05 +6.236375890478207e-06 +7.797521840435806e-07 +0.0