COM fix

simulation/controllers/olympian/olympian.py

@@ -4,6 +4,55 @@
 #  from controller import Robot, Motor, DistanceSensor
 from controller import Robot
 import time
+import numpy as np
+
+def calculateCOM(robot):
+  # links needed to calculate COM
+  torso = robot.getDevice("torsoMotor")
+  upperLeftLeg= robot.getDevice("upperLeftLegMotor")
+  upperRightLeg = robot.getDevice("upperRightLegMotor")
+
+  links = [torso, upperLeftLeg, upperRightLeg] # additional links will be added, when they're made
+
+  totalMass = len(links) # assumption that all links have mass of 1
+  centerOfMass = [0, 0, 0, 0]
+
+  for i in links: 
+    # instantiation of values
+    # need to add code to get joint angles
+    if i == torso:
+        r = 0.25
+        theta0 = 0
+        theta1 = 0
+    else:
+        r = 0.225
+        theta0 = 0
+        theta1 = 0
+    
+    # instantiation of matrices 
+    # assumption that all angle values are in degrees
+    transformationMatrixToLink = [[np.cos(theta0*np.pi/180), -np.sin(theta0*np.pi/180), 
+                                 0, 2*r*np.cos(theta0*np.pi/180)], [np.sin(theta0*np.pi/180), 
+                                 np.cos(theta0*np.pi/180), 0, 2*r*np.sin(theta0*np.pi/180)], 
+                                 [0, 0, 1, 0], [0, 0, 0, 1]]                       
+    vectorOfJointAngles = [2*r*np.cos(theta1*np.pi/180), 2*r*np.sin(theta1*np.pi/180), 0, 1]
+    transformationMatrixToLinkCOM = [[np.cos(theta1*np.pi/180), 0, -np.sin(theta1*np.pi/180), 
+                                    2*r*np.cos(theta0*np.pi/180)], [0, 1, 0, 0], 
+                                    [np.sin(theta1*np.pi/180), 0, np.cos(theta1*np.pi/180), 
+                                    2*r*np.sin(theta0*np.pi/180)], [0, 0, 0, 1]]
+    
+    # COM of each link is calculated forward kinematics equations
+    calc = np.dot(transformationMatrixToLink,vectorOfJointAngles)
+    print(calc)
+    calc=np.dot(calc,transformationMatrixToLinkCOM) 
+    print(calc)                      
+    #calc = transformationMatrixToLink.dot(vectorOfJointAngles).dot(transformationMatrixToLinkCOM)
+    
+    # COM of the entire rigid body is calculated using a weighted average
+    centerOfMass += (1/totalMass)*calc
+    
+  return centerOfMass[0], centerOfMass[1], centerOfMass[2]
+
 
 def main():
     # create the Robot instance.
@@ -12,6 +61,7 @@ def main():
     timestep = 64
     start = 0
     while robot.step(32) != -1:
+        print(calculateCOM(robot))
         start += 32/1000.0
         head_motor = robot.getDevice("torso_yaw")
         # motor2 = robot.getDevice("neck_roll")