class Thrust(morse.core.actuator.Actuator):
_name = "Thrust"
_short_desc = "Thrust actuator for a quadcopter"
add_data('throttle', 0, 'float', 'The throttle, between 0.0 and 1.0 inclusive')
add_property('_number_of_props', 4, 'NumberOfProps', 'int',
'Number of propellors')
add_property('_prop_diameter', 0.1, 'PropDiameter', 'float',
'Diameter of a propellor (in m)')
add_property('_prop_pitch', 0.1, 'PropPitch', 'float',
'Pitch of a propellor (in m)')
add_property('_prop_max_speed', 100.0, 'PropMaxSpeed', 'float',
'Max speed of a propellor (in revs / s)')
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)
def default_action(self):
prop_area = math.pi * (self._prop_diameter / 2)**2
volume_per_rev = self._prop_pitch * prop_area
mass_per_rev = AIR_DENSITY * volume_per_rev
self.local_data['throttle'] = min(1, max(self.local_data['throttle'], 0))
z_velocity = self.robot_parent.bge_object.linearVelocity[2]
prop_speed = self.local_data['throttle'] * self._prop_max_speed
thrust = (prop_speed * self._prop_pitch - z_velocity) * mass_per_rev * prop_speed
thrust *= self._number_of_props
self.robot_parent.bge_object.applyForce((0, 0, thrust), True)
def __init__(self, obj, parent=None):
""" Constructor method
Receives the reference to the Blender object.
Optionally it gets the name of the object's parent,
but that information is not currently used for a robot.
"""
add_property('_speed', 0.0, 'Speed', 'float',
"movement speed of the robot in m/s")
self._speed = 0.0
logger.info('%s initialization' % obj.name)
morse.core.robot.Robot.__init__(self, obj, parent)
# Do here robot specific initializations
logger.info('Component initialized')
#self.module_map = {}
#self.robot_children = {}#[child for child in self.bge_object.childrenRecursive if 'robot.' in child.name]
# for child in self._children:
# if child.name == 'robot.torso':
# self.module_map['robot.torso'] = child
self.func_map = {}
self.func_map['move_forward'] = self
self.func_map['getBbones'] = self
self.func_map['getBoundingBox'] = self
class Saucisse(morse.core.robot.Robot):
add_property(
'_sea_level', 0, 'SeaLevel', 'float',
'Indicates the level of the sea. For example, if the mesh representing the sea is 0.7m above 0, set _sea_level=0'
)
add_property(
'_lever_arm_length', 1, 'LeverArmLength', 'float',
'Distance from a thruster to the center of gravity of the robot. Does not apply to the vertical thruster'
)
add_property('_mf', (0.1**2) * 3.14 * 1 * 1030, 'MF', 'float',
'Volume of the robot. Used to simulate Archimedes force.')
add_property('_radius', 0.1, 'Radius', 'float',
'Radius of the cylinder of the robot in meter.')
_name = 'saucisse robot'
_u1 = 0 # Left thruster, looking forward (positive means turns right)
_u2 = 0 # Right thruster, looking forward (positive means turns left)
_u3 = 0 # Z-thruster, looking up (positive means go up)
##
# sea_level : for example if the map is 1m too high, sea_level = 1
# lever_arm_length : distance between a left/right thruster to the center of mass
# mf : mass of the fluid displaced by the robot, used for Archimedes thrust, defaults to
# the mass displaced by a cylinder of 20cm diameter, 1meter in length with a
# water volumic mass of 1030 kg.m-3
def __init__(self, obj, parent=None):
logger.info('%s initialization' % obj.name)
morse.core.robot.Robot.__init__(self, obj, parent)
logger.info('Component initialized')
print(self._sea_level)
def default_action(self):
force = [self._u1 + self._u2, 0, self._u3]
torque = [
0, 0, self._u2 * self._lever_arm_length -
self._u1 * self._lever_arm_length
]
## Compute an approximation of Archimedes force
# If robot is underwater, apply Archimedes force
# if robot is partially underwater, assumes Archimedes force varies linearly with its immersion
delta = self.position_3d.z - self._sea_level
if delta < self._radius and delta > -self._radius:
h_imm = self._radius - delta
vol_imm = self._radius**2 * acos(
(self._radius - h_imm) / self._radius) - (
self._radius - h_imm) * sqrt(2 * self._radius * h_imm -
h_imm**2) * 1 / (0.1**2 *
3.14 * 1)
force[2] += self.bge_object.mass * 9.81
elif self.position_3d.z - self._sea_level < -self._radius:
force[2] += 1.01 * self.bge_object.mass * 9.81
self.bge_object.applyForce(force, True)
self.bge_object.applyTorque(torque, True)
class Drag(morse.core.actuator.Actuator):
"""
This actuator allows to simulate the drag force, or fluid
resistance. It is not controlable, as it depends only of robot
parameters, and environment.
The drag force is generally written as:
Fd = 0.5 * rho * vel_sq * A * Cd
where
- rho is the density of the fluid (env dependant)
- A is the cross sectional area (robot dependent)
- Cd is the drag coefficient, related to the object shape and
the Reynolds Number
All these values are aggregated into the property DragCoeff, to ease
the configuration of the component.
"""
_name = "Drag"
_short_desc = "Actuator allowing to simulate the drag force"
add_property('_drag_coeff_x', 0.09, 'DragCoeffX', 'float'
'An aggregated value to scale the drag force on X robot axis')
add_property('_drag_coeff_y', 0.09, 'DragCoeffY', 'float'
'An aggregated value to scale the drag force on Y robot axis')
add_property('_drag_coeff_z', 0.5, 'DragCoeffZ', 'float'
'An aggregated value to scale the drag force on Z robot axis')
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)
# Make new reference to the robot velocities (mathutils.Vector)
self.robot_vel = self.robot_parent.bge_object.localLinearVelocity
self._drag_coeff = [self._drag_coeff_x,
self._drag_coeff_y,
self._drag_coeff_z]
logger.info("Component initialized, runs at %.2f Hz ", self.frequency)
def default_action(self):
robot = self.robot_parent
drag = Vector((0.0, 0.0, 0.0))
for i in range(3):
v = self.robot_vel[i]
drag[i] = - copysign(1.0, v) * v * v * self._drag_coeff[i]
logger.debug("Drag force %s" % drag)
robot.bge_object.applyForce(drag, True)
class NoiseModifier(AbstractModifier):
"""
This modifier allows to simulate Gaussian noise for pose measurements.
If the variable ``orientation`` exists, it is taken to be a unit quaternion
and noise added to it. Otherwise rotational noise will be added to the roll,
pitch and yaw variables.
This modifier attempts to alter data ``x``, ``y`` and ``z`` for position,
and either ``orientation`` or ``yaw``, ``pitch`` and ``roll`` for orientation.
The PoseNoise modifier provides as modifiers:
* :py:class:`morse.modifiers.pose_noise.PositionNoiseModifier`
* :py:class:`morse.modifiers.pose_noise.OrientationNoiseModifier`
* :py:class:`morse.modifiers.pose_noise.PoseNoiseModifier`
"""
_name = "PoseNoise"
add_property('_pos_std_dev', {'x': 0.05, 'y': 0.05, 'z': 0.05}, "pos_std", type="dict",
doc="Standard deviation for position noise as dictionary with x,y,z as floats")
add_property('_rot_std_dev', {'roll': radians(5), 'pitch': radians(5), 'yaw': radians(5)},
"rot_std", type="dict",
doc="Standard deviation for rotation noise of roll,pitch,yaw axes as floats in radians")
add_property('_2D', False, "_2D", type="bool",
doc="If True, noise is only applied to 2D pose attributes (i.e., x, y and yaw)")
def initialize(self):
pos_std = self.parameter("pos_std", default=0.05)
if isinstance(pos_std, dict):
self._pos_std_dev = pos_std
else:
self._pos_std_dev = {'x': float(pos_std), 'y': float(pos_std), 'z': float(pos_std)}
rot_std = self.parameter("rot_std", default=radians(5))
if isinstance(rot_std, dict):
self._rot_std_dev = rot_std
else:
self._rot_std_dev = {'roll': float(rot_std), 'pitch': float(rot_std), 'yaw': float(rot_std)}
self._2D = bool(self.parameter("_2D", default=False))
if self._2D:
logger.info("Noise modifier standard deviations: x:%.4f, y:%.4f, yaw:%.3f deg",
self._pos_std_dev.get('x', 0),
self._pos_std_dev.get('y', 0),
degrees(self._rot_std_dev.get('yaw', 0)))
else:
logger.info("Noise modifier standard deviations: x:%.4f, y:%.4f, z:%.4f, "
"roll:%.3f deg, pitch:%.3f deg, yaw:%.3f deg",
self._pos_std_dev.get('x', 0),
self._pos_std_dev.get('y', 0),
self._pos_std_dev.get('z', 0),
degrees(self._rot_std_dev.get('roll', 0)),
degrees(self._rot_std_dev.get('pitch', 0)),
degrees(self._rot_std_dev.get('yaw', 0)))
class VelocityModifier(AbstractModifier):
_name = "VelocityNoise"
add_property(
'_velocity_std_dev',
0.04,
"velocity_std",
type="float",
doc="Standard deviation for noise applied to velocity measurement")
def initialize(self):
"""
This modifier allows to add a Gaussian noise for the velocity measurements
of the Velocity sensor
"""
self._velocity_std_dev = float(
self.parameter("velocity_std", default=0.04))
logger.info("Velocity noise standard deviation: %.4f" %
self._velocity_std_dev)
def modify(self):
try:
for i in range(0, 3):
self.data['linear_velocity'][i]=\
random.gauss(self.data['linear_velocity'][i], self._velocity_std_dev)
self.data['angular_velocity'][i]=\
random.gauss(self.data['angular_velocity'][i], self._velocity_std_dev)
except KeyError as detail:
self.key_error(detail)
class DepthCamera(AbstractDepthCamera):
"""
This sensor generates a 3D point cloud from the camera perspective.
See also :doc:`../sensors/camera` for generic informations about Morse cameras.
"""
_name = "Depth (XYZ) camera"
_short_desc = "A camera capturing 3D points cloud"
add_data(
'points', 'none', 'memoryview', "List of 3D points from the depth "
"camera. memoryview of a set of float(x,y,z). The data is of size "
"``(nb_points * 12)`` bytes (12=3*sizeof(float).")
add_data(
'nb_points', 0, 'int', "the number of points found in the "
"points list. It must be inferior to cam_width * cam_height")
add_property(
'_keep_list', None, 'keep_list', 'list(int)',
"a list of lines number we preserve. Other lines are discared. It allows to "
"better simulate sparse sensor such as velodyne. If not specified, keep the "
"image dense")
add_property(
'_keep_resolution', False, 'keep_resolution', 'boolean',
"By default the clipping of the camera removes unreals points"
"This option allows adding dummy points (0,0,0) for keeping the sensor resolution"
)
def initialize(self):
from morse.sensors.zbufferto3d import ZBufferTo3D
if self._keep_list is None:
keep_list = list(range(self.image_height))
else:
keep_list = eval(self._keep_list)
# Store the camera parameters necessary for image processing
self.converter = ZBufferTo3D(self.local_data['intrinsic_matrix'][0][0],
self.local_data['intrinsic_matrix'][1][1],
self.near_clipping, self.far_clipping,
self.image_width, self.image_height,
self._keep_resolution, keep_list)
def process_image(self, image):
self.pts = self.converter.recover(image)
self.local_data['points'] = self.pts
self.local_data['nb_points'] = int(len(self.pts) / 12)
class Battery(morse.core.sensor.Sensor):
"""
This sensor emulates the remaining charge of a battery on the robot.
It is meant to be used only as an informative measure, to be taken in
consideration by the planning algorithms. It does not prevent the robot
from working.
The charge of the battery decreases with time, using a predefined
**Discharge rate** specified as a property of the Blender object.
This rate is independent of the actions performed by the robot, and
only dependant on the time elapsed since the beginning of the simulation.
If the battery enters in a **Charging zone**, the battery will
gradually recharge.
"""
_name = "Battery Sensor"
add_property('_discharging_rate', 0.05, 'DischargingRate', "float",
"Battery discharging rate, in percent per seconds")
add_data('charge', 100.0, "float", "Initial battery level, in percent")
add_data('status', "Charged", "string", "Charging Status")
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)
self._time = self.robot_parent.gettime()
logger.info('Component initialized, runs at %.2f Hz', self.frequency)
def default_action(self):
""" Main function of this component. """
charge = self.local_data['charge']
dt = self.robot_parent.gettime() - self._time
if self.in_zones(type='Charging'):
charge = charge + dt * self._discharging_rate
status = "Charging"
if charge > 100.0:
charge = 100.0
status = "Charged"
else:
charge = charge - dt * self._discharging_rate
status = "Discharging"
if charge < 0.0:
charge = 0.0
# Store the data acquired by this sensor that could be sent
# via a middleware.
self.local_data['charge'] = float(charge)
self.local_data['status'] = status
# update the current time
self._time = self.robot_parent.gettime()
class Airspeed(Sensor):
"""
Sensor to compute Airspeed
- https://en.wikipedia.org/wiki/Airspeed
The current implementation is only correct for speed < 1Mach
"""
_name = "Airspeed"
_short_desc = "Mesure airspeed"
add_data('diff_pressure', 0.0, "float", 'Diff pressure in Pa')
add_property(
'_type', 'Automatic', 'ComputationMode', 'string',
"Kind of computation, can be one of ['Velocity', 'Position']. "
"Only robot with dynamic and Velocity control can choose Velocity "
"computation. Default choice is Velocity for robot with physics, "
"and Position for others")
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)
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':
self.robot_vel_body = self.robot_parent.bge_object.localLinearVelocity
self.airspeed2body = self.sensor_to_robot_position_3d()
# rotate vector from body to airspeed frame
self.rot_b2a = self.airspeed2body.rotation.conjugated()
else:
self.pp = copy(self.position_3d)
logger.info('Component initialized, runs at %.2f Hz', self.frequency)
def default_action(self):
if self._type == 'Velocity':
vel = self.rot_b2a * self.robot_vel_body
else:
vel = linear_velocities(self.pp, self.position_3d,
1 / self.frequency)
self.pp = copy(self.position_3d)
v_x = vel[0]
self.local_data['diff_pressure'] = 0.5 * AIR_DENSITY * v_x * v_x
class AbstractDepthCamera(VideoCamera):
add_property('near_clipping', 1.0, 'cam_near')
add_property('far_clipping', 20.0, 'cam_far')
add_property('retrieve_depth', True, 'retrieve_depth')
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 VideoCamera class
VideoCamera.__init__(self, obj, parent)
# Component specific initialize (converters)
self.pts = None
self.initialize()
# abstractmethod
def process_image(self, image):
pass
# abstractmethod
def initialize(self):
pass
def default_action(self):
""" Update the texture image. """
# Grab an image from the texture
if self.bge_object['capturing'] and (self._n != 0):
# Call the action of the Camera class
Camera.default_action(self)
self.process_image(self.image_data)
self.capturing = True
if self._n > 0:
self._n -= 1
if self._n == 0:
self.completed(status.SUCCESS)
else:
self.capturing = False
class DepthCameraRotationZ(DepthCamera):
"""Used for Velodyne sensor"""
add_property('rotation', 0.01745, 'rotation')
def default_action(self):
DepthCamera.default_action(self)
self.applyRotationZ(self.rotation)
def applyRotationZ(self, rotation):
# The second parameter specifies a "local" movement
self.bge_object.applyRotation([0, rotation, 0], True)
class MotionXYW(morse.core.actuator.Actuator):
"""
This actuator reads the values of forwards movement x, sidewards
movement y and angular speed w and applies them to the robot as
direct translation. This controller is supposed to be used with
robots that allow for sidewards movements.
"""
_name = 'Linear and angular speed (Vx, Vy, W) actuator'
_short_desc = 'Motion controller using linear and angular speeds'
add_data('x', 0.0, 'float',
'linear velocity in x direction (forward movement) (m/s)')
add_data('y', 0.0, 'float',
'linear velocity in y direction (sidewards movement) (m/s)')
add_data('w', 0.0, 'float', 'angular velocity (rad/s)')
add_property('_type', 'Position', 'ControlType', 'string',
"Kind of control, can be one of ['Velocity', 'Position']")
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.info('Component initialized')
def default_action(self):
""" Apply (x, y, w) to the parent robot. """
# Reset movement variables
vx, vy, vz = 0.0, 0.0, 0.0
rx, ry, rz = 0.0, 0.0, 0.0
# Scale the speeds to the time used by Blender
try:
if self._type == 'Position':
vx = self.local_data['x'] / self.frequency
vy = self.local_data['y'] / self.frequency
rz = self.local_data['w'] / self.frequency
else:
vx = self.local_data['x']
vy = self.local_data['y']
rz = self.local_data['w']
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
self.apply_speed(self._type, [vx, vy, vz], [rx, ry, rz])
class Magnetometer(morse.core.sensor.Sensor):
"""
This sensor computes the magnetic field vector, at the sensor
position. It relies on the WMM2015 model, available
`at NOAA <http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml>`_.
.. warning::
For proper computation, the sensor needs a real position on
Earth, and so the following properties should be added at the
environment level:
- **longitude** in degrees (double) of Blender origin
- **latitude** in degrees (double) of Blender origin
- **altitude** in m of the Blender origin
- optionnaly **angle_against_north** in degrees is the angle
between geographic north and the blender X axis.
**angle_against_north** is positive when the blender X-axis is
east of true north, and negative when it is to the west.
"""
_name = "Magnetometer"
add_data('x', 0.0, "float",
'Northern component of the magnetic field vector, in nT')
add_data('y', 0.0, "float",
'Eastern component of the magnetic field vector, in nT')
add_data('z', 0.0, "float",
'Downward component of the magnetic field vector, in nT')
add_property(
'date', None, 'date', 'float', 'the date used to adjust \
for magnetic field. If not precised, consider the today \
date')
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)
self._mag = MagnetoDriver(self.date)
logger.info('Component initialized, runs at %.2f Hz', self.frequency)
def default_action(self):
mag_field = self._mag.compute(self.position_3d)
self.local_data['x'] = mag_field[0]
self.local_data['y'] = mag_field[1]
self.local_data['z'] = mag_field[2]
class LidarLite(morse.core.sensor.Sensor):
_name = "LidarLite"
_short_desc = "Lidar Lite v2 Rangefinder (pointing in the +x direction)"
add_data('range', float('NaN'), 'float', 'Range reading')
add_property('min_range', 0.0, 'min_range')
add_property('max_range', float('Inf'), 'max_range')
def __init__(self, obj, parent=None):
logger.info("%s initialization" % obj.name)
morse.core.sensor.Sensor.__init__(self, obj, parent)
def default_action(self):
obj = self.bge_object
direction = self.position_3d.matrix * mathutils.Vector([1, 0, 0])
ray_cast = obj.rayCast(direction, None, 100)
if ray_cast[0] is not None:
self.local_data['range'] = round(obj.getDistanceTo(ray_cast[1]), 2)
else:
self.local_data['range'] = float('NaN')
class SegwayRMP400(morse.core.wheeled_robot.MorsePhysicsRobot):
"""
Simple definition of the RMP400 platform distributed by Segway.
This robot uses the Physics Constraints in Blender to allow the wheels to
behave more realistically. The wheels turn as the robot moves, and they have
``Rigid Body`` physics, so that they can also have collisions with nearby
objects.
It has four differential drive wheels, with the pairs of wheels on each side
always moving at the same speed. Since the wheels of this robot use the
``Rigid Body`` physics, it must be controlled with the :doc:`v_omega_diff_drive
<../actuators/v_omega_diff_drive>` actuator.
"""
_name = 'Segway RMP 400 platform'
add_property(
'_fix_turning', 0.0, 'FixTurningSpeed', 'double',
'Overwrite the value of the distance between wheels in '
'the computations of the wheel speeds. This effectively '
'changes the turning speed of the robot, and can be used '
'to compensate for the slip of the wheels while turning. '
'The real distance between wheels in the robot is 0.624m. '
'By forcing a distance of 1.23m, the robot will turn over '
'a smaller radius, as would a two wheeled differential '
'drive robot. If the value 0.0 is used, the real '
'distance between wheels is used.')
def __init__(self, obj, parent=None):
""" Constructor method.
Receives the reference to the Blender object.
Optionally it gets the name of the object's parent,
but that information is not currently used for a robot. """
logger.info('%s initialization' % obj.name)
# Call the constructor of the parent class
super(self.__class__, self).__init__(obj, parent)
if self._fix_turning != 0.0:
self._trackWidth = self._fix_turning
logger.warn("Using wheel separation of %.4f" % self._trackWidth)
logger.info('Component initialized')
def default_action(self):
""" Main function of this component. """
pass
class UTMModifier(AbstractModifier):
"""
This modifier converts the coordinates generated by the MORSE simulator to use
UTM global coordinates. This is achieved by setting the offset of the Blender
origin with respect to the UTM reference. The offset can be either given in
the modifier parameters, or globally defined as three
properties of the ``Scene_Script_Holder`` object of the scene: ``UTMXOffset``,
``UTMYOffset`` and ``UTMZOffset``.
.. note:: Due to limitation in Blender, to pass offset outside of range
[-10000.0, 10000.0] as global scene properties,
you need to pass the offset value as a string.
This modifier attempts to alter data ``x``, ``y`` and ``z`` of modified components.
The UTM modifier provides as modifiers:
* :py:class:`morse.modifiers.utm.CoordinatesToUTM`
* :py:class:`morse.modifiers.utm.CoordinatesFromUTM`
"""
_name = "UTM"
add_property('_x_offset', 0, "x_offset", type = "float", doc = "UTM X Offset")
add_property('_y_offset', 0, "y_offset", type = "float", doc = "UTM Y Offset")
add_property('_z_offset', 0, "z_offset", type = "float", doc = "UTM Z Offset")
def initialize(self):
""" Initialize the UTM coordinates reference.
"""
self._x_offset = float(self.parameter('x_offset', prop='UTMXOffset', default=0))
self._y_offset = float(self.parameter('y_offset', prop='UTMYOffset', default=0))
self._z_offset = float(self.parameter('z_offset', prop='UTMZOffset', default=0))
logger.info("UTM reference point is (%s,%s,%s)"
% (self._x_offset, self._y_offset, self._z_offset))
class ForceTorque(morse.core.actuator.Actuator):
"""
This class will read force and torque as input
from an external middleware.
The forces and torques are transformed from the actuator frame to the
parent robot frame and then applied to the robot blender object.
If the property RobotFrame is set to True it will be applied
directly in the robot frame without changes.
"""
_name = "Force/Torque Motion Controller"
_short_desc = "Motion controller using force and torque"
add_data('force', [0.0, 0.0, 0.0], "vec3<float>", "force along x, y, z")
add_data('torque', [0.0, 0.0, 0.0], "vec3<float>", "torque around x, y, z")
add_property(
'_robot_frame', False, 'RobotFrame', 'bool', 'If set to true '
'the inputs are applied in the Robot coordinate frame instead of the '
'actuator frame.')
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.info('Component initialized')
def default_action(self):
""" Apply (force, torque) to the parent robot. """
# Get the the parent robot
robot = self.robot_parent
if self._robot_frame:
# directly apply local forces and torques to the blender object of the parent robot
robot.bge_object.applyForce(self.local_data['force'], True)
robot.bge_object.applyTorque(self.local_data['torque'], True)
else:
(loc, rot, scale) = robot.position_3d.transformation3d_with(
self.position_3d).matrix.decompose()
# rotate into robot frame, but still at actuator origin
force = rot * mathutils.Vector(self.local_data['force'])
torque = rot * mathutils.Vector(self.local_data['torque'])
# add torque due to lever arm
torque += loc.cross(force)
robot.bge_object.applyForce(force, True)
robot.bge_object.applyTorque(torque, True)
class Lejaune(morse.core.robot.Robot):
add_property('_lever_arm_length',0.3,'LeverArmLength','float','Distance from the center of mass of the boat to the thruster')
_name = 'lejaune robot'
_u1 = 0 # Thruster force, positive means the boat goes forward
_u2 = 0 # Thruster orientation, in degrees. 0 is neutral, 90 means the boat turns right, -90 means the boat turns left (the angle represents the rudder-stick angle with the x-axis of the robot.)
def __init__(self, obj, parent=None):
logger.info('%s initialization' % obj.name)
morse.core.robot.Robot.__init__(self, obj, parent)
# Do here robot specific initializations
logger.info('Component initialized')
def default_action(self):
self.bge_object.applyForce([self._u1*cos(radians(self._u2)),self._u1*sin(radians(self._u2)),0],True)
self.bge_object.applyTorque([0,0,self._u1*sin(radians(self._u2))],True)
class ProximityModifier(AbstractModifier):
_name="ProximityNoise"
add_property('_dist_std_dev', 0.5, "dist_std", type = "float", doc = "Standard deviation for noise applied to distance measurement")
def initialize(self):
"""
This modifier allows to add a Gaussian noise for the distance measurements
of the proximity sensor
"""
self._dist_std_dev = float(self.parameter("dist_std", default=0.5))
logger.info("Pinger Noise standard deviation: %.4f"% self._dist_std_dev)
def modify(self):
try:
for s in self.data['near_objects']:
self.data['near_objects'][s]=\
random.gauss(self.data['near_objects'][s], self._dist_std_dev)
except KeyError as detail:
self.key_error(detail)
class Collision(Sensor):
"""
Sensor to detect objects colliding with the current object,
with more settings than the Touch sensor
"""
_name = "Collision"
_short_desc = "Detect objects colliding with the current object."
add_data('collision', False, "bool",
"objects colliding with the current object")
add_data('objects', "", "string", "A list of colliding objects.")
# These properties are not used directly in the logic, but are used
# in the builder to create the radar properly.
# These value cannot be changed dynamically in bge.
add_property(
'_collision_property', "", 'collision_property', 'string',
'Only look for objects with this property, '
'default "" (all objects)')
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
Sensor.__init__(self, obj, parent)
logger.info('Component initialized, runs at %.2f Hz', self.frequency)
def default_action(self):
""" Is currently in collision """
controller = blenderapi.controller()
sensor = controller.sensors[-1]
# see hitObjectList and hitObject for last collided object(s)
self.local_data['collision'] = sensor.positive
self.local_data['objects'] = ','.join(
[o.name for o in sensor.hitObjectList])
class Echosounder(morse.core.sensor.Sensor):
"""Write here the general documentation of your sensor.
It will appear in the generated online documentation.
"""
_name = "Echosounder"
_short_desc = "Acoustic range sensor to compute distance to seafloor"
add_data('range', 0.0, "float", 'Range measured')
add_property(
'_max_range', 30.0, "MaxRange", "float", "Maximum range distance."
"If nothing is detected, return zero")
def __init__(self, obj, parent=None):
logger.info("%s initialization" % obj.name)
# Call the constructor of the parent class
morse.core.sensor.Sensor.__init__(self, obj, parent)
# Do here sensor specific initializations
self._distance = 0 # dummy internal variable, for testing purposes
logger.info('Component initialized')
def default_action(self):
"""
Send a laser directly underneath and check the position of what it hits.
"""
target = self.position_3d.translation
target[2] -= 1.0
_, point, _ = self.bge_object.rayCast(target, None, self._max_range)
logger.debug("Echosounder points to %s and hits %s" % (target, point))
if point:
self.local_data['range'] = self.bge_object.getDistanceTo(point)
else:
self.local_data['range'] = 0
class Barometer(Sensor):
"""
Sensor to compute the atmopsheric pressure, using the ISA model:
- https://en.wikipedia.org/wiki/International_Standard_Atmosphere
- http://en.wikipedia.org/wiki/Atmospheric_pressure
The current implementation is only correct in the Troposphere, i.e.
for an altitude less than 11000m
"""
_name = "Barometer"
_short_desc = "Mesure the atmospheric pressure"
add_data('pressure', 0.0, "float", 'Pressure in Pa')
add_property(
'_ref_p', 101325, "ReferencePressure", "float",
"Reference pressue, in Pascal. By default, the standard \
pressure at the sea level")
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 default_action(self):
dz = self.position_3d.z - self._ref_z
tmp = 1 - (TEMPERATURE_LAPSE_RATE * dz / SEA_LEVEL_TEMP)
self.local_data['pressure'] = self._ref_p * pow(tmp, self._inv_exp)
class DirectControl(ExternalActuator):
"""
The DirectControl actuator takes some control value, such as engine
power and apply them.
"""
_name = "DirectControl"
_short_desc = ""
add_property(
'cmd_description', [], 'cmd_description', '[string]',
' A short name to describe the channel associated \
to each value of cmd')
add_data('cmd', [], '[float]', 'A list of command')
def __init__(self, obj, parent=None):
logger.info("%s initialization" % obj.name)
# Call the constructor of the parent class
ExternalActuator.__init__(self, obj, parent)
self.cmd_description = self.cmd_description.split(',')
logger.info('Component initialized')
def default_action(self):
logger.debug(self.local_data['cmd'])
class RadarAltimeter(Sensor):
"""
Sensor to compute the distance to the ground
To work properly, the ground and relevant obstacles must be marked as Actor.
"""
_name = "Radar Altimeter"
_short_desc = "Compute the distance to the ground"
add_data('z', 0.0, "float", 'Distance to "ground"')
add_property('_max_range', 30.0, "MaxRange", "float",
"Maximum distance to which ground is detected."
"If nothing is detected, return +infinity")
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)
logger.info('Component initialized, runs at %.2f Hz', self.frequency)
def default_action(self):
"""
Send a laser directly underneath and check the position of what it hits.
"""
target = self.position_3d.translation
target[2] -= 1.0
_, point, _ = self.bge_object.rayCast(target, None, self._max_range)
logger.debug("RadarAltimeter points to %s and hits %s" % (target, point))
if point:
self.local_data['z'] = self.bge_object.getDistanceTo(point)
else:
self.local_data['z'] = float('inf')
class Destination(morse.core.actuator.Actuator):
"""
This actuator reads the coordinates of a destination point, and moves the robot
in a straight line towards the given point, without turning. It provides a
very simplistic movement, and can be used for testing or for robots with
holonomic movement. The speeds provided are internally adjusted to the Blender
time measure.
"""
_name = "Destination"
_short_desc = "Instruct the robot to move towards a given target"
add_data('x', 'current X pos', "float", "X coordinate of the destination")
add_data('y', 'current Y pos', "float", "Y coordinate of the destination")
add_data('z', 'current Z pos', "float", "Z coordinate of the destination")
add_property('_tolerance', 0.5, 'Tolerance')
add_property('_speed', 5.0, 'Speed')
add_property('_type', 'Velocity', 'ControlType', 'string',
"Kind of control, can be one of ['Velocity', 'Position']")
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)
self.destination = self.bge_object.position
#self.local_data['speed'] = 0.0
self.local_data['x'] = self.destination[0]
self.local_data['y'] = self.destination[1]
self.local_data['z'] = self.destination[2]
logger.info('Component initialized')
def default_action(self):
""" Move the object towards the destination. """
parent = self.robot_parent
self.destination = [
self.local_data['x'], self.local_data['y'], self.local_data['z']
]
logger.debug("STRAIGHT GOT DESTINATION: {0}".format(self.destination))
logger.debug("Robot {0} move status: '{1}'".format(
parent.bge_object.name, parent.move_status))
# Vectors returned are already normalised
distance, global_vector, local_vector = self.bge_object.getVectTo(
self.destination)
logger.debug("My position: {0}".format(self.bge_object.position))
logger.debug("GOT DISTANCE: {0}".format(distance))
logger.debug("Global vector: {0}".format(global_vector))
logger.debug("Local vector: {0}".format(local_vector))
if distance > self._tolerance:
# Set the robot status
parent.move_status = "Transit"
# Scale the speeds to the time used by Blender
try:
if self._type == 'Position':
vx = global_vector[0] * self._speed / self.frequency
vy = global_vector[1] * self._speed / self.frequency
vz = global_vector[2] * self._speed / self.frequency
else:
vx = global_vector[0] * self._speed
vy = global_vector[1] * self._speed
vz = global_vector[2] * self._speed
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
# If the target has been reached, change the status
else:
# Reset movement variables
vx, vy, vz = 0.0, 0.0, 0.0
#rx, ry, rz = 0.0, 0.0, 0.0
parent.move_status = "Stop"
logger.debug("TARGET REACHED")
logger.debug("Robot {0} move status: '{1}'".format(
parent.bge_object.name, parent.move_status))
self.robot_parent.apply_speed(self._type, [vx, vy, vz], [0, 0, 0])
class Camera(morse.core.sensor.Sensor):
"""
A generic camera class, which is expected to be used as a base class
for real camera. Concrete instantiation are currently:
- :doc:`video_camera <../sensors/video_camera>`
- :doc:`depth_camera <../sensors/depth_camera>`
- :doc:`semantic_camera <../sensors/semantic_camera>`
.. note::
The streaming of data from this sensor can be toggled off and on by
pressing the SPACE key during the simulation. This will affect all the
video cameras on the scene.
Toggling off the cameras can help make the simulation run faster,
specially when there are several cameras. However, the lack of
data on the stream may cause problems to some middlewares.
"""
_name = "Generic Camera"
_short_desc = "Base class for cameras in MORSE"
# Set the values of image size from the variables
# in the Blender Logic Properties
add_property('image_width', 256, 'cam_width')
add_property('image_height', 256, 'cam_height')
add_property('image_focal', 25.0, 'cam_focal')
add_property('near_clipping', 0.1, 'cam_near')
add_property('far_clipping', 100.0, 'cam_far')
add_property('vertical_flip', False, 'Vertical_Flip')
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(Camera, self).__init__(obj, parent)
# Set the background color of the scene
self.bg_color = [143, 143, 143, 255]
self._texture_ok = False
self._camera_running = False
import camera_scene # file like 'component_config.py' in the .blend
self.scene_name = camera_scene.camera_scene[self.bge_object.name]
blenderapi.add_scene(self.scene_name, overlay=0)
logger.info('Component initialized, runs at %.2f Hz', self.frequency)
def default_action(self):
""" Update the texture image. """
# Configure the texture settings the first time the sensor is called
if not self._texture_ok:
self._texture_ok = True
if blenderapi.isfastmode():
logger.warning("Running in fastmode! No camera support!")
else:
# Prepare the camera object in Blender
self._setup_video_texture()
# Exit if the cameras could not be prepared
if not blenderapi.hascameras():
logger.warning(
"Blender's bge.logic does not have the 'cameras' variable, \
something must have failed when configuring the cameras"
)
else:
self._camera_running = True
if self._camera_running:
# Update all objects pose/orientation before to refresh the image
self._update_scene()
# Call the bge.texture method to refresh the image
blenderapi.cameras()[self.name()].refresh(True)
def _update_pose(self, obj):
copy_pose(self._morse_scene.objects[obj.name], obj)
def _update_scene(self):
for obj in self._scene.objects:
if obj.name != '__default__cam__':
try:
self._update_pose(obj)
except Exception as e:
logger.warning(str(e))
def _setup_video_texture(self):
""" Prepare this camera to use the bge.texture module.
Extract the references to the Blender camera and material where
the images will be rendered.
"""
for child in self.bge_object.children:
# The camera object that will produce the image in Blender
if 'CameraRobot' in child.name:
camera = child
# The object that contains the material where the image is rendered
if 'CameraMesh' in child.name:
screen = child
# Considering it consists of a single mesh
mesh = child.meshes[0]
# Get the material name
for material in mesh.materials:
material_index = material.getMaterialIndex()
mesh_material_name = mesh.getMaterialName(material_index)
if 'MAScreenMat' in mesh_material_name:
material_name = mesh_material_name
try:
logger.debug("\tCAMERA: %s" % camera.name)
logger.debug("\tSCREEN: %s" % screen.name)
logger.debug("\tMATERIAL: %s" % material_name)
except UnboundLocalError:
logger.error("The video camera could not be properly initialized."
"The children object could not be found."
"Best solution is to re-link the camera.")
return False
# Get the reference to the scene
scene_map = blenderapi.get_scene_map()
logger.info("Scene %s from %s" %
(self.scene_name, repr(scene_map.keys())))
self._scene = scene_map[self.scene_name]
self._morse_scene = scene_map['S.MORSE_LOGIC']
# Link the objects using bge.texture
if not blenderapi.hascameras():
blenderapi.initcameras()
mat_id = blenderapi.texture().materialID(screen, material_name)
vt_camera = blenderapi.texture().Texture(screen, mat_id)
vt_camera.source = blenderapi.texture().ImageRender(
self._scene, camera)
# Set the focal length of the camera using the Game Logic Property
camera.lens = self.image_focal
logger.info("\tFocal length of the camera is: %s" % camera.lens)
# Set the clipping distances of the camera using the Game Logic Property
camera.near = self.near_clipping
logger.info("\tNear clipping distance of the camera is: %s" %
camera.near)
camera.far = self.far_clipping
logger.info("\tFar clipping distance of the camera is: %s" %
camera.far)
# Set the background to be used for the render
vt_camera.source.background = self.bg_color
# Define an image size. It must be powers of two. Default 512 * 512
vt_camera.source.capsize = [self.image_width, self.image_height]
logger.info("Camera '%s': Exporting an image of capsize: %s pixels" %
(self.name(), vt_camera.source.capsize))
# Reverse the image (boolean game-property)
vt_camera.source.flip = self.vertical_flip
try:
# Use the Z-Buffer as an image texture for the camera
if 'retrieve_zbuffer' in self.bge_object:
vt_camera.source.zbuff = self.bge_object['retrieve_zbuffer']
except AttributeError as detail:
logger.warn("%s\nPlease use Blender > 2.65 for Z-Buffer support" %
detail)
try:
# Use the Z-Buffer as input with an array of depths
if 'retrieve_depth' in self.bge_object:
vt_camera.source.depth = self.bge_object['retrieve_depth']
except AttributeError as detail:
logger.warn("%s\nPlease use Blender > 2.65 for Z-Buffer support" %
detail)
blenderapi.cameras()[self.name()] = vt_camera
class RotorcraftAttitude(morse.core.actuator.Actuator):
"""
This actuator reads roll,pitch, yaw rate and thrust commands as e.g.
used to manually control a quadrotor via RC or by higher level
control loops. This controller is meant to be used by quadrotors and
similar flying robots with Rigid Body physics in blender. It is a
simple PD-controller which applies torques to the robot to change
and control the attitude. The yaw-rate input is integrated to yield
an absolute yaw setpoint for the controller. Thrust is directly
applied as force in z-direction of the robot.
.. note:: Angle are given in aerospace North East Down convention (NED)
"""
_name = "Rotorcraft attitude motion controller"
_short_desc = "Motion controller using force and torque to achieve desired attitude."
add_data('roll', 0.0, 'float', "roll angle in radians")
add_data('pitch', 0.0, 'float', "pitch angle in radians")
add_data('yaw', 0.0, 'float', "yaw angle in radians")
add_data('thrust', 0.0, 'float', "collective thrust: 0 .. 1 (= 0 .. 100%)")
add_property('_rp_pgain', 100.0, 'RollPitchPgain', 'float',
'proportional gain for roll/pitch control')
add_property('_rp_dgain', 20.0, 'RollPitchDgain', 'float',
'derivative gain for roll/pitch control')
add_property('_yaw_pgain', 16.0, 'YawPgain', 'float'
'proportional gain for yaw control')
add_property('_yaw_dgain', 4.0, 'YawDgain', 'float',
'derivative gain for yaw control')
add_property('_thrust_factor', 40.0, 'ThrustFactor', 'float',
'multiplication factor for applied thrust force in N')
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))
logger.info("Component initialized, runs at %.2f Hz ", self.frequency)
def default_action(self):
""" Run attitude controller and apply resulting force and torque to the parent robot. """
# Get the the parent robot
robot = self.robot_parent
if self.local_data['thrust'] > 0:
# yaw_rate and yaw_setpoint in NED
self.yaw_setpoint += self.local_data['yaw'] / self.frequency
# wrap angle
self.yaw_setpoint = normalise_angle(self.yaw_setpoint)
logger.debug("yaw setpoint: %.3f", degrees(self.yaw_setpoint))
logger.debug("yaw current: %.3f setpoint: %.3f",
-degrees(self.position_3d.yaw),
degrees(self.yaw_setpoint))
# Compute errors
#
# e = att_sp - att = attitude error
# current angles to horizontal plane in NED
roll = self.position_3d.roll
pitch = -self.position_3d.pitch
yaw = -self.position_3d.yaw
roll_err = self.local_data['roll'] - roll
pitch_err = self.local_data['pitch'] - pitch
# wrapped yaw error
yaw_err = normalise_angle(self.yaw_setpoint - yaw)
err = Vector((roll_err, pitch_err, yaw_err))
logger.debug("attitude error: (% .3f % .3f % .3f)",
degrees(err[0]), degrees(err[1]), degrees(err[2]))
# derivative
we = (err - self.prev_err) * self.frequency
logger.debug("yaw rate error: %.3f", we[2])
kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
kd = Vector((self._rp_dgain, self._rp_dgain, self._yaw_dgain))
# torque = self.inertia * (kp * err + kd * we)
t = []
for i in range(3):
t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
# convert to blender frame and scale with thrust
torque = Vector((t[0], -t[1], -t[2])) * self.local_data['thrust']
logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0],
torque[1], torque[2])
force = Vector(
(0.0, 0.0, self.local_data['thrust'] * self._thrust_factor))
logger.debug("applied thrust force: %.3f", force[2])
self.prev_err = err.copy()
else:
force = Vector((0.0, 0.0, 0.0))
torque = Vector((0.0, 0.0, 0.0))
# directly apply local forces and torques to the blender object of the parent robot
robot.bge_object.applyForce(force, True)
robot.bge_object.applyTorque(torque, True)
class Gripper(morse.core.actuator.Actuator):
"""
Actuator capable of grabbing objects marked with the ``Graspable``
Game Property. Currently it only works using services: **grab** and
**release**. When instructed to grab an object, it will check if it
is within range, and if so, will parent the grabbed object to
itself.
.. note::
For objects to be detected and grabbed by the gripper, they must
have the following settings in the **Physics Properties** panel:
- **Actor** must be checked
- **Collision Bounds** must be checked
- **Physics Type** must be ``Rigid Body``
This will work even for Static objects
.. warning::
This actuator does not simulate the physical interaction of the
gripper fingers with the objects it grabs. Its purpose is to
abstract the action of taking an object, for human-robot
interaction experiments.
"""
_name = "Gripper"
_short_desc = "Instruct the robot to move towards a given target"
add_data('grab', False, "bool", "Currently not used")
# These properties are not used directly in the logic, but are used
# in the builder to create the radar properly.
# These value cannot be changed dynamically in bge.
add_property('_angle', 60.0, 'Angle', 'float',
'Aperture angle of the radar capable to detecting the \
graspable objects (in degree)')
add_property('_distance', 0.5, 'Distance', 'float',
'Detection distance in meter. Graspable objects further \
way from the gripper than this distance cannot be \
held')
def __init__(self, obj, parent=None):
"""
Constructor method.
Receives the reference to the Blender object.
"""
logger.info('%s initialization' % obj.name)
# Call the constructor of the parent class
super(self.__class__, self).__init__(obj, parent)
self._near_object = None
self._grabbed_object = None
# Variable to indicate whether the mesh should animate closing
# or opening the gripper
self._animation = ''
# Get references to the Logic Bricks to play animations
self._contr = self.bge_object.controllers[0]
self._close_anim = self._contr.actuators['Close_anim']
self._open_anim = self._contr.actuators['Open_anim']
logger.info('Component initialized')
logger.setLevel(logging.DEBUG)
def find_object(self):
"""
Store the object that is within reach of the gripper Uses a
Blender Radar Sensor to detect objects with the 'Graspable'
property in front of this component
"""
# Get reference to the Radar Blender sensor
contr = blenderapi.controller()
radar = contr.sensors['Radar']
self._near_object = None
if radar.triggered and radar.positive:
min_distance = 100
for test_obj in radar.hitObjectList:
# Find the closest object and its distance
new_distance = self.bge_object.getDistanceTo(test_obj)
if new_distance < min_distance:
self._near_object = test_obj
min_distance = new_distance
@service
def grab(self):
"""
Tries to grab an object close to the gripper.
:returns: if successful (or if an object is already in hand), the
name of the object, else None.
"""
# Check that no other object is being carried
if not self._grabbed_object:
# If the object is draggable
if self._near_object is not None:
logger.debug("Grabbing object: '%s'" % self._near_object)
# Remove Physic simulation
#self._near_object.suspendDynamics()
# Parent the selected object to the gripper
self._grabbed_object = self._near_object
self._grabbed_object.setParent (self.bge_object)
logger.debug("New parent: %s" % self._grabbed_object.parent)
self._near_object = None
# Execute the close grip animation:
self._animation = 'close'
return self._grabbed_object.name
else:
logger.debug("No 'Graspable' object within range of gripper")
return None
else:
logger.debug("Already holding object %s" % self._grabbed_object )
return self._grabbed_object.name
@service
def release(self):
"""
Free the grabbed object.
Let it fall down after resetting its rotation.
Does nothing if no object is held.
:returns: True if an object has been released, else False (if
no object was held).
"""
# Clear the previously selected object, if any
if self._grabbed_object is not None:
logger.debug("Releasing object: '%s'" % self._near_object)
# Remove the parent
self._grabbed_object.removeParent()
# Place the object on the nearest surface
#morse.helpers.place_object.do_place(previous_object)
# Reset rotation of object
#self._grabbed_object.worldOrientation = [0.0, 0.0, 0.0]
# Restore Physics simulation
#previous_object.restoreDynamics()
#previous_object.setLinearVelocity([0, 0, 0])
#previous_object.setAngularVelocity([0, 0, 0])
# Clear the object from dragged status
self._grabbed_object = None
# Execute the open grip animation:
self._animation = 'open'
return True
else:
logger.debug("No object currently being held: nothing to release.")
return False
def default_action(self):
"""
Check if an object is within reach of the hand
"""
self.find_object()
# Play the animations when necessary
if self._animation == 'close':
self._contr.activate(self._close_anim)
self._animation = ''
logger.debug('Playing CLOSE animation')
if self._animation == 'open':
self._contr.activate(self._open_anim)
self._animation = ''
logger.debug('Playing OPEN animation')
class Robot(morse.core.object.Object):
""" Basic Class for all robots
Inherits from the base object class.
"""
add_property(
'_no_gravity', False, 'NoGravity', 'bool',
'Indicate if we want to consider the gravity for this \
robot If true, the behaviour is less realistic as the \
simulator will automatically compensate it. This setting \
is useful for non-realistic model flying or submarine \
robot ')
add_property(
'_is_ground_robot', False, 'GroundRobot', 'bool',
'Indicate if the robot is a ground robot, i.e. \
basically if it has no way to control its position on the \
Z axis')
# Make this an abstract class
__metaclass__ = ABCMeta
def __init__(self, obj, parent=None):
""" Constructor method. """
# Call the constructor of the parent class
morse.core.object.Object.__init__(self, obj, parent)
# Add the variable move_status to the object
self.move_status = "Stop"
# shift against the simulator time (in ms)
self.time_shift = 0.0
self.is_dynamic = bool(self.bge_object.getPhysicsId())
def action(self):
""" Call the regular action function of the component. """
# Update the component's position in the world
self.position_3d.update(self.bge_object)
self.default_action()
def gettime(self):
""" Return the current time, as seen by the robot, in seconds """
return blenderapi.persistantstorage().current_time + self.time_shift
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 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()
class LaserScanner(Sensor):
"""
This is a generic sensor class used to emulate laser range scanners,
including a variety of SICK and Hokuyo sensors.
This sensor works by generating a series of rays in predefined directions, and
then computing whether any active object is found within a certain distance
from the origin of the sensor.
The resolution and detection range can be completely configured using the MORSE
Builder API. This will generate a flat mesh with a semi-circular shape, where
its vertices represent the directions in which the rays of the sensor are cast.
It is also possible to create a sensor with multiple scan layers, such as the
SICK LD-MRS. This is configured using the parameters specified below.
.. note::
Objects in the scene with the **No collision** setting in their Game
properties will not be detected by this sensor
+-----------------------------------------------------------+------------------------------------------------------------------+
| .. figure:: ../../../media/sensors/laserscanners/sick.png | .. figure:: ../../../media/sensors/laserscanners/sick-ld-mrs.png |
| :width: 200 | :width: 200 |
| | |
| SICK LMS500 | SICK LD-MRS |
+-----------------------------------------------------------+------------------------------------------------------------------+
|.. figure:: ../../../media/sensors/laserscanners/hokuyo.png| |
| :width: 200 | |
| | |
| Hokuyo | |
+-----------------------------------------------------------+------------------------------------------------------------------+
LaserScanner with remission values
___________________________________
Remission "is the reflection or scattering of light by a material."
(http://en.wikipedia.org/wiki/Remission_%28spectroscopy%29)
The level "rssi" adds a list of remission values to the LaserScanner. If a ray
during the scan hits an object the rssi-value is the specular intenisty of the
object's material; If it does not hit an object with a material the remission
value is set to 0.
The intensity of the material can be changed in Blender (Property -> Material ->
Specular -> Intensity). The important options are highlighted in the first image.
+---------------------------------------------------------------------------------+
| .. figure:: ../../../media/rssi_blender_intensity_material.png |
| :align: center |
| |
| Specular intensity of a material in Blender |
+---------------------------------------------------------------------------------+
| .. figure:: ../../../media/rssi_laserscanner_example.png |
| :align: center |
| |
| Example of the LaserScanner with remission values |
+---------------------------------------------------------------------------------+
In the second image the sensor is illustrated. Above every box the material
properties and a corresponding excerpt from the socket stream is displayed.
.. note::
The remission values are **not** comparable to any physical remission value
and are **not** calculated. They are just based on a property of a visual effect.
Configuration of the scanning parameters
----------------------------------------
The number and direction of the rays emitted by the sensor is determined by the
vertices of a semi-circle mesh parented to the sensor. The sensor will cast
rays from the center of the sensor in the direction of each of the vertices in
the semi-circle.
Three preconfigured scanners are available: a **SICK LMS500** laser scanner, a
**Hokuyo** and a **SICK LD-MRS**. The example below shows how to add them
in a simulation:
.. code-block:: python
from morse.builder import *
# Append a sick laser
sick = Sick() # range: 30m, field: 180deg, 180 sample points
hokuyo = Hokuyo() # range: 30m, field: 270deg, 1080 sample points
sick_ld_mrs = SickLDMRS() # range: 30m, field 100deg, 4 layers, 400 points per layer
All these default parameters can be changed i(cf *Configuration
parameters* below). An example of how to change the arc object using the
Builder API is show below:
.. code-block:: python
from morse.builder import *
# Append a sick laser
sick = Sick()
sick.properties(resolution = 5)
sick.properties(scan_window = 90)
sick.properties(laser_range = 5.0)
.. note::
In some special cases (like multi-robot setups), you may need to
additionally call ``sick.create_sick_arc()`` after setting your
scanner properties.
The ray will be created from (-window/2) to (+window/2). So the
``range_list`` will contain the range clockwise.
Another example for the SICK LD-MRS:
.. code-block:: python
from morse.builder import *
sick = SickLDMRS()
sick.properties(Visible_arc = True)
sick.properties(resolution = 1.0)
sick.properties(scan_window = 100)
sick.properties(laser_range = 50.0)
sick.properties(layers = 4)
sick.properties(layer_separation = 0.8)
sick.properties(layer_offset = 0.25)
As with any other component, it is possible to adjust the refresh frequency of
the sensor, after it has been defined in the builder script. For example, to
set the frequency to 1 Hz:
.. code-block:: python
sick.frequency(1.0)
"""
_name = "Laser Scanner Sensors"
_short_desc = "Generic laser range sensors"
add_level("raw",
None,
doc="raw laserscanner: \
Laserscan with point_list and range_list",
default=True)
add_level("rssi",
"morse.sensors.laserscanner.RSSILaserScanner",
doc="laserscanner with rssi: \
Laserscan with point_list, range_list and remission_list")
add_data('point_list', [],
"list",
"Array that stores the positions of \
the points found by the laser. The points are given with respect \
to the location of the sensor, and stored as lists of three \
elements. The number of points depends on the geometry of the arc \
parented to the sensor (see below). The point (0, 0, 0) means that\
this ray has not it anything in its range",
level=["raw", "rssi"])
add_data('range_list', [],
"list",
"Array that stores the distance to the \
first obstacle detected by each ray. The order indexing of this \
array is the same as for point_list, so that the element in the \
same index of both lists will correspond to the measures for the \
same ray. If the ray does not hit anything in its range it returns \
laser_range",
level=["raw", "rssi"])
add_data('remission_list', [],
"list",
"Array that stores the remission \
value for the points found by the laser. The specular intensity \
is set as the remission value. If no object is hit, the remission \
value is set to 0",
level="rssi")
add_property(
'laser_range', 30.0, 'laser_range', "float",
"The distance in meters from the center of the sensor to which\
it is capable of detecting other objects.")
add_property(
'resolution', 1.0, 'resolution', "float",
"The angle between each laser in the sensor. Expressed in \
degrees in decimal format. (i. e.), half a degree is \
expressed as 0.5. Used when creating the arc object.")
add_property(
'scan_window', 180.0, 'scan_window', "float",
"The full angle covered by the sensor. Expressed in degrees \
in decimal format. Used when creating the arc object.")
add_property('visible_arc', False, 'Visible_arc', "boolean",
"if the laser arc should be displayed during the simulation")
add_property('layers', 1, 'layers', "integer",
"Number of scanning planes used by the sensor.")
add_property('layer_separation', 0.8, 'layer_separation', "float",
"The angular distance between the planes, in degrees.")
add_property(
'layer_offset', 0.125, 'layer_offset', "float",
"The horizontal distance between the scan points in \
consecutive scanning layers. Must be given in degrees.")
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
Sensor.__init__(self, obj, parent)
arc_prefix = 'Arc_'
# Look for a child arc to use for the scans
for child in obj.children:
if arc_prefix in child.name:
self._ray_arc = child
logger.info("Sick: Using arc object: '%s'" % self._ray_arc)
break
# Set its visibility, according to the settings
self._ray_arc.setVisible(self.visible_arc)
self._ray_list = []
# Create an empty list to store the intersection points
self.local_data['point_list'] = []
self.local_data['range_list'] = []
# Get the datablock of the arc, to extract its vertices
ray_object = blenderapi.objectdata(self._ray_arc.name)
for vertex in ray_object.data.vertices:
logger.debug("Vertex %d = %s" % (vertex.index, vertex.co))
# Skip the first vertex.
# It is the one located at the center of the sensor
if vertex.index == 0:
continue
# Store the position of the vertex in a list
# The position is already given as a mathutils.Vector
self._ray_list.append(vertex.co)
# Insert empty points into the data list
self.local_data['point_list'].append([0.0, 0.0, 0.0])
# Insert zeros into the range list
self.local_data['range_list'].append(0.0)
logger.debug("RAY %d = [%.4f, %.4f, %.4f]" %
(vertex.index, self._ray_list[vertex.index - 1][0],
self._ray_list[vertex.index - 1][1],
self._ray_list[vertex.index - 1][2]))
# Get some information to be able to deform the arcs
if self.visible_arc:
self._layers = 1
if 'layers' in self.bge_object:
self._layers = self.bge_object['layers']
self._vertex_per_layer = len(self._ray_list) // self._layers
logger.info('Component initialized, runs at %.2f Hz', self.frequency)
def default_action(self):
"""
Do ray tracing from the SICK object using a semicircle
Generates a list of lists, with the points located.
Also deforms the geometry of the arc associated to the SICK,
as a way to display the results obtained.
"""
#logger.debug("ARC POSITION: [%.4f, %.4f, %.4f]" %
# (self.bge_object.position[0],
# self.bge_object.position[1],
# self.bge_object.position[2]))
# Get the inverse of the transformation matrix
inverse = self.position_3d.matrix.inverted()
index = 0
for ray in self._ray_list:
# Transform the ray to the current position and rotation
# of the sensor
correct_ray = self.position_3d.matrix * ray
# Shoot a ray towards the target
target, point, normal = self.bge_object.rayCast(
correct_ray, None, self.laser_range)
#logger.debug("\tTarget, point, normal: %s, %s, %s" %
# (target, point, normal))
# Register when an intersection occurred
if target:
distance = self.bge_object.getDistanceTo(point)
# Return the point to the reference of the sensor
new_point = inverse * point
#logger.debug("\t\tGOT INTERSECTION WITH RAY: [%.4f, %.4f, %.4f]" % (correct_ray[0], correct_ray[1], correct_ray[2]))
#logger.debug("\t\tINTERSECTION AT: [%.4f, %.4f, %.4f] = %s" % (point[0], point[1], point[2], target))
# If there was no intersection, store the default values
else:
distance = self.laser_range
new_point = [0.0, 0.0, 0.0]
# Save the information gathered
self.local_data['point_list'][index] = new_point[:]
self.local_data['range_list'][index] = distance
index += 1
self.change_arc()
def change_arc(self):
# Change the shape of the arc to show what the sensor detects
# Display only for 1 layer scanner
if (2, 65, 0) < blenderapi.version() <= (2, 66, 3):
# see http://projects.blender.org/tracker/?func=detail&aid=34550
return # not supported in 2.66 due to BGE bug #34550
# TODO rework the LDMRS (3 layers) display [code in 1.0-beta2]
if self.visible_arc and self._layers == 1:
for mesh in self._ray_arc.meshes:
for m_index in range(len(mesh.materials)):
# Skip the first vertex (located at the center of the sensor)
for v_index in range(1,
mesh.getVertexArrayLength(m_index)):
vertex = mesh.getVertex(m_index, v_index)
point = self.local_data['point_list'][v_index - 1]
if point == [0.0, 0.0, 0.0]:
# If there was no intersection, move the vertex
# to the laser range
point = self._ray_list[v_index -
1] * self.laser_range
vertex.setXYZ(point[:3])
class MotionVW(morse.core.actuator.Actuator):
"""
This actuator reads the values of linear and angular speed and
applies them to the robot as direct translation. The speeds provided
are internally adjusted to the Blender time measure.
"""
_name = 'Linear and angular speed (V, W) actuator'
_short_desc = 'Motion controller using linear and angular speeds'
add_data('v', 0.0, 'float',
'linear velocity in x direction (forward movement) (m/s)')
add_data('w', 0.0, 'float', 'angular velocity (rad/s)')
add_property('_type', 'Position', 'ControlType', 'string',
"Kind of control, can be one of ['Velocity', 'Position']")
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.info('Component initialized')
@service
def set_speed(self, v, w):
"""
Modifies v and w according to the parameters
:param v: desired linear velocity (meter by second)
:param w: desired angular velocity (radian by second)
"""
self.local_data['v'] = v
self.local_data['w'] = w
@service
def stop(self):
"""
Stop the robot
Internally, it sets (v, w) to (0.0, 0.0)
"""
self.local_data['v'] = 0.0
self.local_data['w'] = 0.0
def default_action(self):
""" Apply (v, w) to the parent robot. """
# Reset movement variables
vx, vy, vz = 0.0, 0.0, 0.0
rx, ry, rz = 0.0, 0.0, 0.0
# Scale the speeds to the time used by Blender
try:
if self._type == 'Position':
vx = self.local_data['v'] / self.frequency
rz = self.local_data['w'] / self.frequency
elif self._type == 'Velocity':
vx = self.local_data['v']
rz = self.local_data['w']
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
self.apply_speed(self._type, [vx, vy, vz], [rx, ry, rz])
