diff --git a/simulation/controllers/olympian/olympian.py b/simulation/controllers/olympian/olympian.py index cbcf1f7..3294c2d 100644 --- a/simulation/controllers/olympian/olympian.py +++ b/simulation/controllers/olympian/olympian.py @@ -11,11 +11,11 @@ def main(): robot = Robot() head_motor = robot.getDevice("torso_yaw") - motor2 = robot.getDevice("right_elbow_pitch") - motor3 = robot.getDevice("shoulder_yaw") - head_motor.setVelocity(0.3) + motor2 = robot.getDevice("right_hip_pitch") + motor3 = robot.getDevice("right_knee_pitch") + head_motor.setVelocity(1) motor2.setVelocity(9) - motor3.setVelocity(1) + motor3.setVelocity(9) pos = 0 add = False @@ -27,16 +27,17 @@ def main(): while robot.step(timestep) != -1: head_motor.setPosition(pos) # motor2.setPosition(pos) - # motor3.setPosition(pos) + # motor2.setPosition(pos) + # motor3.setPosition(-pos) if add: pos += 0.2 else: pos -= 0.2 - if pos >= 6: + if pos >= 1.4: add = False - elif pos <= -6: + elif pos <= -1.4: add = True # Enter here exit cleanup code. diff --git a/simulation/worlds/.olympian.wbproj b/simulation/worlds/.olympian.wbproj index c65847b..ddb7ba0 100644 --- a/simulation/worlds/.olympian.wbproj +++ b/simulation/worlds/.olympian.wbproj @@ -5,5 +5,5 @@ sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa01000000020100000 maximizedDockId: -1 centralWidgetVisible: 1 orthographicViewHeight: 1 -textFiles: 0 "controllers/olympian/standingBalance.py" "controllers/olympian/olympian.py" +textFiles: 1 "controllers/olympian/standingBalance.py" "controllers/olympian/olympian.py" consoles: Console:All:All diff --git a/simulation/worlds/olympian.wbt b/simulation/worlds/olympian.wbt index a3d657e..5830e7f 100644 --- a/simulation/worlds/olympian.wbt +++ b/simulation/worlds/olympian.wbt @@ -104,20 +104,20 @@ Robot { } device [ RotationalMotor { - name "left_foot_pitch" + name "left_ankle_pitch" maxTorque 100 } ] device2 [ RotationalMotor { - name "left_foot_yaw" + name "left_ankle_yaw" minPosition -1.5707963267948966 maxPosition 1.5707963267948966 } ] device3 [ RotationalMotor { - name "left_foot_roll" + name "left_ankle_roll" maxTorque 100 } ] @@ -136,18 +136,45 @@ Robot { ] boundingObject USE left_foot physics Physics { + density -1 + mass 4.000000000000001 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.054166666666666696 0.0666666666666667 0.014166666666666675 + 0 0 0 + ] } } } ] boundingObject USE left_shin physics Physics { + density -1 + mass 7.068583470577034 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.10586308213356384 0.018886371460448012 0.10586308213356384 + 0 0 0 + ] } } } ] boundingObject USE upperLeftLeg physics Physics { + density -1 + mass 7.068583470577034 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.10586308213356384 0.018886371460448012 0.10586308213356384 + 0 0 0 + ] } } } @@ -225,20 +252,20 @@ Robot { } device [ RotationalMotor { - name "right_foot_pitch" + name "right_ankle_pitch" maxTorque 100 } ] device2 [ RotationalMotor { - name "right_foot_yaw" + name "right_ankle_yaw" minPosition -1.5707963267948966 maxPosition 1.5707963267948966 } ] device3 [ RotationalMotor { - name "right_foot_roll" + name "right_ankle_roll" maxTorque 100 } ] @@ -257,6 +284,15 @@ Robot { ] boundingObject USE right_foot physics Physics { + density -1 + mass 4.000000000000001 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.054166666666666696 0.0666666666666667 0.014166666666666675 + 0 0 0 + ] } } } @@ -279,6 +315,15 @@ Robot { name "upperRightLegMotor" boundingObject USE upperRightLeg physics Physics { + density -1 + mass 7.068583470577034 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.10586308213356384 0.018886371460448012 0.10586308213356384 + 0 0 0 + ] } } } @@ -332,19 +377,19 @@ Robot { } device [ RotationalMotor { - name "shoulder_roll" + name "right_shoulder_roll" } ] device2 [ RotationalMotor { - name "shoulder_yaw" + name "right_shoulder_yaw" minPosition -1.5707963267948966 maxPosition 1.5707963267948966 } ] device3 [ RotationalMotor { - name "shoulder_pitch" + name "right_shoulder_pitch" } ] endPoint Solid { @@ -356,14 +401,14 @@ Robot { } device [ RotationalMotor { - name "elbow_pitch" + name "right_elbow_pitch" } ] endPoint Solid { translation 0 -0.42499999999726895 -1.5235950555825944e-06 - rotation 1 0 0 3.5849604925388305e-06 + rotation 0.9999999999999999 0 0 3.5849604925388305e-06 children [ - DEF elbow Shape { + DEF right_elbow Shape { appearance PBRAppearance { roughness 1 metalness 0 @@ -374,12 +419,21 @@ Robot { } } ] - boundingObject USE elbow + boundingObject USE right_elbow physics Physics { + density -1 + mass 6.185010536754905 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.07323531077523626 0.016401322584073275 0.07323531077523626 + 0 0 0 + ] } } } - DEF shoulder Shape { + DEF right_shoulder Shape { appearance PBRAppearance { roughness 1 metalness 0 @@ -390,8 +444,17 @@ Robot { } } ] - boundingObject USE shoulder + boundingObject USE right_shoulder physics Physics { + density -1 + mass 7.068583470577034 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.10586308213356384 0.018886371460448012 0.10586308213356384 + 0 0 0 + ] } } } @@ -438,6 +501,15 @@ Robot { name "solid(2)" boundingObject USE head physics Physics { + density -1 + mass 8.18123086872342 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.05113269292952138 0.05113269292952138 0.05113269292952138 + 0 0 0 + ] } } } @@ -452,19 +524,19 @@ Robot { } device [ RotationalMotor { - name "right_shoulder_roll" + name "left_shoulder_roll" } ] device2 [ RotationalMotor { - name "right_shoulder_yaw" + name "left_shoulder_yaw" minPosition -1.5707963267948966 maxPosition 1.5707963267948966 } ] device3 [ RotationalMotor { - name "right_shoulder_pitch" + name "left_shoulder_pitch" } ] endPoint Solid { @@ -475,14 +547,14 @@ Robot { } device [ RotationalMotor { - name "right_elbow_pitch" + name "left_elbow_pitch" } ] endPoint Solid { translation 0 -0.42499999999726895 -1.5235950555825944e-06 - rotation 1 0 0 3.5849604925388305e-06 + rotation 0.9999999999999999 0 0 3.5849604925388305e-06 children [ - DEF right_elbow Shape { + DEF left_elbow Shape { appearance PBRAppearance { roughness 1 metalness 0 @@ -493,12 +565,21 @@ Robot { } } ] - boundingObject USE right_elbow + boundingObject USE left_elbow physics Physics { + density -1 + mass 6.185010536754905 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.07323531077523626 0.016401322584073275 0.07323531077523626 + 0 0 0 + ] } } } - DEF right_shoulder Shape { + DEF left_shoulder Shape { appearance PBRAppearance { roughness 1 metalness 0 @@ -510,8 +591,17 @@ Robot { } ] name "solid(1)" - boundingObject USE right_shoulder + boundingObject USE left_shoulder physics Physics { + density -1 + mass 7.068583470577034 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.10586308213356384 0.018886371460448012 0.10586308213356384 + 0 0 0 + ] } } } @@ -519,12 +609,30 @@ Robot { name "solid(2)" boundingObject USE torso physics Physics { + density -1 + mass 87.5 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 4.028645833333333 2.2786458333333335 5.395833333333334 + 0 0 0 + ] } } } ] boundingObject USE Body physics Physics { + density -1 + mass 10.000000000000002 + centerOfMass [ + 0 1.05 0 + ] + inertiaMatrix [ + 11.11041666666667 0.08541666666666668 11.091666666666669 + 0 0 0 + ] } controller "olympian" }