def apply_speed(self, kind, linear_speed, angular_speed): """ Apply speed parameter to the robot :param string kind: the kind of control to apply. Can be one of ['Position', 'Velocity']. :param list linear_speed: the list of linear speed to apply, for each axis, in m/s. :param list angular_speed: the list of angular speed to apply, for each axis, in rad/s. """ parent = self.bge_object must_fight_against_gravity = self.is_dynamic and self._no_gravity if must_fight_against_gravity: parent.applyForce(-blenderapi.gravity()) if kind == 'Position': if must_fight_against_gravity: parent.worldLinearVelocity = [0.0, 0.0, 0.0] parent.applyMovement(linear_speed, True) parent.applyRotation(angular_speed, True) elif kind == 'Velocity': if self._is_ground_robot: linear_speed[2] = parent.localLinearVelocity[2] # Workaround against 'strange behaviour' for robot with # 'Dynamic' Physics Controller. [0.0, 0.0, 0.0] seems to be # considered in a special way, i.e. is basically ignored. # Setting it to 1e-6 instead of 0.0 seems to do the trick! if angular_speed == [0.0, 0.0, 0.0]: angular_speed = [0.0, 0.0, 1e-6] parent.setLinearVelocity(linear_speed, True) parent.setAngularVelocity(angular_speed, True)
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 self.robot_vel = self.robot_parent.bge_object.worldLinearVelocity else: # reference to sensor position self.pos = self.bge_object.worldPosition # previous position self.pp = self.pos.copy() # previous attitude euler angles as vector self.patt = mathutils.Vector(self.position_3d.euler) # previous linear velocity self.plv = mathutils.Vector((0.0, 0.0, 0.0)) # previous angular velocity self.pav = mathutils.Vector((0.0, 0.0, 0.0)) self.gravity = - blenderapi.gravity() # 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.debug("imu2body rotation RPY [% .3f % .3f % .3f]" % tuple(math.degrees(a) for a in self.imu2body.euler)) logger.debug("imu2body translation [% .3f % .3f % .3f]" % tuple(self.imu2body.translation)) if self.imu2body.translation.length > 0.01: self.compute_offset_acceleration = True else: self.compute_offset_acceleration = False # reference for rotating a vector from imu frame to world frame self.rot_i2w = self.bge_object.worldOrientation self.mag = MagnetoDriver() logger.info("IMU 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) # Get every bge object in the scene objs = blenderapi.scene().objects # Get the water surface object self._water = objs['water'] # Set the water surface property self._water['castable'] = True # Initialise all buoyancy spheres in the scene self.spheres = [ Spheres(child) for child in objs if 'float' in child.name.lower() ] # Sphere volumes by parent sphere_vols = {} # Sum sphere volumes for s in self.spheres: parent = s.obj.parent if not parent.name in sphere_vols: sphere_vols[parent.name] = s.vol else: sphere_vols[parent.name] += s.vol # Distribute buoyancy among # spheres according to volume for s in self.spheres: # Parent object parent = s.obj.parent # Parent mass mass = parent.mass # Parent trim trim = parent.get('trim', 0) # The total buoyancy force will equal the weight # in air of the flotation elements plus the trim mass += trim # Buoyancy force when totally submerged # Vector points up in the global frame buoyancy = -mass * blenderapi.gravity() # Buoyancy for this sphere s.buoy = buoyancy * s.vol / sphere_vols[parent.name] logger.info('Found %d buoyancy elements in scene' % len(self.spheres)) logger.info('Component initialized')
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 self.robot_vel = self.robot_parent.bge_object.worldLinearVelocity else: self.pp = copy(self.position_3d) # previous linear velocity self.plv = mathutils.Vector((0.0, 0.0, 0.0)) # previous angular velocity self.pav = mathutils.Vector((0.0, 0.0, 0.0)) self.gravity = -blenderapi.gravity() # 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.debug("imu2body rotation RPY [% .3f % .3f % .3f]" % tuple(math.degrees(a) for a in self.imu2body.euler)) logger.debug("imu2body translation [% .3f % .3f % .3f]" % tuple(self.imu2body.translation)) if self.imu2body.translation.length > 0.01: self.compute_offset_acceleration = True else: self.compute_offset_acceleration = False # reference for rotating a vector from imu frame to world frame self.rot_i2w = self.bge_object.worldOrientation self.mag = MagnetoDriver() logger.info("IMU Component initialized, runs at %.2f Hz ", self.frequency)
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) # The robot needs a physics controller! # Since the imu does not have physics, self.has_physics = bool(self.robot_parent.bge_object.getPhysicsId()) if not self.has_physics: logger.warning("The robot doesn't have a physics controller," "falling back to simple IMU sensor.") if self.has_physics: # make new references to the robot velocities and use those. self.robot_w = self.robot_parent.bge_object.localAngularVelocity self.robot_vel = self.robot_parent.bge_object.worldLinearVelocity else: # reference to sensor position self.pos = self.bge_object.worldPosition # previous position self.pp = self.pos.copy() # previous attitude euler angles as vector self.patt = mathutils.Vector(self.position_3d.euler) # previous linear velocity self.plv = mathutils.Vector((0.0, 0.0, 0.0)) # previous angular velocity self.pav = mathutils.Vector((0.0, 0.0, 0.0)) self.gravity = -blenderapi.gravity() # 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.debug("imu2body rotation RPY [% .3f % .3f % .3f]" % tuple(math.degrees(a) for a in self.imu2body.euler)) logger.debug("imu2body translation [% .3f % .3f % .3f]" % tuple(self.imu2body.translation)) if self.imu2body.translation.length > 0.01: self.compute_offset_acceleration = True else: self.compute_offset_acceleration = False # reference for rotating a vector from imu frame to world frame self.rot_i2w = self.bge_object.worldOrientation logger.info("IMU Component initialized, runs at %.2f Hz ", self.frequency)
def __init__(self, obj, parent=None): """ Constructor method. """ logger.info('%s initialization' % obj.name) # Call the constructor of the parent class Sensor.__init__(self, obj, parent) self._inv_exp = (- blenderapi.gravity()[2] * MOLAR_MASS) / \ (GAS_CONSTANT * TEMPERATURE_LAPSE_RATE) self._ref_z = self.position_3d.z logger.info('Component initialized, runs at %.2f Hz', self.frequency)
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 super(self.__class__, self).__init__(obj, parent) # The robot needs a physics controller! # Since the imu does not have physics, self.has_physics = bool(self.robot_parent.bge_object.getPhysicsId()) if not self.has_physics: logger.warning("The robot doesn't have a physics controller," "falling back to simple IMU sensor.") if self.has_physics: # make new references to the robot velocities and use those. self.robot_w = self.robot_parent.bge_object.localAngularVelocity self.robot_vel = self.robot_parent.bge_object.worldLinearVelocity else: # reference to sensor position self.pos = self.bge_object.worldPosition # previous position self.pp = self.pos.copy() # previous attitude euler angles as vector self.patt = mathutils.Vector(self.position_3d.euler) # previous linear velocity self.plv = mathutils.Vector((0.0, 0.0, 0.0)) # previous angular velocity self.pav = mathutils.Vector((0.0, 0.0, 0.0)) self.gravity = - blenderapi.gravity() # 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.debug("imu2body rotation RPY [% .3f % .3f % .3f]" % tuple(math.degrees(a) for a in self.imu2body.euler)) logger.debug("imu2body translation [% .3f % .3f % .3f]" % tuple(self.imu2body.translation)) if self.imu2body.translation.length > 0.01: self.compute_offset_acceleration = True else: self.compute_offset_acceleration = False # reference for rotating a vector from imu frame to world frame self.rot_i2w = self.bge_object.worldOrientation logger.info("IMU Component initialized, runs at %.2f Hz ", self.frequency)
def apply_speed(self, kind, linear_speed, angular_speed): """ Apply speed parameter to the robot :param string kind: the kind of control to apply. Can be one of ['Position', 'Velocity']. :param list linear_speed: the list of linear speed to apply, for each axis, in m/s. :param list angular_speed: the list of angular speed to apply, for each axis, in rad/s. """ parent = self.bge_object must_fight_against_gravity = self.is_dynamic and self._no_gravity if must_fight_against_gravity: parent.applyForce(-blenderapi.gravity()) if kind == 'Position': if must_fight_against_gravity: parent.worldLinearVelocity = [0.0, 0.0, 0.0] parent.applyMovement(linear_speed, True) parent.applyRotation(angular_speed, True) elif kind == 'Velocity': if self._is_ground_robot: """ A ground robot cannot control its vz not rx, ry speed in the world frame. So convert {linear, angular}_speed in world frame, remove uncontrolable part and then pass it against in the robot frame""" linear_speed = mathutils.Vector(linear_speed) angular_speed = mathutils.Vector(angular_speed) linear_speed.rotate(parent.worldOrientation) angular_speed.rotate(parent.worldOrientation) linear_speed[2] = parent.worldLinearVelocity[2] angular_speed[0] = parent.worldAngularVelocity[0] angular_speed[1] = parent.worldAngularVelocity[1] linear_speed.rotate(parent.worldOrientation.transposed()) angular_speed.rotate(parent.worldOrientation.transposed()) # Workaround against 'strange behaviour' for robot with # 'Dynamic' Physics Controller. [0.0, 0.0, 0.0] seems to be # considered in a special way, i.e. is basically ignored. # Setting it to 1e-6 instead of 0.0 seems to do the trick! if angular_speed == [0.0, 0.0, 0.0]: angular_speed = [0.0, 0.0, 1e-6] parent.setLinearVelocity(linear_speed, True) parent.setAngularVelocity(angular_speed, True)
def force_pose(self, position, orientation): parent = self.bge_object if self.is_dynamic: parent.applyForce(-blenderapi.gravity()) parent.worldLinearVelocity = [0.0, 0.0, 0.0] parent.suspendDynamics() if position: parent.worldPosition = position if orientation: parent.worldOrientation = orientation if self.is_dynamic: parent.restoreDynamics()
def __init__(self, obj, parent=None): logger.info('%s initialization' % obj.name) # Call the constructor of the parent class super(self.__class__, self).__init__(obj, parent) logger.setLevel(logging.INFO) robot_obj = self.robot_parent.bge_object # set desired velocity to zero self.local_data['vx'] = 0.0 self.local_data['vy'] = 0.0 self.local_data['vz'] = 0.0 self.local_data['vyaw'] = 0.0 # get the robot inertia (list [ix, iy, iz]) robot_inertia = robot_obj.localInertia self.inertia = Vector(tuple(robot_inertia)) logger.info("robot inertia: (%.3f %.3f %.3f)" % tuple(self.inertia)) self.nominal_thrust = robot_obj.mass * abs(blenderapi.gravity()[2]) logger.info("nominal thrust: %.3f", self.nominal_thrust) self._attitude_compensation_limit = cos(self._max_bank_angle)**2 # current attitude setpoints in radians self.roll_setpoint = 0.0 self.pitch_setpoint = 0.0 self.yaw_setpoint = 0.0 self.thrust = 0.0 self._prev_vel_blender = Vector((0.0, 0.0, 0.0)) self._prev_yaw = 0.0 # previous attitude error self._prev_err = Vector((0.0, 0.0, 0.0)) logger.info("Component initialized, runs at %.2f Hz ", self.frequency)