def instance():
if not CoordinateConverter._instance:
try:
ssr = blenderapi.getssr()
latitude = ssr["latitude"]
longitude = ssr["longitude"]
altitude = ssr["altitude"]
CoordinateConverter._instance = \
CoordinateConverter(latitude, longitude, altitude)
except KeyError as e:
logger.error("Missing environment parameter %s\n", e)
return CoordinateConverter._instance
def instance():
if not CoordinateConverter._instance:
try:
ssr = blenderapi.getssr()
latitude = ssr["latitude"]
longitude = ssr["longitude"]
altitude = ssr["altitude"]
CoordinateConverter._instance = \
CoordinateConverter(latitude, longitude, altitude)
except KeyError as e:
logger.error("Missing environment parameter %s\n", e)
return CoordinateConverter._instance
def parameter(self, arg, prop=None, default=None):
""" get a modifier parameter
The parameter is (by priority order):
1. get from modifier kwargs
2. get from the scene properties
3. set to None
"""
if arg in self.kwargs:
return self.kwargs[arg]
else:
ssr = blenderapi.getssr()
return ssr.get(prop, ssr.get(arg, default))
def details(self):
"""Returns a structure containing all possible details
about the simulation currently running, including
the list of robots, the list of services and datastreams,
the list of middleware in use, etc.
"""
simu = blenderapi.persistantstorage()
details = {}
# Retrieves the list of services and associated middlewares
services = {}
services_iface = {}
for n, i in simu.morse_services.request_managers().items():
services.update(i.services())
for cmpt in i.services():
services_iface.setdefault(cmpt, []).append(n)
def cmptdetails(c):
c = simu.componentDict[c.name]
cmpt = {"type": type(c).__name__,}
if c.name() in services:
cmpt["services"] = services[c.name()]
cmpt["service_interfaces"] = services_iface[c.name()]
if c.name() in simu.datastreams:
streams = simu.datastreams[c.name()]
cmpt["stream_interfaces"] = [(stream[0], stream[2]) for stream in streams]
return cmpt
def robotdetails(r):
robot = {"name": r.name(),
"type": type(r).__name__,
"components": {c.name:cmptdetails(c) for c in r.components},
}
if r.name() in services:
robot["services"] = services[r.name()]
robot["services_interfaces"] = services_iface[r.name()]
return robot
for n, i in simu.stream_managers.items():
pass
details['robots'] = [robotdetails(r) for n, r in simu.robotDict.items()]
details['environment'] = blenderapi.getssr()['environment_file']
return details
def details(self):
"""Returns a structure containing all possible details
about the simulation currently running, including
the list of robots, the list of services and datastreams,
the list of middleware in use, etc.
"""
simu = blenderapi.persistantstorage()
details = {}
# Retrieves the list of services and associated middlewares
services = {}
services_iface = {}
for n, i in simu.morse_services.request_managers().items():
services.update(i.services())
for cmpt in i.services():
services_iface.setdefault(cmpt, []).append(n)
def cmptdetails(c):
c = simu.componentDict[c.name]
cmpt = {"type": type(c).__name__}
if c.name() in services:
cmpt["services"] = services[c.name()]
cmpt["service_interfaces"] = services_iface[c.name()]
if c.name() in simu.datastreams:
streams = simu.datastreams[c.name()]
cmpt["stream_interfaces"] = [(stream[0], stream[2]) for stream in streams]
return cmpt
def robotdetails(r):
robot = {
"name": r.name(),
"type": type(r).__name__,
"components": {c.name: cmptdetails(c) for c in r.components},
}
if r.name() in services:
robot["services"] = services[r.name()]
robot["services_interfaces"] = services_iface[r.name()]
return robot
for n, i in simu.stream_managers.items():
pass
details["robots"] = [robotdetails(r) for n, r in simu.robotDict.items()]
details["environment"] = blenderapi.getssr()["environment_file"]
return details
def instance():
if not CoordinateConverter._instance:
try:
ssr = blenderapi.getssr()
latitude = ssr["latitude"]
longitude = ssr["longitude"]
altitude = ssr["altitude"]
try:
angle_against_east = radians(ssr["angle_against_north"]) - pi / 2
except KeyError as e:
angle_against_east = 0.0
CoordinateConverter._instance = \
CoordinateConverter(latitude, longitude, altitude,
angle_against_east)
except KeyError as e:
logger.error("Missing environment parameter %s\n", e)
return CoordinateConverter._instance
def instance():
if not CoordinateConverter._instance:
try:
ssr = blenderapi.getssr()
latitude = ssr["latitude"]
longitude = ssr["longitude"]
altitude = ssr["altitude"]
try:
angle_against_east = radians(
ssr["angle_against_north"]) - pi / 2
except KeyError as e:
angle_against_east = 0.0
CoordinateConverter._instance = \
CoordinateConverter(latitude, longitude, altitude,
angle_against_east)
except KeyError as e:
logger.error("Missing environment parameter %s\n", e)
return CoordinateConverter._instance
def parameter(self, arg, prop=None, default=None):
""" get a modifier parameter
The parameter is (by priority order):
1. get from modifier kwargs
2. get from the scene properties
3. set to None
"""
ssr = blenderapi.getssr()
if arg in self.kwargs:
return self.kwargs[arg]
else:
try:
if prop:
x = ssr[prop]
else:
x = ssr[arg]
return x
except KeyError:
return default
def parameter(self, arg, prop=None, default=None):
""" get a modifier parameter
The parameter is (by priority order):
1. get from modifier kwargs
2. get from the scene properties
3. set to None
"""
ssr = blenderapi.getssr()
if arg in self.kwargs:
return self.kwargs[arg]
else:
try:
if prop:
x = ssr[prop]
else:
x = ssr[arg]
return x
except KeyError:
return default
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)
###################################################
# Initializations #
###################################################
env = blenderapi.getssr()
self.rho = env.get('rho')
if not self.rho:
self.rho = 1025
#print(self.rho)
# get environment gravity constant
self.g = blenderapi.game_settings().physics_gravity
# make a local reference to the bge_object of the parent robot
self.robot = self.robot_parent.bge_object
# make a local reference to the robot local linear and angular velocity
self.v = self.robot_parent.bge_object.localLinearVelocity
self.w = self.robot_parent.bge_object.localAngularVelocity
#Make a local reference to the robot pose
self.pose3d = self.robot_parent.position_3d
# 6DOF rotation from enu to ned (R_etn == R_nte)
self.R_etn = numpy.matrix([[0, 1, 0, 0, 0, 0], [1, 0, 0, 0, 0, 0],
[0, 0, -1, 0, 0, 0], [0, 0, 0, 0, 1, 0],
[0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, -1]])
# make a local variable for 6-DOF velocity
self.nu = numpy.matrix([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).T
try:
from parameters import p
FluidParameters = p[self.bge_object.name]
except:
FluidParameters = {}
if 'A' in FluidParameters:
self.A = numpy.matrix(FluidParameters['A'])
else:
self.A = numpy.zeros((6, 6))
if 'M_rb' in FluidParameters:
self.M_rb = numpy.matrix(FluidParameters['M_rb'])
else:
self.M_rb = numpy.zeros((6, 6))
if 'D_l' in FluidParameters:
self.D_l = numpy.matrix(FluidParameters['D_l'])
else:
self.D_l = numpy.zeros((6, 6))
if 'D_q' in FluidParameters:
self.D_q = numpy.matrix(FluidParameters['D_q'])
else:
self.D_q = numpy.zeros((6, 6))
if 'r_g' in FluidParameters:
self.r_g = numpy.matrix(FluidParameters['r_g']).T
else:
self.r_g = numpy.zeros((3, 1))
if 'r_b' in FluidParameters:
self.r_b = numpy.matrix(FluidParameters['r_b']).T
else:
self.r_b = numpy.zeros((3, 1))
self.mass = self.M_rb[0, 0]
self.I_g = self.M_rb[3:6, 3:6]
# TODO: Get inertia from builder script
self.totalInertia_NED = self.A + self.M_rb
# Calculate total inertia in enu and set in the physics model
self.totalInertia_ENU = self.R_etn.T * self.totalInertia_NED * self.R_etn
self.robot.set6DOFinertia(self.totalInertia_ENU[0:3, 0:3].tolist(),
self.totalInertia_ENU[0:3, 3:6].tolist(),
self.totalInertia_ENU[3:6, 0:3].tolist(),
self.totalInertia_ENU[3:6, 3:6].tolist())
self.robot.enable6DOF()
# Set Damping in blender to zero (linear,angular)
self.robot.setDamping(0.0, 0.0)
# Open files TODO find better way to store information
#self.lin_damping=open('lin_damping_morse.txt','w')
#self.quad_damping=open('quad_damping_morse.txt', 'w')
#self.coriolis_a=open('coriolis_a_morse.txt', 'w')
#self.coriolis_rb=open('coriolis_rb_morse.txt', 'w')
#self.nu_morse=open('nu_morse.txt', 'w')
#Set init time
#self.time=time=morse.core.blenderapi.persistantstorage().time.time
#self.time_init=self.time
#self.begin=1
#Set buoyancy force
self.robot.setBuoyancy([0, 0, self._V * self.rho * self.g])
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)
# Do here actuator specific initializations
self.robot = self.robot_parent.bge_object
env = blenderapi.getssr()
self.U = env.get('Current_velocity')
self.pose3d = self.robot_parent.position_3d
self.nu = numpy.matrix([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).T
self.robot_parent.RPM = numpy.matrix([0.0, 0.0, 0.0, 0.0, 0.0])
self.thrust_lim = False
try:
from parameters import p
ThrustParameters = p[self.bge_object.name]
except:
ThrustParameters = {}
if 'nThrusters' in ThrustParameters:
self.nThrusters = int(ThrustParameters['nThrusters'])
else:
self.nThrusters = 1
if 'thruster_diameter' in ThrustParameters:
self.thruster_diameter = numpy.matrix(
ThrustParameters['thruster_diameter'])
else:
self.thruster_diameter = numpy.zeros((self.nThrusters))
if 'RPM_limits' in ThrustParameters:
self.RPM_limits = numpy.matrix(ThrustParameters['RPM_limits'])
else:
self.RPM_limits = numpy.zeros((2, self.nThrusters))
if 'thrust_coeff' in ThrustParameters:
self.thrust_coeff = numpy.matrix(ThrustParameters['thrust_coeff'])
else:
self.thrust_coeff = numpy.zeros((2, 4))
if 'thrustloss_factors' in ThrustParameters:
self.thrustloss_factors = numpy.matrix(
ThrustParameters['thrustloss_factors'])
else:
self.thrustloss_factors = numpy.zeros((2, self.nThrusters))
if 'T' in ThrustParameters:
self.T = numpy.matrix(ThrustParameters['T'])
else:
self.T = numpy.zeros((6, self.nThrusters))
if 'simple' in ThrustParameters:
self.simple = ThrustParameters['simple']
else:
self.simple = False
if 'thrust_lim' in ThrustParameters:
self.f_thrust_lim = numpy.matrix(ThrustParameters['thrust_lim'])
self.r_thrust_lim = -numpy.matrix(ThrustParameters['thrust_lim'])
self.thrust_lim = True
self.thrust = numpy.zeros((self.nThrusters, 1))
self.RPM = numpy.zeros((self.nThrusters, 1))
logger.info('Component initialized')
def default_action(self):
""" Main loop of the actuator.
Implements the component behaviour
"""
# update internal states
self._update_states()
# Store velocity for Thrustforce calculation
self.robot_parent.nu = self.nu
# Relative velocity TODO include current
beta = blenderapi.getssr().get('Current_heading')
NED_current = (
blenderapi.getssr().get('Current_velocity') * numpy.matrix(
[numpy.cos(beta), numpy.sin(beta), 0, 0, 0, 0]).T)
nu_current = self.Rzyx6(self.pose3d.roll, self.pose3d.pitch,
self.pose3d.yaw) * NED_current
self.nu_rel = self.nu - nu_current
###################################################
# Calculate and apply forces in the vehicle frame #
###################################################
localForce = numpy.matrix([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).T
# Restoring moments (forces already taken into account)
localForce -= self._Restoring_moment(self.g, self.rho, self.mass,
self._V, self.r_g, self.r_b,
self.pose3d.roll,
self.pose3d.pitch)
restoring_force_ned = localForce
restoring_force_enu = self._to_enu(restoring_force_ned)
# Linear Damping
Ld = self.D_l * self.nu_rel
localForce -= Ld
# Quadratic damping
Qd = self.D_q * numpy.multiply(
numpy.multiply(self.nu_rel, self.nu_rel), numpy.sign(self.nu_rel))
localForce -= Qd
# Coreolis MOMENT of added mass TODO investigate if this is correct to include
localForce -= self._Coreolis_addedmass(self.A,
self.nu_rel) * self.nu_rel
# Coriolis of MOMENT rigid body TODO
localForce -= self._Coreolis_rigid(self.mass, self.nu, self.r_g,
self.I_g) * self.nu
#Forces in SFU (starboard-forward-up)
localForce = self._to_enu(localForce)
force = localForce[0:3].T.tolist()[0]
torque = localForce[3:6].T.tolist()[0]
#Apply forces
self.robot.applyExternalForce(force, True)
self.robot.applyExternalTorque(torque, True)
