From e37e3d92c1b9963ffe31c26942c6fe08b43fdf1c Mon Sep 17 00:00:00 2001 From: Aditya Vasantharao Date: Fri, 19 Feb 2021 23:54:24 -0500 Subject: [PATCH] Fix all motor anchors --- simulation/controllers/olympian/olympian.py | 86 +++++++++++++++------ simulation/worlds/.olympian.wbproj | 4 +- simulation/worlds/olympian.wbt | 32 ++++---- 3 files changed, 84 insertions(+), 38 deletions(-) diff --git a/simulation/controllers/olympian/olympian.py b/simulation/controllers/olympian/olympian.py index 3294c2d..59c2f0b 100644 --- a/simulation/controllers/olympian/olympian.py +++ b/simulation/controllers/olympian/olympian.py @@ -3,42 +3,84 @@ # You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor from controller import Robot - +import time def main(): # create the Robot instance. print("Initializing world...") robot = Robot() - - head_motor = robot.getDevice("torso_yaw") - motor2 = robot.getDevice("right_hip_pitch") - motor3 = robot.getDevice("right_knee_pitch") - head_motor.setVelocity(1) - motor2.setVelocity(9) - motor3.setVelocity(9) - pos = 0 - add = False + timestep = 64 + start = 0 + while robot.step(32) != -1: + start += 32/1000.0 + head_motor = robot.getDevice("torso_yaw") + # motor2 = robot.getDevice("neck_roll") + lknee = robot.getDevice("left_knee_pitch") + rknee = robot.getDevice("right_knee_pitch") + lhip = robot.getDevice("left_hip_pitch") + rhip = robot.getDevice("right_hip_pitch") + lfoot = robot.getDevice("left_ankle_pitch") + rfoot = robot.getDevice("right_ankle_pitch") + + torso_pitch = robot.getDevice("torso_pitch") + + head_motor.setVelocity(1) + lknee.setVelocity(2) + rknee.setVelocity(2) + lknee.setPosition(2) + rknee.setPosition(2) + torso_pitch.setVelocity(10) + # torso_pitch.setTorque(10) + + lhip.setVelocity(2) + rhip.setVelocity(2) + lhip.setPosition(-0.7) + rhip.setPosition(-0.7) + + lfoot.setVelocity(2) + rfoot.setVelocity(2) + lfoot.setPosition(0.3) + rfoot.setPosition(0.3) + + pos = 0 + add = False + + if start > 6: + while robot.step(32) != -1: + torso_pitch.setPosition(pos) + # time.sleep(0.1) + + if add: + pos += 0.2 + else: + pos -= 0.2 + + if pos >= 2: + add = False + elif pos <= 0: + add = True # get the time step of the current world. - timestep = int(robot.getBasicTimeStep()) - + # timestep = int(robot.getBasicTimeStep()) + # motor2.setPosition(3) # Main loop: # - perform simulation steps until Webots is stopping the controller - while robot.step(timestep) != -1: - head_motor.setPosition(pos) + # while robot.step(timestep) != -1: + # head_motor.setPosition(pos) # motor2.setPosition(pos) # motor2.setPosition(pos) # motor3.setPosition(-pos) + # time.sleep(0.1) - if add: - pos += 0.2 - else: - pos -= 0.2 + # if add: + # pos += 0.2 + # else: + # pos -= 0.2 - if pos >= 1.4: - add = False - elif pos <= -1.4: - add = True + # if pos >= 3: + # add = False + # elif pos <= 3: + # add = True # Enter here exit cleanup code. diff --git a/simulation/worlds/.olympian.wbproj b/simulation/worlds/.olympian.wbproj index 68612c6..36e2b80 100644 --- a/simulation/worlds/.olympian.wbproj +++ b/simulation/worlds/.olympian.wbproj @@ -1,6 +1,6 @@ Webots Project File version R2021a -perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000023a000003ccfc0200000002fb0000001400540065007800740045006400690074006f00720100000016000002f90000008900fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000311000000d10000003900ffffff0000000300000780000001e8fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007400000000000000000000004fc000003cc00000004000000040000000100000008fc00000000 -simulationViewPerspectives: 000000ff00000001000000020000016c000003d60100000002010000000101 +perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000023a000003b6fc0200000002fb0000001400540065007800740045006400690074006f0072010000001a000002e30000008700fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000002ff000000d10000003900ffffff0000000300000780000001e8fc0100000001fb0000000e0043006f006e0073006f006c0065010000000000000740000000000000000000000544000003b600000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff0000000100000002000001dd000003650100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000201 maximizedDockId: -1 centralWidgetVisible: 1 diff --git a/simulation/worlds/olympian.wbt b/simulation/worlds/olympian.wbt index aa2ab77..f28aa14 100644 --- a/simulation/worlds/olympian.wbt +++ b/simulation/worlds/olympian.wbt @@ -1,10 +1,11 @@ #VRML_SIM R2021a utf8 WorldInfo { + gravity 0.2 coordinateSystem "NUE" } Viewpoint { - orientation -0.7108788721149584 -0.6798276004460428 -0.18023779529371817 0.7628366439986357 - position -1.529898492340523 2.6913118496032213 2.7543281313960564 + orientation -0.048379992001633905 -0.9988161558244542 -0.005065889653491801 0.7456617071754322 + position -3.612561450129352 1.0571051522533501 1.7706349845759928 } TexturedBackground { } @@ -14,7 +15,8 @@ RectangleArena { floorSize 5 5 } Robot { - translation 0 -0.12 0 + translation -1 2 0 + rotation -1 0 0 1.57 children [ DEF Body Transform { translation 0 1.05 0 @@ -32,7 +34,7 @@ Robot { } BallJoint { jointParameters BallJointParameters { - anchor 0.185 0.85 0 + anchor 0.185 1.15 0 } jointParameters2 JointParameters { axis 1 0 0 @@ -72,7 +74,7 @@ Robot { } HingeJoint { jointParameters HingeJointParameters { - anchor 0 -0.45 0 + anchor 0 -0.225 0 } device [ RotationalMotor { @@ -104,20 +106,20 @@ Robot { } device [ RotationalMotor { - name "left_ankle_pitch" + name "left_ankle_yaw" maxTorque 100 } ] device2 [ RotationalMotor { - name "left_ankle_yaw" + name "left_ankle_roll" minPosition -1.5707963267948966 maxPosition 1.5707963267948966 } ] device3 [ RotationalMotor { - name "left_ankle_roll" + name "left_ankle_pitch" maxTorque 100 } ] @@ -180,7 +182,7 @@ Robot { } BallJoint { jointParameters BallJointParameters { - anchor -0.185 0.85 0 + anchor -0.185 1.15 0 } jointParameters2 JointParameters { axis 1 0 0 @@ -220,7 +222,7 @@ Robot { } HingeJoint { jointParameters HingeJointParameters { - anchor 0 -0.45 0 + anchor 0 -0.225 0 } device [ RotationalMotor { @@ -243,7 +245,7 @@ Robot { } BallJoint { jointParameters BallJointParameters { - anchor 0 -0.25 0 + anchor 0 -0.225 0 } jointParameters2 JointParameters { } @@ -252,20 +254,20 @@ Robot { } device [ RotationalMotor { - name "right_ankle_pitch" + name "right_ankle_yaw" maxTorque 100 } ] device2 [ RotationalMotor { - name "right_ankle_yaw" + name "right_ankle_roll" minPosition -1.5707963267948966 maxPosition 1.5707963267948966 } ] device3 [ RotationalMotor { - name "right_ankle_roll" + name "right_ankle_pitch" maxTorque 100 } ] @@ -398,6 +400,7 @@ Robot { children [ HingeJoint { jointParameters HingeJointParameters { + anchor 0 -0.2125 0 } device [ RotationalMotor { @@ -544,6 +547,7 @@ Robot { children [ HingeJoint { jointParameters HingeJointParameters { + anchor 0 -0.2125 0 } device [ RotationalMotor {