rushilwiz/engine-software
1
0
Fork 0
mirror of https://github.com/PotentiaRobotics/engine-software.git synced 2025-04-17 18:40:18 -04:00
Code Issues Actions Packages Projects Releases Wiki Activity

Add all sensors to webots

Browse Source
This commit is contained in:
Aditya Vasantharao 2021-03-03 20:17:44 -05:00
parent 84787a5f9d
commit efc74a55ee
2 changed files with 264 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
simulation/controllers/olympian/olympian.py
View File

@ -2,7 +2,7 @@
# You may need to import some classes of the controller module. Ex: # You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor # from controller import Robot, Motor, DistanceSensor
from controller import Robot from controller import Robot, TouchSensor
import time import time
import numpy as np import numpy as np
@ -43,9 +43,9 @@ def calculateCOM(robot):
# COM of each link is calculated forward kinematics equations # COM of each link is calculated forward kinematics equations
calc = np.dot(transformationMatrixToLink,vectorOfJointAngles) calc = np.dot(transformationMatrixToLink,vectorOfJointAngles)
print(calc) # print(calc)
calc=np.dot(calc,transformationMatrixToLinkCOM) calc=np.dot(calc,transformationMatrixToLinkCOM)
print(calc) # print(calc)
#calc = transformationMatrixToLink.dot(vectorOfJointAngles).dot(transformationMatrixToLinkCOM) #calc = transformationMatrixToLink.dot(vectorOfJointAngles).dot(transformationMatrixToLinkCOM)
# COM of the entire rigid body is calculated using a weighted average # COM of the entire rigid body is calculated using a weighted average
@ -58,57 +58,81 @@ def main():
# create the Robot instance. # create the Robot instance.
print("Initializing world...") print("Initializing world...")
robot = Robot() robot = Robot()
timestep = 64 timestep = 16
start = 0 curr_time = 0
while robot.step(32) != -1:
print(calculateCOM(robot)) lfootsensor = robot.getDevice("torso_gyro")
start += 32/1000.0
head_motor = robot.getDevice("torso_yaw")
# motor2 = robot.getDevice("neck_roll") 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") lknee = robot.getDevice("left_knee_pitch")
rknee = robot.getDevice("right_knee_pitch") rknee = robot.getDevice("right_knee_pitch")
lhip = robot.getDevice("left_hip_pitch") lhip = robot.getDevice("left_hip_pitch")
rhip = robot.getDevice("right_hip_pitch") rhip = robot.getDevice("right_hip_pitch")
lfoot = robot.getDevice("left_ankle_pitch") lfootsensor.enable(10)
rfoot = robot.getDevice("right_ankle_pitch")
torso_pitch = robot.getDevice("torso_pitch")
head_motor.setVelocity(1) # print sensor data every second
lknee.setVelocity(2)
rknee.setVelocity(2)
lknee.setPosition(2)
rknee.setPosition(2)
torso_pitch.setVelocity(10)
# torso_pitch.setTorque(10)
lhip.setVelocity(2) if curr_time % 1 < 0.01:
rhip.setVelocity(2)
lhip.setPosition(-0.7) print(lfootsensor.getValues())
rhip.setPosition(-0.7)
# lfoot = robot.getDevice("left_ankle_pitch")
lfoot.setVelocity(2) # rfoot = robot.getDevice("right_ankle_pitch")
rfoot.setVelocity(2)
lfoot.setPosition(0.3) def func():
rfoot.setPosition(0.3)
torso_pitch = robot.getDevice("torso_pitch")
pos = 0
add = False # head_motor.setVelocity(1)
lknee.setVelocity(2)
if start > 6: # rknee.setVelocity(2)
while robot.step(32) != -1: lknee.setPosition(1)
torso_pitch.setPosition(pos) # rknee.setPosition(2)
# time.sleep(0.1) 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: # if pos >= 2:
pos += 0.2 # add = False
else: # elif pos <= 0:
pos -= 0.2 # add = True
if pos >= 2:
add = False
elif pos <= 0:
add = True
# get the time step of the current world. # get the time step of the current world.
# timestep = int(robot.getBasicTimeStep()) # timestep = int(robot.getBasicTimeStep())
@ -134,6 +158,4 @@ def main():
# Enter here exit cleanup code. # Enter here exit cleanup code.
main()
if __name__ == '__main__':
main()

198
simulation/worlds/olympian.wbt
View File

