add gait gen

control_systems/.gitignore

@@ -0,0 +1,32 @@
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app

control_systems/.replit

@@ -0,0 +1,2 @@
+language = "undefined"
+run = ""

control_systems/Control System/Propioception.py

@@ -0,0 +1,209 @@
+"""
+This class keeps track of all the motors, sensors and import variables needed for the robot.
+"""
+class Propioception:
+    """
+    Standard kwargs to give for this function
+
+    data = {
+        # Motors (all units are in degrees)
+        'left_hip_pitch': 0,
+        'left_hip_yaw': 0,
+        'left_hip_roll': 0,
+        'left_knee_pitch': 0,
+        'left_ankle_pitch': 0,
+        'left_ankle_yaw': 0,
+        'left_ankle_roll': 0,
+        'right_hip_pitch': 0,
+        'right_hip_yaw': 0,
+        'right_hip_roll': 0,
+        'right_knee_pitch': 0,
+        'right_ankle_pitch': 0,
+        'right_ankle_yaw': 0,
+        'right_ankle_roll': 0,
+        'left_shoulder_pitch': 0,
+        'left_shoulder_yaw': 0,
+        'left_shoulder_roll': 0,
+        'left_elbow_pitch': 0,
+        'right_shoulder_pitch': 0,
+        'right_shoulder_yaw': 0,
+        'right_shoulder_roll': 0,
+        'right_elbow_pitch': 0,
+        'torso_pitch': 0,
+        'torso_yaw': 0,
+        'torso_roll': 0,
+        'neck_pitch': 0,
+        'neck_yaw': 0,
+        'neck_roll': 0,
+
+        # Sensors
+        # Add later
+    }
+
+    This will also have the constants
+    """
+    def __init__(self, **kwargs):
+        self.data = {
+            # Add defualt later
+        }
+        self.constants = {
+            # Add some constants here later
+        }
+        for key, value in kwargs.items():
+            self.data[str(key)] = value
+
+    def update(self, partsAndValues):
+        """
+        This function updates the dictionary
+
+        arg artsAndValues is {string --> int or float}
+
+        Error codes:
+        1 is success
+        -1 is the error for NoneType variables
+        -2 is for incorrect datatypes
+        -3 is for when the key is not in the dictionary
+        """
+
+        if partsAndValues == None:
+            return -1
+
+        if type(partsAndValues) != dict:
+            return -2
+
+        for key in partsAndValues.keys():
+            if key in self.data:
+                self.data[str(key)] = partsAndValues[key]
+            else:
+                return -3
+            return 1
+
+    def add(self, partsAndValues):
+        """
+        This function will add a part to the system if needed
+
+        arg artsAndValues is {string --> int or float}
+
+        Error codes:
+        1 is success
+        -1 is the error for NoneType variables
+        -2 is for incorrect datatypes
+        -3 is for when the key is in the dictionary
+        """
+
+        if partsAndValues == None:
+            return -1
+
+        if type(partsAndValues) != dict:
+            return -2
+
+        for key in partsAndValues.keys():
+            if key in self.data:
+                return -3
+            else:
+                self.data[str(key)] = partsAndValues[key]
+            return 1
+
+    def remove(self, partsAndValues):
+        """
+        This function will remove a part to the system if needed
+
+        arg artsAndValues is [string]
+
+        Error codes:
+        1 is success
+        -1 is the error for NoneType variables
+        -2 is for incorrect datatypes
+        -3 is for when the key is not in the dictionary
+        """
+
+        if partsAndValues == None:
+            return -1
+            
+
+        if type(partsAndValues) != list:
+            return -2
+
+        for key in partsAndValues:
+            if key in self.data:
+                self.data.pop(str(key))
+            else:
+                return -3
+            return 1
+        
+
+# # Test
+# stuff = Propioception(
+#         left_hip_pitch= 0,
+#         left_hip_yaw= 0,
+#         left_hip_roll= 0,
+#         left_knee_pitch= 0,
+#         left_ankle_pitch= 0,
+#         left_ankle_yaw= 0,
+#         left_ankle_roll= 0,
+#         right_hip_pitch= 0,
+#         right_hip_yaw= 0,
+#         right_hip_roll= 0,
+#         right_knee_pitch= 0,
+#         right_ankle_pitch= 0,
+#         right_ankle_yaw= 0,
+#         right_ankle_roll= 0,
+#         left_shoulder_pitch= 0,
+#         left_shoulder_yaw= 0,
+#         left_shoulder_roll= 0,
+#         left_elbow_pitch= 0,
+#         right_shoulder_pitch= 0,
+#         right_shoulder_yaw= 0,
+#         right_shoulder_roll= 0,
+#         right_elbow_pitch= 0,
+#         torso_pitch= 0,
+#         torso_yaw= 0,
+#         torso_roll= 0,
+#         neck_pitch= 0,
+#         neck_yaw= 0,
+#         neck_roll= 0
+#         )
+        
+# """Update cases"""
+# # Success case
+# print(stuff.data['neck_roll'])        
+# stuff.update({'neck_roll': 21})
+# print(stuff.data['neck_roll'])
+
+# # -1 case
+# print(stuff.update(None))
+
+# # -2 case
+# print(stuff.update([]))
+
+# # -3 case
+# print(stuff.update({'test': 21}))
+
+# """Remove cases"""
+# # Success case
+# print(stuff.data['neck_roll'])        
+# print(stuff.remove(['neck_roll']))
+
+# # -1 case
+# print(stuff.remove(None))
+
+# # # -2 case
+# print(stuff.remove({}))
+
+# # -3 case
+# print(stuff.remove(['test']))
+
+# """Add cases"""
+# # Success case
+# print('test' in stuff.data.keys())        
+# stuff.add({'test': 21})
+# print(stuff.data['test'])
+
+# # -1 case
+# print(stuff.add(None))
+
+# # -2 case
+# print(stuff.add([]))
+
+# # -3 case
+# print(stuff.add({'test': 21}))

