From f505a89be4d4630134c662f6673081556d3f9a60 Mon Sep 17 00:00:00 2001
From: eugenechoi2004 <35019459+eugenechoi2004@users.noreply.github.com>
Date: Sat, 20 Feb 2021 13:31:25 -0500
Subject: [PATCH] COM fix

---
 .DS_Store                                   | Bin 6148 -> 6148 bytes
 simulation/.DS_Store                        | Bin 0 -> 6148 bytes
 simulation/controllers/olympian/olympian.py |  50 ++++++++++++++++++++
 3 files changed, 50 insertions(+)
 create mode 100644 simulation/.DS_Store

diff --git a/.DS_Store b/.DS_Store
index 62b4cb79ee0761c814ff3adb7d22745c2e459904..85767a3ccb0269c599267872bffcd005dbb6cf90 100644
GIT binary patch
delta 64
zcmV-G0Kfl)FoZCW7XgQnaTbv-ApruBP&<<_6a<lv6$oT=V{Bq?Z)~xPTL-fT2oMMX
Whq1Bu1CtLFEdc<tK@`ISk$?n~GZZ`k

delta 52
zcmV-40L%Y`FoZCW7XgNmaTbv-AprrAP&=`#TL+QgA+rYv5C{Q=vC;PflMfUv1pxp6
K0Kl_76vG3-#t=>b

diff --git a/simulation/.DS_Store b/simulation/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..d858b898559f19419eee37ac2ec978b583de300a
GIT binary patch
literal 6148
zcmeHK%TB{U3>?!6RqCZjj{6Jz!Kx}>zz?7{+#({SAg;Ob+l=ky5v9FyK;z0gS+B>d
z%ptA;*#51%2X+89bVq#nvNS(;U)W8HI9k*gF`(z~8RH`Ra=^J;oXC5@D_KAMb9X-V
z$6@GM8n<}o(a6#rW(Sw6Dk=q}fE17dQa}n^Q@|T5ZL>|(Dg~r~6!=!azYm4(SQ}1>
z@#)|aBLH#Da2TIsmLL`n5NpFJkrA3Dm6%j(BZeiN@zU~Y!znT8u(+9Z+RfG`6pPy#
zFOd$biCU$A6qqZp%I(hk{{#KX{68mYCk3Ryzf!=~hsVR7uhe?$;^n;87WxzY%a|MK
nT%r{dqZM=Gt@!R&Uh!w1*M?JK&>0UpQGWzn7nv0J3k5y_u7ewD

literal 0
HcmV?d00001

diff --git a/simulation/controllers/olympian/olympian.py b/simulation/controllers/olympian/olympian.py
index 59c2f0b..5069787 100644
--- a/simulation/controllers/olympian/olympian.py
+++ b/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")