@ -1,11 +1,12 @@
#VRML_SIM R2021a utf8 #VRML_SIM R2021a utf8
WorldInfo { WorldInfo {
gravity 0.2 gravity 0.2
basicTimeStep 16
coordinateSystem "NUE" coordinateSystem "NUE"
} }
Viewpoint { Viewpoint {
orientation -0.7752447244164626 -0.5132245726366987 -0.36823383237600654 1.5186048037836888 orientation -0.6002466325295073 0.7753295528057134 0.19638753698511255 0.7477472527006542
position -2.175946533475156 4.937428530305314 0.06846856025694995 position 0.844819390262747 2.391065676253801 3.2173738573028166
} }
TexturedBackground { TexturedBackground {
} }
@ -15,9 +16,16 @@ RectangleArena {
floorSize 5 5 floorSize 5 5
} }
Robot { Robot {
translation -1 2 0 translation -1 0 0
rotation -1 0 0 1.57
children [ children [
Gyro {
translation 0 1.05 0
name "pelvis_gyro"
}
Accelerometer {
translation 0 1.05 0
name "pelvis_accelerometer"
}
DEF Body Transform { DEF Body Transform {
translation 0 1.05 0 translation 0 1.05 0
children [ children [
@ -42,11 +50,17 @@ Robot {
jointParameters3 JointParameters { jointParameters3 JointParameters {
} }
device [ device [
PositionSensor {
name "left_hip_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_hip_pitch" name "left_hip_pitch"
} }
] ]
device2 [ device2 [
PositionSensor {
name "left_hip_yaw_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_hip_yaw" name "left_hip_yaw"
minPosition -1.5707963267948966 minPosition -1.5707963267948966
@ -54,6 +68,9 @@ Robot {
} }
] ]
device3 [ device3 [
PositionSensor {
name "left_hip_roll_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_hip_roll" name "left_hip_roll"
} }
@ -77,6 +94,9 @@ Robot {
anchor 0 -0.225 0 anchor 0 -0.225 0
} }
device [ device [
PositionSensor {
name "left_knee_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_knee_pitch" name "left_knee_pitch"
} }
@ -105,12 +125,18 @@ Robot {
axis 1 0 0 axis 1 0 0
} }
device [ device [
PositionSensor {
name "left_ankle_yaw_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_ankle_yaw" name "left_ankle_yaw"
maxTorque 100 maxTorque 100
} }
] ]
device2 [ device2 [
PositionSensor {
name "left_ankle_roll_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_ankle_roll" name "left_ankle_roll"
minPosition -1.5707963267948966 minPosition -1.5707963267948966
@ -118,6 +144,9 @@ Robot {
} }
] ]
device3 [ device3 [
PositionSensor {
name "left_ankle_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_ankle_pitch" name "left_ankle_pitch"
maxTorque 100 maxTorque 100
@ -126,6 +155,51 @@ Robot {
endPoint Solid { endPoint Solid {
translation 0 -0.25 0 translation 0 -0.25 0
children [ 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 { DEF left_foot Shape {
appearance PBRAppearance { appearance PBRAppearance {
roughness 1 roughness 1
@ -190,11 +264,17 @@ Robot {
jointParameters3 JointParameters { jointParameters3 JointParameters {
} }
device [ device [
PositionSensor {
name "right_hip_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_hip_pitch" name "right_hip_pitch"
} }
] ]
device2 [ device2 [
PositionSensor {
name "right_hip_yaw_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_hip_yaw" name "right_hip_yaw"
minPosition -1.5707963267948966 minPosition -1.5707963267948966
@ -202,6 +282,9 @@ Robot {
} }
] ]
device3 [ device3 [
PositionSensor {
name "right_hip_roll_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_hip_roll" name "right_hip_roll"
} }
@ -225,6 +308,9 @@ Robot {
anchor 0 -0.225 0 anchor 0 -0.225 0
} }
device [ device [
PositionSensor {
name "right_knee_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_knee_pitch" name "right_knee_pitch"
} }
@ -253,12 +339,18 @@ Robot {
axis 1 0 0 axis 1 0 0
} }
device [ device [
PositionSensor {
name "right_ankle_yaw_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_ankle_yaw" name "right_ankle_yaw"
maxTorque 100 maxTorque 100
} }
] ]
device2 [ device2 [
PositionSensor {
name "right_ankle_roll_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_ankle_roll" name "right_ankle_roll"
minPosition -1.5707963267948966 minPosition -1.5707963267948966
@ -266,6 +358,9 @@ Robot {
} }
] ]
device3 [ device3 [
PositionSensor {
name "right_ankle_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_ankle_pitch" name "right_ankle_pitch"
maxTorque 100 maxTorque 100
@ -274,6 +369,51 @@ Robot {
endPoint Solid { endPoint Solid {
translation 0 -0.25 0 translation 0 -0.25 0
children [ 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 { DEF right_foot Shape {
appearance PBRAppearance { appearance PBRAppearance {
roughness 1 roughness 1
@ -339,11 +479,17 @@ Robot {
jointParameters3 JointParameters { jointParameters3 JointParameters {
} }
device [ device [
PositionSensor {
name "torso_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "torso_pitch" name "torso_pitch"
} }
] ]
device2 [ device2 [
PositionSensor {
name "torso_yaw_position_sensor"
}
RotationalMotor { RotationalMotor {
name "torso_yaw" name "torso_yaw"
minPosition -1.5707963267948966 minPosition -1.5707963267948966
@ -351,6 +497,9 @@ Robot {
} }
] ]
device3 [ device3 [
PositionSensor {
name "torso_roll_position_sensor"
}
RotationalMotor { RotationalMotor {
name "torso_roll" name "torso_roll"
} }
@ -359,6 +508,12 @@ Robot {
translation 0 1.51 0 translation 0 1.51 0
rotation 1 0 0 0 rotation 1 0 0 0
children [ children [
Gyro {
name "torso_gyro"
}
Accelerometer {
name "torso_accelerometer"
}
DEF torso Shape { DEF torso Shape {
appearance PBRAppearance { appearance PBRAppearance {
roughness 1 roughness 1
@ -378,11 +533,17 @@ Robot {
axis 1 0 0 axis 1 0 0
} }
device [ device [
PositionSensor {
name "right_shoulder_roll_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_shoulder_roll" name "right_shoulder_roll"
} }
] ]
device2 [ device2 [
PositionSensor {
name "right_shoulder_yaw_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_shoulder_yaw" name "right_shoulder_yaw"
minPosition -1.5707963267948966 minPosition -1.5707963267948966
@ -390,6 +551,9 @@ Robot {
} }
] ]
device3 [ device3 [
PositionSensor {
name "right_shoulder_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_shoulder_pitch" name "right_shoulder_pitch"
} }
@ -403,13 +567,16 @@ Robot {
anchor 0 -0.2125 0 anchor 0 -0.2125 0
} }
device [ device [
PositionSensor {
name "right_elbow_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "right_elbow_pitch" name "right_elbow_pitch"
} }
] ]
endPoint Solid { endPoint Solid {
translation 0 -0.42499999999726895 -1.5235950555825944e-06 translation 0 -0.42499999999726895 -1.5235950555825944e-06
rotation 0.9999999999999999 0 0 3.5849604925388305e-06 rotation 1 0 0 3.5849604925388305e-06
children [ children [
DEF right_lower_arm Shape { DEF right_lower_arm Shape {
appearance PBRAppearance { appearance PBRAppearance {
@ -471,11 +638,17 @@ Robot {
jointParameters3 JointParameters { jointParameters3 JointParameters {
} }
device [ device [
PositionSensor {
name "neck_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "neck_pitch" name "neck_pitch"
} }
] ]
device2 [ device2 [
PositionSensor {
name "neck_yaw_position_sensor"
}
RotationalMotor { RotationalMotor {
name "neck_yaw" name "neck_yaw"
minPosition -1.5707963267948966 minPosition -1.5707963267948966
@ -483,6 +656,9 @@ Robot {
} }
] ]
device3 [ device3 [
PositionSensor {
name "neck_roll_position_sensor"
}
RotationalMotor { RotationalMotor {
name "neck_roll" name "neck_roll"
} }
@ -526,11 +702,17 @@ Robot {
axis 1 0 0 axis 1 0 0
} }
device [ device [
PositionSensor {
name "left_shoulder_roll_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_shoulder_roll" name "left_shoulder_roll"
} }
] ]
device2 [ device2 [
PositionSensor {
name "left_shoulder_yaw_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_shoulder_yaw" name "left_shoulder_yaw"
minPosition -1.5707963267948966 minPosition -1.5707963267948966
@ -538,6 +720,9 @@ Robot {
} }
] ]
device3 [ device3 [
PositionSensor {
name "left_shoulder_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_shoulder_pitch" name "left_shoulder_pitch"
} }
@ -550,6 +735,9 @@ Robot {
anchor 0 -0.2125 0 anchor 0 -0.2125 0
} }
device [ device [
PositionSensor {
name "left_elbow_pitch_position_sensor"
}
RotationalMotor { RotationalMotor {
name "left_elbow_pitch" name "left_elbow_pitch"
} }