control_systems/Control System/bootup.py

@@ -0,0 +1,135 @@
+# Create priority queue
+# In main create three threads
+# 1 for managing actions (stores priority queue)
+# 1 for reading sensor data
+# Updates sensor data through wifi and propiosense system
+# 1 for executing the first action in the priority queue
+
+from Propioception import *
+# from commands import *
+
+import re
+import threading
+import socket
+import heapq
+import time
+#import serial
+
+class Receiver:
+  def __init__(self, host, port):
+    self.commands = []
+    self.executions = []
+    self.transmit = []
+    self.timer = 0
+    self.HOST = host
+    self.PORT = port
+
+    #connecting using scokets
+    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    print('Socket created')
+    #managing error exception
+    try:
+      s.bind((self.HOST, self.PORT))
+    except socket.error:
+        print ('Bind failed ')
+
+    s.listen(5)
+    print('Socket awaiting messages')
+    (self.conn, addr) = s.accept()
+    print('Connected')
+  
+  """Method receives commands from client and add to queue"""
+  def telemetryReceive(self):
+    # Commands must be in form "PRIORITY|{COMMAND}|TIMESTAMP|CHECKSUM"
+    # awaiting for message
+    while True:
+      action = self.conn.recv(1024).decode('UTF-8')
+      if len(action) > 0:
+        heapq.heappush(self.commands,action)
+        print("Received|"+action)
+        heapq.heappush(self.transmit,"Received|"+action)
+          
+  """Method checks commands from queue and adds to execution queue"""
+  def checkCommand(self):
+    while True:
+      if len(self.commands) > 0:
+        #checking if the checksum of the command
+        #equals the sum of all ascii values of every character 
+        #in command statement
+        pattern = "^[0-5]\|.*\|[0-9]{2}:[0-9]{2}\|" #everything but the checksum value
+        checksum = "\w+$" #checksum value
+        popped = heapq.heappop(self.commands) #gets smallest value command
+        com = re.findall(pattern, popped) 
+        numval = re.findall(checksum, popped)
+        numval = numval[0]
+        numval = int(numval,16) #converts hex to int
+        print(com[0])
+        print(sum([ord(i) for i in com[0]]))
+        if numval == sum([ord(i) for i in com[0]]):
+          print("working")
+          heapq.heappush(self.transmit, "Correct|"+popped)
+          heapq.heappush(self.executions, popped)
+          print(self.transmit)
+          print(self.executions)
+        else: 
+          heapq.heappush(self.transmit, "Incorrect|"+popped)
+        
+  def telemetryTransmit(self):
+    while True:
+      if len(self.transmit) > 0:
+        print("Transmit queue", self.transmit)
+        self.conn.send(bytes(heapq.heappop(self.transmit),'utf-8'))    
+
+  def execute(self):
+    while True:
+      if len(self.executions) > 0:
+        command = heapq.heappop(self.executions)
+        print(command)
+        heapq.heappush(self.transmit, "Executed|"+command)
+        # if command == "Password A_on_LED":
+          # if onAndOfffLed():
+          #   print("Executed: ", command)
+          # else:
+          #   print("Did not execute correctly ", command)
+        print("Inside execute",self.executions)
+        time.sleep(5)
+        if "Balancing" not in self.executions:
+          heapq.heappush(self.executions, "Balancing")
+
+  def balance(self):
+    print("Nothing")
+
+  def gaitGen(self):
+    print("Nothing")
+
+  def computerVision(self):
+    print("Nothing")
+
+  def sensorData(self):
+    var = ""
+    # Test
+  #  print("Inside")
+  #  ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
+  #  ser.reset_input_buffer()
+  #  while True:
+  #    print("Inside data")
+  ##    # Read from Arduinos to know what motors and sensors there are
+    #  ser.write("Send ****s plz\n".encode('utf-8'))
+    #  line = ser.readline().decode('utf-8').rstrip()
+    #  print(line)
+
+  def runSimul(self):
+    threading.Thread(target=self.telemetryReceive).start()
+    threading.Thread(target=self.checkCommand).start()
+    threading.Thread(target=self.telemetryTransmit).start()
+    threading.Thread(target=self.execute).start()
+
+    # threading.Thread(target=self.sensorData).start()
+
+    # threading.Thread(target=self.balance).start()
+    threading.Thread(target=self.gaitGen).start()
+    # threading.Thread(target=self.comuterVision).start()
+
+def startBoot():
+  simulation = Receiver('10.235.1.145',12345)
+  simulation.runSimul()

control_systems/Control System/commands.py

@@ -0,0 +1,24 @@
+# Commands for arduino
+
+import serial
+import time
+
+ser = serial.Serial('/dev/ttyACM0', 9600, timeout=1) # fix
+ser.reset_input_buffer()
+
+def onAndOfffLed():
+    ser.write("ON\n".encode('utf-8'))
+    line = ser.readline().decode('utf-8').rstrip()
+    print(line)
+
+    time.sleep(4)
+
+    ser.write("OFF\n".encode('utf-8'))
+    line = ser.readline().decode('utf-8').rstrip()
+    print(line)
+    time.sleep(4)
+
+    if line == "You sent me: OFF":
+        return 1
+    else:
+        return 0

control_systems/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)

control_systems/Control System/gaitGen/olympian.urdf

