def _generate_env_properties(self): env_props = bpymorse.get_properties(self.env._bpy_object) longitude = env_props.get('longitude') geod_latitude = env_props.get('latitude') altitude = env_props.get('altitude') if 'angle_against_north' in env_props: angle = env_props.get('angle_against_north') - pi / 2 else: angle = 0.0 try: self.coord_conv = CoordinateConverter(geod_latitude, longitude, altitude, angle) except: self.coord_conv = CoordinateConverter(geod_latitude, longitude, altitude) geoc = self.coord_conv.ecef_to_geocentric( self.coord_conv.ltp_to_ecef( self.coord_conv.blender_to_ltp(numpy.matrix([0.0, 0.0, 0.0])))) geoc[0, 0] = degrees(geoc[0, 0]) geoc[0, 1] = degrees(geoc[0, 1]) geoc[0, 2] -= self.coord_conv.A self.properties['env'] = { 'longitude': geoc[0, 0], 'latitude': geoc[0, 1], 'altitude': geoc[0, 2] }
def __init__(self, date=None): from morse.sensors._magnetometer import Magnetometer as Mag self._mag = Mag(os.path.join(MORSE_COMPONENTS, 'WMM.COF')) self._coord_conv = CoordinateConverter.instance() if date: self._date = date else: self._date = _decimal_date(datetime.date.today())
def __init__(self, date = None): from morse.sensors._magnetometer import Magnetometer as Mag self._mag = Mag(os.path.join(MORSE_COMPONENTS, 'WMM.COF')) self._coord_conv = CoordinateConverter.instance() if date: self._date = date else: self._date = _decimal_date(datetime.date.today())
def __init__(self, obj, parent=None): """ Constructor method. Receives the reference to the Blender object. The second parameter should be the name of the object's parent. """ # Call the constructor of the parent class GPS.__init__(self, obj, parent) # Variables to store the previous LTP position self.pltp = None self.v = [0.0, 0.0, 0.0] self.coord_converter = CoordinateConverter.instance()
def __init__(self, obj, parent=None): """ Constructor method. Receives the reference to the Blender object. The second parameter should be the name of the object's parent. """ # Call the constructor of the parent class GPS.__init__(self, obj, parent) ##copied from accelerometer # Variables to store the previous position self.ppx = 0.0 self.ppy = 0.0 self.ppz = 0.0 # Variables to store the previous velocity self.pvx = 0.0 self.pvy = 0.0 self.pvz = 0.0 # Make a new reference to the sensor position self.p = self.bge_object.position self.v = [0.0, 0.0, 0.0] # Velocity self.pv = [0.0, 0.0, 0.0] # Previous Velocity self.coord_converter = CoordinateConverter.instance()
def __init__(self, obj, parent=None): """ Constructor method. Receives the reference to the Blender object. The second parameter should be the name of the object's parent. """ logger.info('%s initialization' % obj.name) # Call the constructor of the parent class morse.core.sensor.Sensor.__init__(self, obj, parent) has_physics = bool(self.robot_parent.bge_object.getPhysicsId()) if self._type == 'Automatic': if has_physics: self._type = 'Velocity' else: self._type = 'Position' if self._type == 'Velocity' and not has_physics: logger.error("Invalid configuration : Velocity computation without " "physics") return if self._type == 'Velocity': # make new references to the robot velocities and use those. self.robot_w = self.robot_parent.bge_object.localAngularVelocity else: # previous attitude euler angles as vector self.pp = copy(self.position_3d) if self._use_angle_against_north: self._coord_converter = CoordinateConverter.instance() # imu2body will transform a vector from imu frame to body frame self.imu2body = self.sensor_to_robot_position_3d() # rotate vector from body to imu frame self.rot_b2i = self.imu2body.rotation.conjugated() logger.info("Attitude Component initialized, runs at %.2f Hz ", self.frequency)
def __init__(self, obj, parent=None): logger.info('%s initialization' % obj.name) # Call the constructor of the parent class morse.core.actuator.Actuator.__init__(self, obj, parent) #logger.setLevel(logging.DEBUG) # Make new reference to the robot velocities (mathutils.Vector) self.robot_w = self.robot_parent.bge_object.localAngularVelocity # get the robot inertia (list [ix, iy, iz]) robot_inertia = self.robot_parent.bge_object.localInertia self.inertia = Vector(tuple(robot_inertia)) logger.debug("robot inertia: (%.3f %.3f %.3f)" % tuple(self.inertia)) # yaw setpoint in radians is just integrated from yaw rate input self.yaw_setpoint = 0.0 self.prev_err = Vector((0.0, 0.0, 0.0)) if self._use_angle_against_north: self._coord_converter = CoordinateConverter.instance() logger.info("Component initialized, runs at %.2f Hz ", self.frequency)
def initialize(self): self.converter = CoordinateConverter.instance()
def initialize(self): self.converter = CoordinateConverter.instance() self.method = None
class JSBSimExporter: def __init__(self, env): self.env = env self.properties = {} self.coord_conv = None def _generate_hla_properties(self): hla_stream_manager = MORSE_DATASTREAM_MODULE['hla'] hla_config = Configuration.stream_manager[hla_stream_manager] hla_federation = hla_config['federation'] hla_sync_point = hla_config.get('sync_point', None) if hla_sync_point: self.properties['hla'] = { 'federation': hla_federation, 'sync_point': hla_sync_point } else: self.properties['hla'] = {'federation': hla_federation} def _generate_simu_properties(self): self.properties['simu'] = {'fps': bpymorse.get_fps()} def _generate_env_properties(self): env_props = bpymorse.get_properties(self.env._bpy_object) longitude = env_props.get('longitude') geod_latitude = env_props.get('latitude') altitude = env_props.get('altitude') if 'angle_against_north' in env_props: angle = env_props.get('angle_against_north') - pi / 2 else: angle = 0.0 try: self.coord_conv = CoordinateConverter(geod_latitude, longitude, altitude, angle) except: self.coord_conv = CoordinateConverter(geod_latitude, longitude, altitude) geoc = self.coord_conv.ecef_to_geocentric( self.coord_conv.ltp_to_ecef( self.coord_conv.blender_to_ltp(numpy.matrix([0.0, 0.0, 0.0])))) geoc[0, 0] = degrees(geoc[0, 0]) geoc[0, 1] = degrees(geoc[0, 1]) geoc[0, 2] -= self.coord_conv.A self.properties['env'] = { 'longitude': geoc[0, 0], 'latitude': geoc[0, 1], 'altitude': geoc[0, 2] } def _generate_robot_properties(self): robots = {} for obj in bpymorse.get_objects(): p = obj.game.properties if 'Robot_Tag' in p and 'jsbsim_model' in p: loc = obj.location rot = obj.rotation_euler model = p['jsbsim_model'].value geoc = self.coord_conv.ecef_to_geocentric( self.coord_conv.ltp_to_ecef( self.coord_conv.blender_to_ltp(numpy.matrix(loc)))) geoc[0, 0] = degrees(geoc[0, 0]) geoc[0, 1] = degrees(geoc[0, 1]) geoc[0, 2] -= self.coord_conv.A # XXX check angles orientation robots[obj.name] = { 'longitude': geoc[0, 0], 'latitude': geoc[0, 1], 'altitude': geoc[0, 2], 'yaw': rot.z, 'pitch': rot.y, 'roll': rot.x, 'model': model } self.properties['robots'] = robots def _generate_properties(self): self._generate_simu_properties() self._generate_hla_properties() self._generate_env_properties() self._generate_robot_properties() def dump(self): self._generate_properties() f = open("/tmp/jsbsim_config.json", "w") json.dump(self.properties, f, indent=4)