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