@@ -0,0 +1,927 @@
+<?xml version="1.0" ?>
+<robot name="olympian">
+
+<material name="silver">
+  <color rgba="0.700 0.700 0.700 1.000"/>
+</material>
+
+<link name="base_link">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.10000000000000274 -1.5987211554602254e-16 0.8751666232732108"/>
+    <mass value="1.1521008690150791"/>
+    <inertia ixx="0.008991901943259162" ixy="-9.892367496064471e-18" ixz="0.0" iyy="0.005223496631877156" iyz="-6.132347048742014e-19" izz="0.008530264202422316"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <geometry>
+      <mesh filename="meshes/base_link.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <geometry>
+      <mesh filename="meshes/base_link.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Pelvis_v7_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.789435349370706e-15 0.049999999999999864 0.029353425629582097"/>
+    <mass value="0.5947505206015595"/>
+    <inertia ixx="0.0020170369281689027" ixy="-2.806717788186033e-19" ixz="6.938893903907228e-18" iyy="0.001878872568180201" iyz="-2.3514580861105255e-19" izz="0.0010976440297574977"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 0.05 -0.915"/>
+    <geometry>
+      <mesh filename="meshes/Pelvis_v7_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 0.05 -0.915"/>
+    <geometry>
+      <mesh filename="meshes/Pelvis_v7_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Chest_v13_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.03750000000000275 -8.168836624811604e-05 0.2954266005963746"/>
+    <mass value="5.882339234080889"/>
+    <inertia ixx="0.462024450225778" ixy="-1.2945373939476923e-16" ixz="-5.551115123125783e-16" iyy="0.3281120038896219" iyz="-0.0001155294730347088" izz="0.23780786856479988"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.0625 0.0 -0.99"/>
+    <geometry>
+      <mesh filename="meshes/Chest_v13_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.0625 0.0 -0.99"/>
+    <geometry>
+      <mesh filename="meshes/Chest_v13_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Box_Head_v5_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="1.8735013540549517e-15 2.486899575160351e-16 0.12700000029388225"/>
+    <mass value="0.4129840000000015"/>
+    <inertia ixx="0.00953546956266682" ixy="-1.2615370792445878e-16" ixz="-8.326672684688674e-17" iyy="0.009535469562667043" iyz="-9.005388077923778e-18" izz="0.009535469562666902"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 0.0 -1.5564"/>
+    <geometry>
+      <mesh filename="meshes/Box_Head_v5_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 0.0 -1.5564"/>
+    <geometry>
+      <mesh filename="meshes/Box_Head_v5_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Upper_Arm_Link_type_b_v9_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="3.011479954295737e-15 -0.03931045800644434 -0.010955881721682825"/>
+    <mass value="0.11011202726048713"/>
+    <inertia ixx="0.00018206077085142214" ixy="0.0" ixz="0.0" iyy="0.00016930933911268875" iyz="-1.861439297221046e-05" izz="0.00022075846035968932"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 0.275 -1.5064"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_Link_type_b_v9_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 0.275 -1.5064"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_Link_type_b_v9_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Upper_Arm_Roll_Link_v5_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.05000000000000271 7.771561172376096e-16 -0.022135117628713896"/>
+    <mass value="0.29143432706119626"/>
+    <inertia ixx="0.0002924682777393217" ixy="1.734723475976807e-18" ixz="-6.938893903907228e-18" iyy="0.001232763287599803" iyz="-2.7755575615628914e-17" izz="0.0011938412922212135"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.05 0.35 -1.4939"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_Roll_Link_v5_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.05 0.35 -1.4939"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_Roll_Link_v5_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Upper_Arm_v10_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.706168622523819e-15 -9.46964984205323e-09 -0.20731274229984487"/>
+    <mass value="0.9419240929020993"/>
+    <inertia ixx="0.018812734327517955" ixy="5.35682609381638e-15" ixz="2.7755575615628914e-17" iyy="0.019683528982801013" iyz="-1.6353901011179062e-09" izz="0.00349831999855843"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 0.35 -1.43755"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_v10_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 0.35 -1.43755"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_v10_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Lower_Arm_v8_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="-0.04999999999999716 -1.5210055437364645e-14 -0.22711717106463214"/>
+    <mass value="1.1287862597517475"/>
+    <inertia ixx="0.025477987567271376" ixy="5.370703881624195e-15" ixz="-1.3877787807814457e-17" iyy="0.025545638419054773" iyz="1.887379141862766e-15" izz="0.0030604925430051666"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.15 0.35 -1.07005"/>
+    <geometry>
+      <mesh filename="meshes/Lower_Arm_v8_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.15 0.35 -1.07005"/>
+    <geometry>
+      <mesh filename="meshes/Lower_Arm_v8_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Upper_Arm_Link_type_b_v9_2">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.5118795932144167e-15 0.039310458006443616 -0.010955881721682825"/>
+    <mass value="0.11011202726048713"/>
+    <inertia ixx="0.00018206077085147765" ixy="-1.3010426069826053e-18" ixz="-3.469446951953614e-18" iyy="0.00016930933911263324" iyz="1.8614392972175764e-05" izz="0.00022075846035969453"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 -0.275 -1.5064"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_Link_type_b_v9_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 -0.275 -1.5064"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_Link_type_b_v9_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Upper_Arm_Roll_Link_v5_2">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.0500000000000025 -1.0547118733938987e-15 -0.022135117628713452"/>
+    <mass value="0.29143432706119626"/>
+    <inertia ixx="0.0002924682777396548" ixy="0.0" ixz="0.0" iyy="0.001232763287599914" iyz="-5.551115123125783e-17" izz="0.0011938412922211997"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.05 -0.35 -1.4939"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_Roll_Link_v5_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.05 -0.35 -1.4939"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_Roll_Link_v5_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Upper_Arm_v10_3">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.6922908347160046e-15 -9.469651840454674e-09 -0.2073127422998453"/>
+    <mass value="0.9419240929020993"/>
+    <inertia ixx="0.018812734327517955" ixy="5.370703881624195e-15" ixz="1.3877787807814457e-17" iyy="0.019683528982801457" iyz="-1.6353899900956037e-09" izz="0.0034983199985584718"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 -0.35 -1.43755"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_v10_3.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 -0.35 -1.43755"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Arm_v10_3.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Lower_Arm_v8_2">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.05000000000000264 -1.7263968032921184e-14 -0.22711717106463192"/>
+    <mass value="1.1287862597517475"/>
+    <inertia ixx="0.025477987567271376" ixy="5.3637649877202875e-15" ixz="0.0" iyy="0.02554563841905466" iyz="1.7763568394002505e-15" izz="0.003060492543005139"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.05 -0.35 -1.07005"/>
+    <geometry>
+      <mesh filename="meshes/Lower_Arm_v8_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.05 -0.35 -1.07005"/>
+    <geometry>
+      <mesh filename="meshes/Lower_Arm_v8_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Leg_Pitch_v5_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.7478019859472624e-15 -0.04683508876022918 0.003164911239771362"/>
+    <mass value="0.3280666863874879"/>
+    <inertia ixx="0.0009464761938969279" ixy="0.0" ixz="6.938893903907228e-18" iyy="0.0010883631339250188" iyz="5.307553597741488e-05" izz="0.0010883631339250258"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 0.1 -0.815"/>
+    <geometry>
+      <mesh filename="meshes/Leg_Pitch_v5_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 0.1 -0.815"/>
+    <geometry>
+      <mesh filename="meshes/Leg_Pitch_v5_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Leg_Roll_v4_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.07500000000000276 2.498001805406602e-16 -0.035223067053444135"/>
+    <mass value="1.3532798434145876"/>
+    <inertia ixx="0.002693883608054004" ixy="3.469446951953614e-18" ixz="0.0" iyy="0.008928053332240449" iyz="0.0" izz="0.00837264452845498"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.025 0.15 -0.815"/>
+    <geometry>
+      <mesh filename="meshes/Leg_Roll_v4_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.025 0.15 -0.815"/>
+    <geometry>
+      <mesh filename="meshes/Leg_Roll_v4_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Upper_Leg_v4_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.4702462297909733e-15 1.6653345369377348e-16 -0.15757232755407302"/>
+    <mass value="1.7214182603818151"/>
+    <inertia ixx="0.024564314520888808" ixy="-3.469446951953614e-18" ixz="2.7755575615628914e-17" iyy="0.023647064861463507" iyz="-2.7755575615628914e-17" izz="0.01202868114191328"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 0.15 -0.74"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Leg_v4_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 0.15 -0.74"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Leg_v4_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Lower_Leg_v6_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.345346139520643e-15 0.04999999999999144 -0.14147821247779985"/>
+    <mass value="1.4272888897799032"/>
+    <inertia ixx="0.020278823604420432" ixy="-2.2169766022983595e-15" ixz="2.0816681711721685e-17" iyy="0.02027203031763286" iyz="-1.3183898417423734e-15" izz="0.003070799531336911"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 0.2 -0.4418"/>
+    <geometry>
+      <mesh filename="meshes/Lower_Leg_v6_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 0.2 -0.4418"/>
+    <geometry>
+      <mesh filename="meshes/Lower_Leg_v6_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Ankle_Link_v5_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.03000000000000269 5.551115123125783e-16 -0.014239102534719814"/>
+    <mass value="0.02966359637804546"/>
+    <inertia ixx="2.6914982867941393e-05" ixy="0.0" ixz="-1.0842021724855044e-19" iyy="3.13471723067344e-05" iyz="5.421010862427522e-20" izz="1.4582407636994869e-05"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.07 0.15 -0.0886"/>
+    <geometry>
+      <mesh filename="meshes/Ankle_Link_v5_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.07 0.15 -0.0886"/>
+    <geometry>
+      <mesh filename="meshes/Ankle_Link_v5_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Foot_v4_1">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.03625642783113289 0.019685521169741094 -0.0403940100478858"/>
+    <mass value="0.8921618023336354"/>
+    <inertia ixx="0.001976748372483468" ixy="2.766319069119616e-06" ixz="0.0001489881145850828" iyy="0.005514801902865209" iyz="-1.2922841769059251e-06" izz="0.00723725227735432"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 0.17 -0.0536"/>
+    <geometry>
+      <mesh filename="meshes/Foot_v4_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 0.17 -0.0536"/>
+    <geometry>
+      <mesh filename="meshes/Foot_v4_1.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Leg_Pitch_v5_2">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.3869795029440866e-15 0.046835088760228516 0.003164911239771251"/>
+    <mass value="0.3280666863874879"/>
+    <inertia ixx="0.0009464761938969557" ixy="-8.673617379884035e-19" ixz="3.469446951953614e-18" iyy="0.001088363133924991" iyz="-5.307553597740794e-05" izz="0.0010883631339250258"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 -0.1 -0.815"/>
+    <geometry>
+      <mesh filename="meshes/Leg_Pitch_v5_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 -0.1 -0.815"/>
+    <geometry>
+      <mesh filename="meshes/Leg_Pitch_v5_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Leg_Roll_v4_2">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.07500000000000273 -2.7755575615628914e-16 -0.03522306705344391"/>
+    <mass value="1.3532798434145876"/>
+    <inertia ixx="0.002693883608054115" ixy="-6.938893903907228e-18" ixz="1.3877787807814457e-17" iyy="0.008928053332240782" iyz="0.0" izz="0.008372644528454966"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.025 -0.15 -0.815"/>
+    <geometry>
+      <mesh filename="meshes/Leg_Roll_v4_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.025 -0.15 -0.815"/>
+    <geometry>
+      <mesh filename="meshes/Leg_Roll_v4_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Upper_Leg_v4_2">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.5118795932144167e-15 -1.1102230246251565e-16 -0.15757232755407302"/>
+    <mass value="1.7214182603818151"/>
+    <inertia ixx="0.02456431452088903" ixy="0.0" ixz="-1.3877787807814457e-17" iyy="0.023647064861463618" iyz="0.0" izz="0.012028681141913286"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 -0.15 -0.74"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Leg_v4_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 -0.15 -0.74"/>
+    <geometry>
+      <mesh filename="meshes/Upper_Leg_v4_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Lower_Leg_v6_2">
+  <inertial>
+    <origin rpy="0 0 0" xyz="2.761679773755077e-15 -0.05000000000000848 -0.14147821247779996"/>
+    <mass value="1.4272888897799032"/>
+    <inertia ixx="0.020278823604420404" ixy="-2.1961599205866378e-15" ixz="1.3877787807814457e-17" iyy="0.02027203031763286" iyz="-1.2906342661267445e-15" izz="0.003070799531336904"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 -0.2 -0.4418"/>
+    <geometry>
+      <mesh filename="meshes/Lower_Leg_v6_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 -0.2 -0.4418"/>
+    <geometry>
+      <mesh filename="meshes/Lower_Leg_v6_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Ankle_Link_v5_2">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.030000000000002913 1.6653345369377348e-16 -0.014239102534719814"/>
+    <mass value="0.02966359637804546"/>
+    <inertia ixx="2.6914982867941284e-05" ixy="1.0842021724855044e-19" ixz="-1.0842021724855044e-19" iyy="3.13471723067344e-05" iyz="0.0" izz="1.4582407636994869e-05"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.07 -0.15 -0.0886"/>
+    <geometry>
+      <mesh filename="meshes/Ankle_Link_v5_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.07 -0.15 -0.0886"/>
+    <geometry>
+      <mesh filename="meshes/Ankle_Link_v5_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<link name="Foot_v4_2">
+  <inertial>
+    <origin rpy="0 0 0" xyz="0.03625642783113234 -0.02031447883025933 -0.04039401004788552"/>
+    <mass value="0.8921618023336354"/>
+    <inertia ixx="0.001976748372483454" ixy="2.7663190691230855e-06" ixz="0.00014898811458507067" iyy="0.00551480190286522" iyz="-1.2922841769050578e-06" izz="0.007237252277354382"/>
+  </inertial>
+  <visual>
+    <origin rpy="0 0 0" xyz="-0.1 -0.17 -0.0536"/>
+    <geometry>
+      <mesh filename="meshes/Foot_v4_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+    <material name="silver"/>
+    <material/>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="-0.1 -0.17 -0.0536"/>
+    <geometry>
+      <mesh filename="meshes/Foot_v4_2.stl" scale="0.001 0.001 0.001"/>
+    </geometry>
+  </collision>
+</link>
+
+<joint name="Rev1" type="continuous">
+  <origin rpy="0 0 0" xyz="0.1 -0.05 0.915"/>
+  <parent link="base_link"/>
+  <child link="Pelvis_v7_1"/>
+  <axis xyz="-0.0 1.0 0.0"/>
+</joint>
+<transmission name="Rev1_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev1">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev1_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev2" type="continuous">
+  <origin rpy="0 0 0" xyz="-0.0375 0.05 0.075"/>
+  <parent link="Pelvis_v7_1"/>
+  <child link="Chest_v13_1"/>
+  <axis xyz="-1.0 -0.0 -0.0"/>
+</joint>
+<transmission name="Rev2_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev2">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev2_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev11" type="continuous">
+  <origin rpy="0 0 0" xyz="0.0375 0.0 0.5664"/>
+  <parent link="Chest_v13_1"/>
+  <child link="Box_Head_v5_1"/>
+  <axis xyz="-0.0 -0.0 1.0"/>
+</joint>
+<transmission name="Rev11_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev11">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev11_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev3" type="continuous">
+  <origin rpy="0 0 0" xyz="0.0375 -0.275 0.5164"/>
+  <parent link="Chest_v13_1"/>
+  <child link="Upper_Arm_Link_type_b_v9_1"/>
+  <axis xyz="0.0 -1.0 -0.0"/>
+</joint>
+<transmission name="Rev3_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev3">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev3_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev4" type="continuous">
+  <origin rpy="0 0 0" xyz="-0.05 -0.075 -0.0125"/>
+  <parent link="Upper_Arm_Link_type_b_v9_1"/>
+  <child link="Upper_Arm_Roll_Link_v5_1"/>
+  <axis xyz="-1.0 -0.0 0.0"/>
+</joint>
+<transmission name="Rev4_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev4">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev4_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev5" type="continuous">
+  <origin rpy="0 0 0" xyz="0.05 0.0 -0.05635"/>
+  <parent link="Upper_Arm_Roll_Link_v5_1"/>
+  <child link="Upper_Arm_v10_1"/>
+  <axis xyz="0.0 0.0 -1.0"/>
+</joint>
+<transmission name="Rev5_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev5">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev5_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev6" type="continuous">
+  <origin rpy="0 0 0" xyz="0.05 0.0 -0.3675"/>
+  <parent link="Upper_Arm_v10_1"/>
+  <child link="Lower_Arm_v8_1"/>
+  <axis xyz="-1.0 -0.0 0.0"/>
+</joint>
+<transmission name="Rev6_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev6">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev6_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev7" type="continuous">
+  <origin rpy="0 0 0" xyz="0.0375 0.275 0.5164"/>
+  <parent link="Chest_v13_1"/>
+  <child link="Upper_Arm_Link_type_b_v9_2"/>
+  <axis xyz="-0.0 1.0 0.0"/>
+</joint>
+<transmission name="Rev7_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev7">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev7_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev8" type="continuous">
+  <origin rpy="0 0 0" xyz="-0.05 0.075 -0.0125"/>
+  <parent link="Upper_Arm_Link_type_b_v9_2"/>
+  <child link="Upper_Arm_Roll_Link_v5_2"/>
+  <axis xyz="-1.0 -0.0 -0.0"/>
+</joint>
+<transmission name="Rev8_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev8">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev8_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev9" type="continuous">
+  <origin rpy="0 0 0" xyz="0.05 0.0 -0.05635"/>
+  <parent link="Upper_Arm_Roll_Link_v5_2"/>
+  <child link="Upper_Arm_v10_3"/>
+  <axis xyz="0.0 0.0 -1.0"/>
+</joint>
+<transmission name="Rev9_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev9">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev9_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev10" type="continuous">
+  <origin rpy="0 0 0" xyz="-0.05 0.0 -0.3675"/>
+  <parent link="Upper_Arm_v10_3"/>
+  <child link="Lower_Arm_v8_2"/>
+  <axis xyz="1.0 0.0 0.0"/>
+</joint>
+<transmission name="Rev10_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev10">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev10_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev12" type="continuous">
+  <origin rpy="0 0 0" xyz="0.1 -0.1 0.815"/>
+  <parent link="base_link"/>
+  <child link="Leg_Pitch_v5_1"/>
+  <axis xyz="0.0 -1.0 -0.0"/>
+</joint>
+<transmission name="Rev12_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev12">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev12_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev13" type="continuous">
+  <origin rpy="0 0 0" xyz="-0.075 -0.05 0.0"/>
+  <parent link="Leg_Pitch_v5_1"/>
+  <child link="Leg_Roll_v4_1"/>
+  <axis xyz="-1.0 -0.0 0.0"/>
+</joint>
+<transmission name="Rev13_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev13">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev13_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev14" type="continuous">
+  <origin rpy="0 0 0" xyz="0.075 0.0 -0.075"/>
+  <parent link="Leg_Roll_v4_1"/>
+  <child link="Upper_Leg_v4_1"/>
+  <axis xyz="0.0 0.0 -1.0"/>
+</joint>
+<transmission name="Rev14_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev14">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev14_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev15" type="continuous">
+  <origin rpy="0 0 0" xyz="0.0 -0.05 -0.2982"/>
+  <parent link="Upper_Leg_v4_1"/>
+  <child link="Lower_Leg_v6_1"/>
+  <axis xyz="-0.0 1.0 0.0"/>
+</joint>
+<transmission name="Rev15_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev15">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev15_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev16" type="continuous">
+  <origin rpy="0 0 0" xyz="-0.03 0.05 -0.3532"/>
+  <parent link="Lower_Leg_v6_1"/>
+  <child link="Ankle_Link_v5_1"/>
+  <axis xyz="1.0 0.0 -0.0"/>
+</joint>
+<transmission name="Rev16_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev16">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev16_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev19" type="continuous">
+  <origin rpy="0 0 0" xyz="0.03 -0.02 -0.035"/>
+  <parent link="Ankle_Link_v5_1"/>
+  <child link="Foot_v4_1"/>
+  <axis xyz="0.0 -1.0 -0.0"/>
+</joint>
+<transmission name="Rev19_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev19">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev19_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev20" type="continuous">
+  <origin rpy="0 0 0" xyz="0.1 0.1 0.815"/>
+  <parent link="base_link"/>
+  <child link="Leg_Pitch_v5_2"/>
+  <axis xyz="-0.0 1.0 0.0"/>
+</joint>
+<transmission name="Rev20_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev20">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev20_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev21" type="continuous">
+  <origin rpy="0 0 0" xyz="-0.075 0.05 0.0"/>
+  <parent link="Leg_Pitch_v5_2"/>
+  <child link="Leg_Roll_v4_2"/>
+  <axis xyz="-1.0 -0.0 -0.0"/>
+</joint>
+<transmission name="Rev21_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev21">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev21_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev22" type="continuous">
+  <origin rpy="0 0 0" xyz="0.075 0.0 -0.075"/>
+  <parent link="Leg_Roll_v4_2"/>
+  <child link="Upper_Leg_v4_2"/>
+  <axis xyz="0.0 0.0 -1.0"/>
+</joint>
+<transmission name="Rev22_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev22">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev22_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev23" type="continuous">
+  <origin rpy="0 0 0" xyz="0.0 0.05 -0.2982"/>
+  <parent link="Upper_Leg_v4_2"/>
+  <child link="Lower_Leg_v6_2"/>
+  <axis xyz="0.0 -1.0 -0.0"/>
+</joint>
+<transmission name="Rev23_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev23">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev23_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev24" type="continuous">
+  <origin rpy="0 0 0" xyz="-0.03 -0.05 -0.3532"/>
+  <parent link="Lower_Leg_v6_2"/>
+  <child link="Ankle_Link_v5_2"/>
+  <axis xyz="1.0 0.0 0.0"/>
+</joint>
+<transmission name="Rev24_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev24">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev24_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+<joint name="Rev25" type="continuous">
+  <origin rpy="0 0 0" xyz="0.03 0.02 -0.035"/>
+  <parent link="Ankle_Link_v5_2"/>
+  <child link="Foot_v4_2"/>
+  <axis xyz="-0.0 1.0 0.0"/>
+</joint>
+<transmission name="Rev25_tran">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="Rev25">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="Rev25_actr">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <mechanicalReduction>1</mechanicalReduction>
+  </actuator>
+</transmission>
+
+</robot>

