def set_node_colour(node: Node, colour: Tuple[float, float, float]) -> None:
node.getField('zoneColour').setSFColor(list(colour))
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 set_node_luminosity(self, node: Node, luminosity: float) -> None:
node.getField('luminosity').setSFFloat(luminosity)
def set_node_intensity(self, node: Node, intensity: float) -> None:
node.getField('intensity').setSFFloat(intensity)
def inform_start(node: Node) -> None:
node.getField('customData').setSFString('start')