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