control_systems/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, )
+"""

control_systems/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

control_systems/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

control_systems/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)

control_systems/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

control_systems/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

control_systems/Control System/instructions.py

@@ -0,0 +1,4 @@
+# Use methods online to dehash data packages/ instructions
+# Add instruction to priority queue 
+
+def readInstruction

control_systems/Control System/main.py

@@ -0,0 +1,8 @@
+import sys
+from bootup import *
+ 
+def main():
+  startBoot() # when Olympian is booted up for first time  
+
+if __name__ == "__main__":
+  main()

control_systems/LICENSE

@@ -0,0 +1,661 @@
+                    GNU AFFERO GENERAL PUBLIC LICENSE
+                       Version 3, 19 November 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The GNU Affero General Public License is a free, copyleft license for
+software and other kinds of works, specifically designed to ensure
+cooperation with the community in the case of network server software.
+
+  The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works.  By contrast,
+our General Public Licenses are intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+  Developers that use our General Public Licenses protect your rights
+with two steps: (1) assert copyright on the software, and (2) offer
+you this License which gives you legal permission to copy, distribute
+and/or modify the software.
+
+  A secondary benefit of defending all users' freedom is that
+improvements made in alternate versions of the program, if they
+receive widespread use, become available for other developers to
+incorporate.  Many developers of free software are heartened and
+encouraged by the resulting cooperation.  However, in the case of
+software used on network servers, this result may fail to come about.
+The GNU General Public License permits making a modified version and
+letting the public access it on a server without ever releasing its
+source code to the public.
+
+  The GNU Affero General Public License is designed specifically to
+ensure that, in such cases, the modified source code becomes available
+to the community.  It requires the operator of a network server to
+provide the source code of the modified version running there to the
+users of that server.  Therefore, public use of a modified version, on
+a publicly accessible server, gives the public access to the source
+code of the modified version.
+
+  An older license, called the Affero General Public License and
+published by Affero, was designed to accomplish similar goals.  This is
+a different license, not a version of the Affero GPL, but Affero has
+released a new version of the Affero GPL which permits relicensing under
+this license.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                       TERMS AND CONDITIONS
+
+  0. Definitions.
+
+  "This License" refers to version 3 of the GNU Affero General Public License.
+
+  "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+  "The Program" refers to any copyrightable work licensed under this
+License.  Each licensee is addressed as "you".  "Licensees" and
+"recipients" may be individuals or organizations.
+
+  To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy.  The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+  A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+  To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy.  Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+  To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies.  Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+  An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License.  If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+  1. Source Code.
+
+  The "source code" for a work means the preferred form of the work
+for making modifications to it.  "Object code" means any non-source
+form of a work.
+
+  A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+  The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form.  A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+  The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities.  However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work.  For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+  The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+  The Corresponding Source for a work in source code form is that
+same work.
+
+  2. Basic Permissions.
+
+  All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met.  This License explicitly affirms your unlimited
+permission to run the unmodified Program.  The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work.  This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+  You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force.  You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright.  Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+  Conveying under any other circumstances is permitted solely under
+the conditions stated below.  Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+  3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+  No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+  When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+  4. Conveying Verbatim Copies.
+
+  You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+  You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+  5. Conveying Modified Source Versions.
+
+  You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+    a) The work must carry prominent notices stating that you modified
+    it, and giving a relevant date.
+
+    b) The work must carry prominent notices stating that it is
+    released under this License and any conditions added under section
+    7.  This requirement modifies the requirement in section 4 to
+    "keep intact all notices".
+
+    c) You must license the entire work, as a whole, under this
+    License to anyone who comes into possession of a copy.  This
+    License will therefore apply, along with any applicable section 7
+    additional terms, to the whole of the work, and all its parts,
+    regardless of how they are packaged.  This License gives no
+    permission to license the work in any other way, but it does not
+    invalidate such permission if you have separately received it.
+
+    d) If the work has interactive user interfaces, each must display
+    Appropriate Legal Notices; however, if the Program has interactive
+    interfaces that do not display Appropriate Legal Notices, your
+    work need not make them do so.
+
+  A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit.  Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+  6. Conveying Non-Source Forms.
+
+  You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+    a) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by the
+    Corresponding Source fixed on a durable physical medium
+    customarily used for software interchange.
+
+    b) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by a
+    written offer, valid for at least three years and valid for as
+    long as you offer spare parts or customer support for that product
+    model, to give anyone who possesses the object code either (1) a
+    copy of the Corresponding Source for all the software in the
+    product that is covered by this License, on a durable physical
+    medium customarily used for software interchange, for a price no
+    more than your reasonable cost of physically performing this
+    conveying of source, or (2) access to copy the
+    Corresponding Source from a network server at no charge.
+
+    c) Convey individual copies of the object code with a copy of the
+    written offer to provide the Corresponding Source.  This
+    alternative is allowed only occasionally and noncommercially, and
+    only if you received the object code with such an offer, in accord
+    with subsection 6b.
+
+    d) Convey the object code by offering access from a designated
+    place (gratis or for a charge), and offer equivalent access to the
+    Corresponding Source in the same way through the same place at no
+    further charge.  You need not require recipients to copy the
+    Corresponding Source along with the object code.  If the place to
+    copy the object code is a network server, the Corresponding Source
+    may be on a different server (operated by you or a third party)
+    that supports equivalent copying facilities, provided you maintain
+    clear directions next to the object code saying where to find the
+    Corresponding Source.  Regardless of what server hosts the
+    Corresponding Source, you remain obligated to ensure that it is
+    available for as long as needed to satisfy these requirements.
+
+    e) Convey the object code using peer-to-peer transmission, provided
+    you inform other peers where the object code and Corresponding
+    Source of the work are being offered to the general public at no
+    charge under subsection 6d.
+
+  A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+  A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling.  In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage.  For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product.  A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+  "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source.  The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+  If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information.  But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+  The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed.  Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+  Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+  7. Additional Terms.
+
+  "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law.  If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+  When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it.  (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.)  You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+  Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+    a) Disclaiming warranty or limiting liability differently from the
+    terms of sections 15 and 16 of this License; or
+
+    b) Requiring preservation of specified reasonable legal notices or
+    author attributions in that material or in the Appropriate Legal
+    Notices displayed by works containing it; or
+
+    c) Prohibiting misrepresentation of the origin of that material, or
+    requiring that modified versions of such material be marked in
+    reasonable ways as different from the original version; or
+
+    d) Limiting the use for publicity purposes of names of licensors or
+    authors of the material; or
+
+    e) Declining to grant rights under trademark law for use of some
+    trade names, trademarks, or service marks; or
+
+    f) Requiring indemnification of licensors and authors of that
+    material by anyone who conveys the material (or modified versions of
+    it) with contractual assumptions of liability to the recipient, for
+    any liability that these contractual assumptions directly impose on
+    those licensors and authors.
+
+  All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10.  If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term.  If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+  If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+  Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+  8. Termination.
+
+  You may not propagate or modify a covered work except as expressly
+provided under this License.  Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+  However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+  Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+  Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License.  If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+  9. Acceptance Not Required for Having Copies.
+
+  You are not required to accept this License in order to receive or
+run a copy of the Program.  Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance.  However,
+nothing other than this License grants you permission to propagate or
+modify any covered work.  These actions infringe copyright if you do
+not accept this License.  Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+  10. Automatic Licensing of Downstream Recipients.
+
+  Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License.  You are not responsible
+for enforcing compliance by third parties with this License.
+
+  An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations.  If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+  You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License.  For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+  11. Patents.
+
+  A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based.  The
+work thus licensed is called the contributor's "contributor version".
+
+  A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version.  For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+  Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+  In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement).  To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+  If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients.  "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+  If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+  A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License.  You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+  Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+  12. No Surrender of Others' Freedom.
+
+  If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all.  For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+  13. Remote Network Interaction; Use with the GNU General Public License.
+
+  Notwithstanding any other provision of this License, if you modify the
+Program, your modified version must prominently offer all users
+interacting with it remotely through a computer network (if your version
+supports such interaction) an opportunity to receive the Corresponding
+Source of your version by providing access to the Corresponding Source
+from a network server at no charge, through some standard or customary
+means of facilitating copying of software.  This Corresponding Source
+shall include the Corresponding Source for any work covered by version 3
+of the GNU General Public License that is incorporated pursuant to the
+following paragraph.
+
+  Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU General Public License into a single
+combined work, and to convey the resulting work.  The terms of this
+License will continue to apply to the part which is the covered work,
+but the work with which it is combined will remain governed by version
+3 of the GNU General Public License.
+
+  14. Revised Versions of this License.
+
+  The Free Software Foundation may publish revised and/or new versions of
+the GNU Affero General Public License from time to time.  Such new versions
+will be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+  Each version is given a distinguishing version number.  If the
+Program specifies that a certain numbered version of the GNU Affero General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation.  If the Program does not specify a version number of the
+GNU Affero General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+  If the Program specifies that a proxy can decide which future
+versions of the GNU Affero General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+  Later license versions may give you additional or different
+permissions.  However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+  15. Disclaimer of Warranty.
+
+  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. Limitation of Liability.
+
+  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+  17. Interpretation of Sections 15 and 16.
+
+  If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the program's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU Affero General Public License as published
+    by the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU Affero General Public License for more details.
+
+    You should have received a copy of the GNU Affero General Public License
+    along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+Also add information on how to contact you by electronic and paper mail.
+
+  If your software can interact with users remotely through a computer
+network, you should also make sure that it provides a way for users to
+get its source.  For example, if your program is a web application, its
+interface could display a "Source" link that leads users to an archive
+of the code.  There are many ways you could offer source, and different
+solutions will be better for different programs; see section 13 for the
+specific requirements.
+
+  You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU AGPL, see
+<https://www.gnu.org/licenses/>.

control_systems/README.md

@@ -0,0 +1,2 @@
+# Control System
+Control loops that monitor and manage the robot

control_systems/client1.py

@@ -0,0 +1,27 @@
+import socket
+import threading
+
+HOST = '10.235.1.145' # Enter IP or Hostname of your server
+PORT = 12345 # Pick an open Port (1000+ recommended), must match the server port
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+s.connect((HOST,PORT))
+
+#Lets loop awaiting for your input
+
+def telemetryReceive():
+	while True:
+		reply = s.recv(1024)
+		if reply != None:
+			print(reply)
+
+def telemetrySend():
+	while True:
+		command = input('Enter your command: ')
+		if command != None:
+			s.send(bytes(command, 'utf-8'))
+
+def main():
+	threading.Thread(target=telemetryReceive).start()
+	threading.Thread(target=telemetrySend).start()
+
+main()

control_systems/server1.py

@@ -0,0 +1,26 @@
+import socket
+
+HOST = '10.235.1.127' # Use this for client
+PORT = 12345 # Pick an open Port (1000+ recommended), must match the client sport
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+print('Socket created')
+
+#managing error exception
+try:
+	s.bind((HOST, PORT))
+except socket.error:
+    print ('Bind failed ')
+
+s.listen(5)
+print ('Socket awaiting messages')
+(conn, addr) = s.accept()
+print ('Connected')
+
+# awaiting for message
+while True:
+	data = conn.recv(1024)
+	print (data)
+
+	# # Sending reply
+	conn.send(data+bytes(" works",'utf-8'))
+	# conn.close() # Close connections