From 667395bfa2989457764059da3e4135b59dadceea Mon Sep 17 00:00:00 2001
From: ramyareddy04 <75100092+ramyareddy04@users.noreply.github.com>
Date: Fri, 12 Feb 2021 20:29:46 -0500
Subject: [PATCH] calculateCOM code

---
 .../controllers/olympian/standingBalance.py   | 43 ++++++++++++++++++-
 1 file changed, 42 insertions(+), 1 deletion(-)

diff --git a/simulation/controllers/olympian/standingBalance.py b/simulation/controllers/olympian/standingBalance.py
index 488a09e..661dc57 100644
--- a/simulation/controllers/olympian/standingBalance.py
+++ b/simulation/controllers/olympian/standingBalance.py
@@ -83,7 +83,48 @@ def calculateZMP(gyro, accel):
 
 # Might have to actually code this later
 def calculateCOM(robot):
-  return
+  # 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, 2r*np.cos(theta0*np.pi/180)], [np.sin(theta0*np.pi/180), 
+                                 np.cos(theta0*np.pi/180), 0, 2r*np.sin(theta0*np.pi/180)], 
+                                 [0, 0, 1, 0], [0, 0, 0, 1]]                       
+    vectorOfJointAngles = [2r*np.cos(theta1*np.pi/180), 2r*np.sin(theta1*np.pi/180), 0, 1]
+    transformationMatrixToLinkCOM = [[np.cos(theta1*np.pi/180), 0, -np.sin(theta1*np.pi/180), 
+                                    2r*np.cos(theta0*np.pi/180)], [0, 1, 0, 0], 
+                                    [np.sin(theta1*np.pi/180), 0, np.cos(theta1*np.pi/180), 
+                                    2r*np.sin(theta0*np.pi/180)], [0, 0, 0, 1]]
+    
+    # COM of each link is calculated forward kinematics equations                                
+    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():
   print("Initializing Olympiad...")