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)
# 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. """
# 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 __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)
