From 0409e5a3b7137c072bf0df84a0aeaef7cbe89ed3 Mon Sep 17 00:00:00 2001
From: Aditya Vasantharao <hsfan612@gmail.com>
Date: Fri, 29 Jan 2021 23:07:56 -0500
Subject: [PATCH] Add shoulder joint

---
 simulation/controllers/olympian/olympian.py |   9 +-
 simulation/worlds/olympian.wbt              | 100 +++++++++-----------
 2 files changed, 53 insertions(+), 56 deletions(-)

diff --git a/simulation/controllers/olympian/olympian.py b/simulation/controllers/olympian/olympian.py
index d579820..e3fc2ec 100644
--- a/simulation/controllers/olympian/olympian.py
+++ b/simulation/controllers/olympian/olympian.py
@@ -10,8 +10,9 @@ def main():
     print("Initializing world...")
     robot = Robot()
     
-    head_motor = robot.getDevice("neck_pitch")
-    head_motor.setVelocity(1)
+    head_motor = robot.getDevice("shoulder_pitch")
+    head_motor.setVelocity(0.5)
+    pos = 0
     
     # get the time step of the current world.
     timestep = int(robot.getBasicTimeStep())
@@ -19,7 +20,9 @@ def main():
     # Main loop:
     # - perform simulation steps until Webots is stopping the controller
     while robot.step(timestep) != -1:
-        head_motor.setPosition(6)
+        head_motor.setPosition(pos)
+        
+        pos += 3.141
     
     # Enter here exit cleanup code.
     
diff --git a/simulation/worlds/olympian.wbt b/simulation/worlds/olympian.wbt
index 5e80d59..a4d9b60 100644
--- a/simulation/worlds/olympian.wbt
+++ b/simulation/worlds/olympian.wbt
@@ -3,8 +3,8 @@ WorldInfo {
   coordinateSystem "NUE"
 }
 Viewpoint {
-  orientation -0.7241113403564003 0.6708542148873753 0.16005433182235554 0.3286771770738909
-  position 0.9187932527597619 1.807787251982618 3.3212186235849788
+  orientation -0.9447688433364667 0.30168245796522575 0.12806063882682472 0.3152849474881078
+  position 0.2197976877928071 1.8786536179181716 2.8466989121991344
 }
 TexturedBackground {
 }
@@ -30,6 +30,50 @@ Robot {
         }
       ]
     }
+    BallJoint {
+      jointParameters BallJointParameters {
+        anchor -0.325 1.55 0
+      }
+      jointParameters2 JointParameters {
+      }
+      jointParameters3 JointParameters {
+        axis 1 0 0
+      }
+      device [
+        RotationalMotor {
+          name "shoulder_pitch"
+        }
+      ]
+      device2 [
+        RotationalMotor {
+          name "shoulder_roll"
+          minPosition -1.5707963267948966
+          maxPosition 1.5707963267948966
+        }
+      ]
+      device3 [
+        RotationalMotor {
+          name "shoulder_yaw"
+        }
+      ]
+      endPoint Solid {
+        translation -0.325 1.55 0
+        children [
+          DEF shoulder Shape {
+            appearance PBRAppearance {
+              roughness 1
+              metalness 0
+            }
+            geometry Sphere {
+              radius 0.075
+            }
+          }
+        ]
+        boundingObject USE shoulder
+        physics Physics {
+        }
+      }
+    }
     BallJoint {
       jointParameters BallJointParameters {
         anchor 0 1.775 0
@@ -58,6 +102,7 @@ Robot {
       ]
       endPoint Solid {
         translation 0 1.775 0
+        rotation 0.9999999999999999 3.671812525079458e-10 1.9040221549379054e-10 5.326429205090855
         children [
           DEF head Shape {
             appearance PBRAppearance {
@@ -75,57 +120,6 @@ Robot {
         }
       }
     }
-    HingeJoint {
-      jointParameters HingeJointParameters {
-        anchor -0.25 1.55 0
-      }
-      device [
-        RotationalMotor {
-          name "arm_pitch"
-        }
-      ]
-      endPoint Solid {
-        translation -0.25 1.55 0
-        rotation 0 0 1 1.57
-        children [
-          DEF arm_joint Shape {
-            appearance PBRAppearance {
-              roughness 1
-              metalness 0
-            }
-            geometry Cylinder {
-              height 0.1
-              radius 0.075
-            }
-          }
-        ]
-        boundingObject USE arm_joint
-        physics Physics {
-        }
-      }
-    }
-    HingeJoint {
-      jointParameters HingeJointParameters {
-        axis 0 0 1
-        anchor -0.375 1.55 0
-      }
-      device [
-        RotationalMotor {
-          name "arm_yaw"
-        }
-      ]
-      endPoint Solid {
-        translation -0.375 1.55 0
-        rotation 1 0 0 1.57
-        children [
-          USE arm_joint
-        ]
-        name "solid(1)"
-        boundingObject USE arm_joint
-        physics Physics {
-        }
-      }
-    }
   ]
   boundingObject USE Body
   physics Physics {