def __init__(self, *args, **kwargs):
"""Supervisor constructor."""
self.lastSimulation = False
Supervisor.__init__(self, *args, **kwargs)
self.isParserTest = "parser" in self.getCustomData()
isDefaultWorld = "empty" in self.getCustomData()
if isDefaultWorld:
self.cwdPrefix = '../../../../tests/parser/'
else:
self.cwdPrefix = '../../'
self.lastSimulation = False
# prepare the next simulation
self.indexFileManager = IndexFileManager(self.cwdPrefix +
self.indexFilename)
self.simulationFileManager = SimulationFileManager(
self.cwdPrefix + self.simulationFilename)
self.outputFileManager = OutputFileManager(self.cwdPrefix +
self.outputFilename)
currentIndex = self.indexFileManager.readIndex()
self.currentSimulationFilename = self.simulationFileManager.filenameAtLine(
currentIndex)
print('RUN: ' + self.currentSimulationFilename)
nSimulations = self.simulationFileManager.countSimulations()
if currentIndex + 1 >= nSimulations:
self.lastSimulation = True
def __init__(self):
"""Constructor: initialize constants."""
self.BODY_PARTS_NUMBER = 13
self.WALK_SEQUENCES_NUMBER = 8
self.ROOT_HEIGHT = 1.27
self.CYCLE_TO_DISTANCE_RATIO = 0.22
self.speed = 1.15
self.current_height_offset = 0
self.joints_position_field = []
self.joint_names = [
"leftArmAngle", "leftLowerArmAngle", "leftHandAngle",
"rightArmAngle", "rightLowerArmAngle", "rightHandAngle",
"leftLegAngle", "leftLowerLegAngle", "leftFootAngle",
"rightLegAngle", "rightLowerLegAngle", "rightFootAngle",
"headAngle"
]
self.height_offsets = [ # those coefficients are empirical coefficients which result in a realistic walking gait
-0.02, 0.04, 0.08, -0.03, -0.02, 0.04, 0.08, -0.03
]
self.angles = [ # those coefficients are empirical coefficients which result in a realistic walking gait
[-0.52, -0.15, 0.58, 0.7, 0.52, 0.17, -0.36, -0.74], # left arm
[0.0, -0.16, -0.7, -0.38, -0.47, -0.3, -0.58, -0.21], # left lower arm
[0.12, 0.0, 0.12, 0.2, 0.0, -0.17, -0.25, 0.0], # left hand
[0.52, 0.17, -0.36, -0.74, -0.52, -0.15, 0.58, 0.7], # right arm
[-0.47, -0.3, -0.58, -0.21, 0.0, -0.16, -0.7, -0.38], # right lower arm
[0.0, -0.17, -0.25, 0.0, 0.12, 0.0, 0.12, 0.2], # right hand
[-0.55, -0.85, -1.14, -0.7, -0.56, 0.12, 0.24, 0.4], # left leg
[1.4, 1.58, 1.71, 0.49, 0.84, 0.0, 0.14, 0.26], # left lower leg
[0.07, 0.07, -0.07, -0.36, 0.0, 0.0, 0.32, -0.07], # left foot
[-0.56, 0.12, 0.24, 0.4, -0.55, -0.85, -1.14, -0.7], # right leg
[0.84, 0.0, 0.14, 0.26, 1.4, 1.58, 1.71, 0.49], # right lower leg
[0.0, 0.0, 0.42, -0.07, 0.07, 0.07, -0.07, -0.36], # right foot
[0.18, 0.09, 0.0, 0.09, 0.18, 0.09, 0.0, 0.09] # head
]
Supervisor.__init__(self)
def __init__(self):
"""Constructor: initialize constants."""
self.BODY_PARTS_NUMBER = 13
self.WALK_SEQUENCES_NUMBER = 8
self.ROOT_HEIGHT = 1.27
self.CYCLE_TO_DISTANCE_RATIO = 0.22
self.speed = 1.15
self.clearLidarFeedback = False
self.commandOutput = []
self.buttonPressed = False
self.add_new_current_waypoint = False
self.caneMovement = True
self.stopMoving = False
self.stopMovingFeedbackLevel = 1.0
self.delayMoving = False
self.timeNotMoving = 0
self.stopTimestamp = 0
self.delayStartTimestamp = 0
self.resumeMovingDelay = 1.5
self.timeAfterDelayStart = 0
self.runTime = 0
self.current_height_offset = 0
self.relativeCaneAngle = 0
self.locationOne = [-15, 15]
self.joints_position_field = []
self.joint_names = [
"leftArmAngle", "leftLowerArmAngle", "leftHandAngle",
"rightArmAngle", "rightLowerArmAngle", "rightHandAngle",
"rightArmAngle2", "rightLowerArmAngle2","rightHandAngle2",
"leftLegAngle", "leftLowerLegAngle", "leftFootAngle",
"rightLegAngle", "rightLowerLegAngle", "rightFootAngle",
"headAngle"
]
self.height_offsets = [ # those coefficients are empirical coefficients which result in a realistic walking gait
-0.02, 0.04, 0.08, -0.03, -0.02, 0.04, 0.08, -0.03
]
self.angles = [ # those coefficients are empirical coefficients which result in a realistic walking gait
[-0.52, -0.15, 0.58, 0.7, 0.52, 0.17, -0.36, -0.74], # left arm
[0.0, -0.16, -0.7, -0.38, -0.47, -0.3, -0.58, -0.21], # left lower arm
[0.12, 0.0, 0.12, 0.2, 0.0, -0.17, -0.25, 0.0], # left hand
#[0.52, 0.17, -0.36, -0.74, -0.52, -0.15, 0.58, 0.7], # right arm
#[-0.47, -0.3, -0.58, -0.21, 0.0, -0.16, -0.7, -0.38], # right lower arm
#[0.0, -0.17, -0.25, 0.0, 0.12, 0.0, 0.12, 0.2], # right hand
[-0.4], # right arm
[-0.5], # right lower arm
[0.8, 0.75, 0.75, 0.9, 0.75, 0.75, 0.9, 1], # right hand
[0.0], # right arm "horizontal"
[0.15, 0.1, 0.05, 0, 0.05, 0.1, 0.15, 0.20], # right lower arm "horizontal"
[0.37, 0.14, -0.08, -0.3, -0.08, 0.14, 0.37, 0.6], # right hand "horizontal"
[-0.55, -0.85, -1.14, -0.7, -0.56, 0.12, 0.24, 0.4], # left leg
[1.4, 1.58, 1.71, 0.49, 0.84, 0.0, 0.14, 0.26], # left lower leg
[0.07, 0.07, -0.07, -0.36, 0.0, 0.0, 0.32, -0.07], # left foot
[-0.56, 0.12, 0.24, 0.4, -0.55, -0.85, -1.14, -0.7], # right leg
[0.84, 0.0, 0.14, 0.26, 1.4, 1.58, 1.71, 0.49], # right lower leg
[0.0, 0.0, 0.42, -0.07, 0.07, 0.07, -0.07, -0.36], # right foot
[0.18, 0.09, 0.0, 0.09, 0.18, 0.09, 0.0, 0.09] # head
]
Supervisor.__init__(self)
def __init__(self, port):
"""
Initialization of the supervisor.
:param port: Port number reserved for service
:type port: int
"""
Supervisor.__init__(self)
self._connections = []
try:
self._s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
except socket.error, msg:
print msg[1]
def __init__(self):
Supervisor.__init__(self)
self.opti = optimization.Webots()
if self.opti:
print('Optimization mode!')
print('Settings:')
for s in self.opti.settings:
print(' {0}: {1}'.format(s, self.opti.settings[s]))
print('Parameters:')
for s in self.opti.parameters:
print(' {0}: {1}'.format(s, self.opti.parameters[s].value))
else:
print('Not in optimization mode... :(')
def __init__(self, population_size=10):
"""
Initializes supervisor.
"""
Supervisor.__init__(self)
self.root = self.getRoot()
# Collect all available controller nodes
self.modules = []
self.modules += [self.getFromDef('MODULE_1')]
self.modules += [self.getFromDef('MODULE_2')]
self.modules += [self.getFromDef('MODULE_3')]
self.modules += [self.getFromDef('MODULE_4')]
self.modules += [self.getFromDef('MODULE_5')]
self.modules += [self.getFromDef('MODULE_6')]
self.modules += [self.getFromDef('MODULE_7')]
self.modules += [self.getFromDef('MODULE_8')]
# Register fitness functions
self.fitness_functions = {
'velocity': self.evaluate_velocity,
'distance': self.evaluate_distance
}
# Zero the robot position and timestamp
self.robot_x = None
self.robot_y = None
self.robot_z = None
self.time_record = None
# Load configuration
self.config = {}
try:
self.load_configuration()
except IOError:
# If the config file doesn't exist yet, setup an initial configuration
self.reset_configuration(population_size)
def __init__(self):
"""Constructor: initialize constants."""
self.BODY_PARTS_NUMBER = 19
self.MOVE_SEQUENCES_NUMBER = 2
self.CYCLE_TO_DISTANCE_RATIO = 0.5
self.speed = 1.00
self.joints_position_field = []
self.joint_names = [
"leftArmAngle", "leftLowerArmAngle", "leftHandAngle",
"rightArmAngle", "rightLowerArmAngle", "rightHandAngle",
"leftLegAngle", "leftLowerLegAngle", "leftFootAngle",
"rightLegAngle", "rightLowerLegAngle", "rightFootAngle",
"headAngle", "middleTorsoAngle1", "middleTorsoAngle2",
"middleTorsoAngle3", "upperTorsoAngle1", "upperTorsoAngle2",
"upperTorsoAngle3"
]
self.angles = [
[-0.80, -2.20], # left arm
[-0.80, -0.80], # left lower arm
[+0.00, +0.00], # left hand
[-0.80, -2.20], # right arm
[-0.70, -0.70], # right lower arm
[+0.00, +0.00], # right hand
[-1.57, -1.57], # left leg
[+1.57, +1.57], # left lower leg
[+0.00, +0.00], # left foot
[-1.57, -1.57], # right leg
[+1.57, +1.57], # right lower leg
[+0.00, +0.00], # right foot
[+0.00, +0.10], # head
[+0.00, +0.00], # middle torso turn left/right
[+0.00, -0.40], # middle torso bend front/behind
[+0.00, +0.00], # middle torso bend left/right
[+0.00, +0.00], # upper torso turn left/right
[+0.00, -0.40], # upper torso bend front/behind
[+0.00, +0.00], # upper torso bend left/right
]
Supervisor.__init__(self)
def __init__(self, port):
"""
Initialization of the supervisor.
:param port: Port number reserved for service
:type port: int
"""
Supervisor.__init__(self)
self._connections = []
self._prev_t = 0.0
self._t = 0.0 # simulation time
ball = self.getFromDef("BALL")
self._ball_trans = ball.getField('translation')
self._prev_ball_pos = np.array(self._ball_trans.getSFVec3f())
self._ball_pos = self._prev_ball_pos
try:
self._s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
except socket.error, msg:
print(msg[1])
def __init__(self, port):
"""
Initialization of the supervisor.
:param port: Port number reserved for service
:type port: int
"""
Supervisor.__init__(self)
self._connections = []
self._prev_t = 0.0
self._t = 0.0 # simulation time
ball = self.getFromDef("BALL")
self._ball_trans = ball.getField('translation')
self._prev_ball_pos = np.array(self._ball_trans.getSFVec3f())
self._ball_pos = self._prev_ball_pos
try:
self._s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
except socket.error, msg:
print(msg[1])
def __init__(self):
Supervisor.__init__(self)
self.currentlyPlaying = False
self.devicesNumber = self.getNumberOfDevices()
self.sup = self.getSelf()
# initialize stuff
self.findAndEnableDevices()
# get motors and its limits:
self.motor_device = dict()
self.motorLimits = self.getMotorsLimits()
self.motorSensorsNames = self.getMotorSensorsNames()
self.robot_node = Supervisor.getSelf(self)
self.display = self.getDisplay("ClockDisplay")
self.init_display()
self.trans_field = Node.getField(self.robot_node, 'translation')
self.rot_field = Node.getField(self.robot_node, 'rotation')
# self.INITIAL_TRANS = Field.getSFVec3f(self.trans_field)
# self.INITIAL_ROT = Field.getSFRotation(self.rot_field)
self.INITIAL_TRANS = [0, 0.239, 0]
self.INITIAL_ROT = [-1, 0, 0, 1.5708]
self.motor_names = list(self.motorLimits.keys())
self.INITIAL_MOTOR_POS = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.6,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.6,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.25,
'RHipPitch': -0.4,
'RKneePitch': 0.9,
'RAnklePitch': -0.8,
'RAnkleRoll': 0.4,
'LHipYawPitch': 0.25,
'LHipRoll': -0.25,
'LHipPitch': -0.4,
'LKneePitch': 0.9,
'LAnklePitch': -0.8,
'LAnkleRoll': -0.4
}
self.LEFT_MID = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.3,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.3,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.2,
'RHipPitch': 0.75,
'RKneePitch': -0.8,
'RAnklePitch': -0.2,
'RAnkleRoll': 0.35,
'LHipYawPitch': 0.25,
'LHipRoll': -0.2,
'LHipPitch': 0.05,
'LKneePitch': 0.35,
'LAnklePitch': -0.4,
'LAnkleRoll': -0.4
}
self.LEFT_STEP = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.3,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.3,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.2,
'RHipPitch': 0.85,
'RKneePitch': -0.8,
'RAnklePitch': -0.37,
'RAnkleRoll': 0.35,
'LHipYawPitch': 0.25,
'LHipRoll': -0.25,
'LHipPitch': 0,
'LKneePitch': -0.8,
'LAnklePitch': 0.52,
'LAnkleRoll': -0.35
}
self.RIGHT_MID = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.3,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.3,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.15,
'RHipPitch': 0,
'RKneePitch': 0.35,
'RAnklePitch': -0.3,
'RAnkleRoll': 0.4,
'LHipYawPitch': 0.25,
'LHipRoll': -0.2,
'LHipPitch': 0.75,
'LKneePitch': -0.8,
'LAnklePitch': -0.1,
'LAnkleRoll': -0.4
}
self.RIGHT_STEP = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.3,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.3,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.15,
'RHipPitch': 0,
'RKneePitch': -0.8,
'RAnklePitch': 0.52,
'RAnkleRoll': 0.35,
'LHipYawPitch': 0.25,
'LHipRoll': -0.2,
'LHipPitch': 0.85,
'LKneePitch': -0.8,
'LAnklePitch': -0.35,
'LAnkleRoll': -0.42
}
self.STAND_UP = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.6,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.6,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.25,
'RHipPitch': 0.5,
'RKneePitch': -0.8,
'RAnklePitch': 0.1,
'RAnkleRoll': 0.4,
'LHipYawPitch': 0.25,
'LHipRoll': -0.25,
'LHipPitch': 0.5,
'LKneePitch': -0.8,
'LAnklePitch': 0.1,
'LAnkleRoll': -0.4
}
self.DEFAULT_MOTOR_POS = self.INITIAL_MOTOR_POS
def __init__(self):
Supervisor.__init__(self)
self.updates = [] # list of objects for which location updates are requested
