diff --git a/simulation/controllers/olympian/olympian.py b/simulation/controllers/olympian/olympian.py index 5069787..9740d42 100644 --- a/simulation/controllers/olympian/olympian.py +++ b/simulation/controllers/olympian/olympian.py @@ -2,7 +2,7 @@ # You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor -from controller import Robot +from controller import Robot, TouchSensor import time import numpy as np @@ -43,9 +43,9 @@ def calculateCOM(robot): # COM of each link is calculated forward kinematics equations calc = np.dot(transformationMatrixToLink,vectorOfJointAngles) - print(calc) + # print(calc) calc=np.dot(calc,transformationMatrixToLinkCOM) - print(calc) + # print(calc) #calc = transformationMatrixToLink.dot(vectorOfJointAngles).dot(transformationMatrixToLinkCOM) # COM of the entire rigid body is calculated using a weighted average @@ -58,57 +58,81 @@ def main(): # create the Robot instance. print("Initializing world...") robot = Robot() - 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") + timestep = 16 + curr_time = 0 + + lfootsensor = robot.getDevice("torso_gyro") + + + while robot.step(timestep) != -1: + # print(calculateCOM(robot)) + curr_time += timestep/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") + lfootsensor.enable(10) - 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) + # print sensor data every second - 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 curr_time % 1 < 0.01: + + print(lfootsensor.getValues()) + + # lfoot = robot.getDevice("left_ankle_pitch") + # rfoot = robot.getDevice("right_ankle_pitch") + +def func(): + + torso_pitch = robot.getDevice("torso_pitch") + + # head_motor.setVelocity(1) + lknee.setVelocity(2) + # rknee.setVelocity(2) + lknee.setPosition(1) + # rknee.setPosition(2) + torso_pitch.setVelocity(2) + # torso_pitch.setTorque(10) + + lhip.setVelocity(2) + rhip.setVelocity(2) + lhip.setPosition(-1) + torso_pitch.setPosition(0.2) + # rhip.setPosition(-0.2) + # rknee.setPosition(0.2) + + # rhip.setPosition(-0.7) + + # lfoot.setVelocity(2) + # rfoot.setVelocity(2) + # lfoot.setPosition(0.3) + # rfoot.setPosition(0.3) + + if curr_time > 1: + lknee.setPosition(0.1) + lhip.setPosition(0.5) + torso_pitch.setPosition(0.7) + + # pos = 0 + # add = False + + # if curr_time > 6: + # while robot.step(32) != -1: + # torso_pitch.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 >= 2: - add = False - elif pos <= 0: - add = True + # if pos >= 2: + # add = False + # elif pos <= 0: + # add = True # get the time step of the current world. # timestep = int(robot.getBasicTimeStep()) @@ -134,6 +158,4 @@ def main(): # Enter here exit cleanup code. - -if __name__ == '__main__': - main() \ No newline at end of file +main() \ No newline at end of file diff --git a/simulation/worlds/olympian.wbt b/simulation/worlds/olympian.wbt index 66e4a6c..5c156eb 100644 --- a/simulation/worlds/olympian.wbt +++ b/simulation/worlds/olympian.wbt @@ -1,11 +1,12 @@ #VRML_SIM R2021a utf8 WorldInfo { gravity 0.2 + basicTimeStep 16 coordinateSystem "NUE" } Viewpoint { - orientation -0.7752447244164626 -0.5132245726366987 -0.36823383237600654 1.5186048037836888 - position -2.175946533475156 4.937428530305314 0.06846856025694995 + orientation -0.6002466325295073 0.7753295528057134 0.19638753698511255 0.7477472527006542 + position 0.844819390262747 2.391065676253801 3.2173738573028166 } TexturedBackground { } @@ -15,9 +16,16 @@ RectangleArena { floorSize 5 5 } Robot { - translation -1 2 0 - rotation -1 0 0 1.57 + translation -1 0 0 children [ + Gyro { + translation 0 1.05 0 + name "pelvis_gyro" + } + Accelerometer { + translation 0 1.05 0 + name "pelvis_accelerometer" + } DEF Body Transform { translation 0 1.05 0 children [ @@ -42,11 +50,17 @@ Robot { jointParameters3 JointParameters { } device [ + PositionSensor { + name "left_hip_pitch_position_sensor" + } RotationalMotor { name "left_hip_pitch" } ] device2 [ + PositionSensor { + name "left_hip_yaw_position_sensor" + } RotationalMotor { name "left_hip_yaw" minPosition -1.5707963267948966 @@ -54,6 +68,9 @@ Robot { } ] device3 [ + PositionSensor { + name "left_hip_roll_position_sensor" + } RotationalMotor { name "left_hip_roll" } @@ -77,6 +94,9 @@ Robot { anchor 0 -0.225 0 } device [ + PositionSensor { + name "left_knee_pitch_position_sensor" + } RotationalMotor { name "left_knee_pitch" } @@ -105,12 +125,18 @@ Robot { axis 1 0 0 } device [ + PositionSensor { + name "left_ankle_yaw_position_sensor" + } RotationalMotor { name "left_ankle_yaw" maxTorque 100 } ] device2 [ + PositionSensor { + name "left_ankle_roll_position_sensor" + } RotationalMotor { name "left_ankle_roll" minPosition -1.5707963267948966 @@ -118,6 +144,9 @@ Robot { } ] device3 [ + PositionSensor { + name "left_ankle_pitch_position_sensor" + } RotationalMotor { name "left_ankle_pitch" maxTorque 100 @@ -126,6 +155,51 @@ Robot { endPoint Solid { translation 0 -0.25 0 children [ + TouchSensor { + children [ + Solid { + translation 0 -0.05 0 + children [ + DEF left_foot_force_sensor_shape Shape { + appearance PBRAppearance { + baseColor 0 0 0 + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.05 0.4 + } + } + ] + boundingObject USE left_foot_force_sensor_shape + physics Physics { + density -1 + mass 4.000000000000001 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.054166666666666696 0.0666666666666667 0.014166666666666675 + 0 0 0 + ] + } + } + ] + name "left_foot_force_sensor" + boundingObject USE left_foot_force_sensor_shape + physics Physics { + density -1 + mass 4.000000000000001 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.054166666666666696 0.0666666666666667 0.014166666666666675 + 0 0 0 + ] + } + type "force" + } DEF left_foot Shape { appearance PBRAppearance { roughness 1 @@ -190,11 +264,17 @@ Robot { jointParameters3 JointParameters { } device [ + PositionSensor { + name "right_hip_pitch_position_sensor" + } RotationalMotor { name "right_hip_pitch" } ] device2 [ + PositionSensor { + name "right_hip_yaw_position_sensor" + } RotationalMotor { name "right_hip_yaw" minPosition -1.5707963267948966 @@ -202,6 +282,9 @@ Robot { } ] device3 [ + PositionSensor { + name "right_hip_roll_position_sensor" + } RotationalMotor { name "right_hip_roll" } @@ -225,6 +308,9 @@ Robot { anchor 0 -0.225 0 } device [ + PositionSensor { + name "right_knee_pitch_position_sensor" + } RotationalMotor { name "right_knee_pitch" } @@ -253,12 +339,18 @@ Robot { axis 1 0 0 } device [ + PositionSensor { + name "right_ankle_yaw_position_sensor" + } RotationalMotor { name "right_ankle_yaw" maxTorque 100 } ] device2 [ + PositionSensor { + name "right_ankle_roll_position_sensor" + } RotationalMotor { name "right_ankle_roll" minPosition -1.5707963267948966 @@ -266,6 +358,9 @@ Robot { } ] device3 [ + PositionSensor { + name "right_ankle_pitch_position_sensor" + } RotationalMotor { name "right_ankle_pitch" maxTorque 100 @@ -274,6 +369,51 @@ Robot { endPoint Solid { translation 0 -0.25 0 children [ + TouchSensor { + translation 0 -0.05 0 + children [ + Solid { + children [ + DEF right_foot_force_sensor_shape Shape { + appearance PBRAppearance { + baseColor 0 0 0 + roughness 1 + metalness 0 + } + geometry Box { + size 0.2 0.05 0.4 + } + } + ] + boundingObject USE right_foot_force_sensor_shape + physics Physics { + density -1 + mass 4.000000000000001 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.054166666666666696 0.0666666666666667 0.014166666666666675 + 0 0 0 + ] + } + } + ] + name "right_foot_force_sensor" + boundingObject USE right_foot_force_sensor_shape + physics Physics { + density -1 + mass 4.000000000000001 + centerOfMass [ + 0 0 0 + ] + inertiaMatrix [ + 0.054166666666666696 0.0666666666666667 0.014166666666666675 + 0 0 0 + ] + } + type "force-3d" + } DEF right_foot Shape { appearance PBRAppearance { roughness 1 @@ -339,11 +479,17 @@ Robot { jointParameters3 JointParameters { } device [ + PositionSensor { + name "torso_pitch_position_sensor" + } RotationalMotor { name "torso_pitch" } ] device2 [ + PositionSensor { + name "torso_yaw_position_sensor" + } RotationalMotor { name "torso_yaw" minPosition -1.5707963267948966 @@ -351,6 +497,9 @@ Robot { } ] device3 [ + PositionSensor { + name "torso_roll_position_sensor" + } RotationalMotor { name "torso_roll" } @@ -359,6 +508,12 @@ Robot { translation 0 1.51 0 rotation 1 0 0 0 children [ + Gyro { + name "torso_gyro" + } + Accelerometer { + name "torso_accelerometer" + } DEF torso Shape { appearance PBRAppearance { roughness 1 @@ -378,11 +533,17 @@ Robot { axis 1 0 0 } device [ + PositionSensor { + name "right_shoulder_roll_position_sensor" + } RotationalMotor { name "right_shoulder_roll" } ] device2 [ + PositionSensor { + name "right_shoulder_yaw_position_sensor" + } RotationalMotor { name "right_shoulder_yaw" minPosition -1.5707963267948966 @@ -390,6 +551,9 @@ Robot { } ] device3 [ + PositionSensor { + name "right_shoulder_pitch_position_sensor" + } RotationalMotor { name "right_shoulder_pitch" } @@ -403,13 +567,16 @@ Robot { anchor 0 -0.2125 0 } device [ + PositionSensor { + name "right_elbow_pitch_position_sensor" + } RotationalMotor { name "right_elbow_pitch" } ] endPoint Solid { translation 0 -0.42499999999726895 -1.5235950555825944e-06 - rotation 0.9999999999999999 0 0 3.5849604925388305e-06 + rotation 1 0 0 3.5849604925388305e-06 children [ DEF right_lower_arm Shape { appearance PBRAppearance { @@ -471,11 +638,17 @@ Robot { jointParameters3 JointParameters { } device [ + PositionSensor { + name "neck_pitch_position_sensor" + } RotationalMotor { name "neck_pitch" } ] device2 [ + PositionSensor { + name "neck_yaw_position_sensor" + } RotationalMotor { name "neck_yaw" minPosition -1.5707963267948966 @@ -483,6 +656,9 @@ Robot { } ] device3 [ + PositionSensor { + name "neck_roll_position_sensor" + } RotationalMotor { name "neck_roll" } @@ -526,11 +702,17 @@ Robot { axis 1 0 0 } device [ + PositionSensor { + name "left_shoulder_roll_position_sensor" + } RotationalMotor { name "left_shoulder_roll" } ] device2 [ + PositionSensor { + name "left_shoulder_yaw_position_sensor" + } RotationalMotor { name "left_shoulder_yaw" minPosition -1.5707963267948966 @@ -538,6 +720,9 @@ Robot { } ] device3 [ + PositionSensor { + name "left_shoulder_pitch_position_sensor" + } RotationalMotor { name "left_shoulder_pitch" } @@ -550,6 +735,9 @@ Robot { anchor 0 -0.2125 0 } device [ + PositionSensor { + name "left_elbow_pitch_position_sensor" + } RotationalMotor { name "left_elbow_pitch" }