mirror of
https://github.com/PotentiaRobotics/engine-software.git
synced 2025-04-09 23:00:21 -04:00
Add all sensors to webots
This commit is contained in:
Aditya Vasantharao
parent
84787a5f9d
commit
efc74a55ee
|
|
@ -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()
|
||||
main()
|
||||
|
|
@ -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"
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user