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')