def get_object_global_bbox(self, object_name):
""" Returns the global bounding box of an object as list encapsulated as
string: "[[x0, y0, z0 ], ... ,[x7, y7, z7]]".
:param string object_name: The name of the object.
"""
# Test whether the object exists in the scene
b_obj = get_obj_by_name(object_name)
# Get bounding box of object
bb = blenderapi.objectdata(object_name).bound_box
# Group x,y,z-coordinates as lists
bbox_local = [[bb_corner[i] for i in range(3)] for bb_corner in bb]
world_pos = b_obj.worldPosition
world_ori = b_obj.worldOrientation.to_3x3()
bbox_global = []
for corner in bbox_local:
vec = world_ori * mathutils.Vector(corner) + \
mathutils.Vector(world_pos)
bbox_global.append([vec.x, vec.y, vec.z])
return json.dumps(bbox_global)
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))
# get gravity from scene?
#g = bpy.data.scenes[0].game_settings.physics_gravity
g = 9.81
self.gravity = mathutils.Vector((0.0, 0.0, g))
# 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.
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 _store_joint_position(self, joints, ik_target, joint_index):
joint_position = joints[joint_index].position
# Convert the GEN_POINT_3D into a Blender vector
position_vector = mathutils.Vector([joint_position.x, joint_position.y, joint_position.z])
if transformation_matrix:
#logger.error("Kinect position: [%.4f, %.4f, %.4f]" % (transformation_matrix[0][3], transformation_matrix[1][3], transformation_matrix[2][3]))
new_position = position_vector * transformation_matrix
new_position = new_position + mathutils.Vector([transformation_matrix[0][3], transformation_matrix[1][3], transformation_matrix[2][3]])
else:
new_position = position_vector
self.data[ik_target] = new_position
def get_wheels(self):
# get pointers to and physicsIds of all objects
# get wheel pointers - needed by wheel speed sensors and to
# set up constraints
# bullet vehicles always have 4 wheels
scene = blenderapi.scene()
self._wheel_radius = None
caster_wheel_name = self.bge_object.get('CasterWheelName', None)
# inherited from the parent robot
for index in self._wheel_index:
name = "Wheel%sName" % index
# Get the actual name of the object from the properties
# of the parent robot
try:
wheel = scene.objects[self.bge_object[name]]
except:
#import traceback
#traceback._exc()
wheel = None
if wheel:
self._wheels[index] = wheel
logger.info("\tWheel %s: %s" % (index, wheel.name))
self._wheel_positions[index] = \
mathutils.Vector(wheel.worldPosition)
self._wheel_orientations[index] = \
mathutils.Matrix(wheel.worldOrientation)
# Make the wheels orphans
wheel.removeParent()
# Keep their transformations
#wheel.worldPosition = self._wheel_positions[index]
#wheel.worldOrientation = self._wheel_orientations[index]
# get wheel radius if not already computed
if wheel.name != caster_wheel_name and not self._wheel_radius:
self._wheel_radius = self.get_wheel_radius(
self.bge_object[name])
logger.debug("get_wheels %s" % self._wheels)
# Add a free rotating wheel if indicated in the robot
if caster_wheel_name and caster_wheel_name != 'None':
wheel = scene.objects[caster_wheel_name]
wheel_position = mathutils.Vector(wheel.worldPosition)
self.attach_caster_wheel_to_body(wheel, self.bge_object,
wheel_position)
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 sim_imu_simple(self):
"""
Simulate angular velocity and linear acceleration measurements via simple differences.
"""
# Compute the differences with the previous loop
#dp = self.pos - self.pp
#deuler = mathutils.Vector(self.position_3d.euler - self.peuler)
# linear and angular velocities
lin_vel = (self.pos - self.pp) * self.frequency
att = mathutils.Vector(self.position_3d.euler)
ang_vel = (att - self.patt) * self.frequency
# linear acceleration in imu frame
dv_imu = self.rot_w2i.transposed() * (lin_vel - self.plv) * self.frequency
# measurement includes gravity and acceleration
accel_meas = dv_imu + self.rot_w2i.transposed() * self.gravity
# save current position and attitude for next step
self.pp = self.pos.copy()
self.peuler = att
# save velocity for next step
self.plv = lin_vel
self.pav = ang_vel
return (ang_vel, accel_meas)
def __init__(self, obj):
self.name = obj.name
self.type = obj['Type']
self.obj = obj
mesh = obj.meshes[0]
# Get list of unique vertexes
vertexes_ = set()
for v_index in range(24):
vertex = mesh.getVertex(0, v_index)
vertexes_.add(vertex.getXYZ()[:])
vertexes = [mathutils.Vector(p) for p in vertexes_]
# Compute range
self._min_values = [float_info.max, float_info.max, float_info.max]
self._max_values = [-float_info.max, -float_info.max, -float_info.max]
for i in range(len(vertexes)):
for j in range(3):
# XXX Here, we assume there is no rotation
vertexes[i][j] = vertexes[i][j] * obj.worldScale[j] + obj.worldPosition[j]
if self._min_values[j] > vertexes[i][j]:
self._min_values[j] = vertexes[i][j]
if self._max_values[j] < vertexes[i][j]:
self._max_values[j] = vertexes[i][j]
def transform_to_obj_frame(self, object_name, point):
""" Transforms a 3D point with respect to the origin into the coordinate
frame of an object and returns the global coordinates.
:param string object_name: The name of the object.
:param string point: coordinates as a list "[x, y, z]"
"""
# Test whether the object exists in the scene
b_obj = get_obj_by_name(object_name)
world_pos = b_obj.worldPosition
world_ori = b_obj.worldOrientation.to_3x3()
pos = world_ori * mathutils.Vector(json.loads(point)) + \
mathutils.Vector(world_pos)
return [pos.x, pos.y, pos.z]
def compute(self, pose):
pos = numpy.matrix(pose.translation)
pos_lla = self._coord_conv.ltp_to_geodetic(pos)
(decl, incl, f, h, x, y,
z) = self._mag.compute(degrees(pos_lla[0, 0]), degrees(pos_lla[0, 1]),
pos_lla[0, 2] / 1000.0, self._date)
mag_field = mathutils.Vector((x, y, z))
return mag_field * pose.rotation_matrix
def force_pose(self, position, orientation):
me = self.aruco['aruco']
parent = self.aruco['parent']
pose3d = parent.position_3d
parent_pose = mathutils.Vector((pose3d.x, pose3d.y, pose3d.z))
if position:
me.worldPosition = position + parent_pose
if orientation:
me.worldOrientation = orientation
def set_gravity(self, gravity=9.81):
""" Set the gravity for the specific scene
:param gravity: float, default: 9.81
"""
if isinstance(gravity, float):
bpymorse.get_context_scene(
).game_settings.physics_gravity = gravity
bpymorse.get_context_scene().gravity = mathutils.Vector(
(0.0, 0.0, -gravity))
def sim_attitude_simple(self):
"""
Simulate angular velocity measurements via simple differences.
"""
# linear and angular velocities
att = mathutils.Vector(self.position_3d.euler)
ang_vel = (att - self.patt) * self.frequency
self.patt = att
return ang_vel
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)
def gravity():
if not fake:
sce = scene()
# All supported version of blender do not support it, so well
# guess if we don't have the support
if hasattr(sce, 'gravity'):
return sce.gravity
else:
return mathutils.Vector((0.0, 0.0, -9.81))
else:
return None
def default_action(self):
position = mathutils.Vector( (self.local_data['z']+self._xoffset,
self.local_data['x']+self._yoffset,
(-1.0*self.local_data['y']+self._zoffset)) )
orientation = mathutils.Euler([ self.local_data['roll' ],
self.local_data['pitch'],
self.local_data['yaw' ] ])
""" Convert Euler to Matrix, worldOrientation accepts Quat, Mat """
orientation.order = "YZX"
orientation_mat = orientation.to_matrix()
self.force_pose(position, orientation_mat)
def default_action(self):
''' Change the parent robot pose. '''
# New parent position
position = mathutils.Vector(
(self.local_data['x'], self.local_data['y'], self.local_data['z']))
# New parent orientation
phi = self.local_data['phi']
theta = self.local_data['theta']
psi = self.local_data['psi']
rotation_matrix = self.get_taitbryan_xyz(phi, theta, psi)
self.robot_parent.force_pose(position, rotation_matrix)
def default_action(self):
""" Change the parent robot pose. """
# New parent position
position = mathutils.Vector(
(self.local_data['x'], self.local_data['y'], self.local_data['z']))
# New parent orientation
orientation = mathutils.Euler([
self.local_data['roll'], self.local_data['pitch'],
self.local_data['yaw']
])
self.robot_parent.force_pose(position, orientation.to_matrix())
def default_action(self):
""" Apply ... to the parent robot. """
# Ticks
dt = 1.0 / self.frequency
# Compute height and Euler Angles
self.f_phi.simulate(self.local_data['phi_c'], dt)
self.f_theta.simulate(self.local_data['theta_c'], dt)
self.f_psi.simulate(self.f_psi.x[0]-self.local_data['psi_c'], dt)
self.f_h.simulate(self.local_data['h_c'], dt)
rot = mathutils.Euler([self.f_phi.x[0], self.f_theta.x[0],
self.f_psi.x[0]])
# Get the parent
parent = self.robot_parent.bge_object
# Compute acceleration
# get previous height and vert. velocity
main_to_origin = self.robot_parent.position_3d
main_to_origin.update(parent)
h = main_to_origin.z
# assume velocity
acc_b = mathutils.Vector([0.0, 0.0,
10.0 + 9.0 * (self.local_data['h_c'] - h) -
2.0 * 0.7 * 3.0 * self.v[2]])
acc_i = mathutils.Vector( rot.to_matrix()*acc_b )
self.v[0] += dt * acc_i[0]
self.v[1] += dt * acc_i[1]
self.v[2] += dt * (acc_i[2] - 10.0)
#Change the parent position
prev_pos = parent.localPosition
parent.localPosition = mathutils.Vector(prev_pos +
dt * mathutils.Vector([self.v[0],self.v[1],self.v[2]]))
#Change the parent orientation
parent.orientation = rot.to_matrix()
def angular_velocities(prev, now, dt):
"""
Return angular velocities between two poses
:param prev: the precedent pose, as a Transformation3d
:param now: the current pose, as a Transformation3d
:param dt: time elapsed between the two poses acquisition, in sec
See https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf
for equation description
"""
patt = mathutils.Vector(prev.euler)
att = mathutils.Vector(now.euler)
euler_rate = (att - patt) / dt
c0 = cos(att[0])
c1 = cos(att[1])
s0 = sin(att[0])
s1 = sin(att[1])
m = mathutils.Matrix(([1, 0, -s1], [0, c0, c1 * s0], [0, -s0, c1 * c0]))
return m * euler_rate
def default_action(self):
""" Change the parent robot pose. """
# New parent position
position = mathutils.Vector(
(self.local_data['x'], self.local_data['y'], self.local_data['z']))
# New parent orientation
orientation = mathutils.Euler([
self.local_data['roll'], self.local_data['pitch'],
self.local_data['yaw']
])
world2actuator = Transformation3d(None)
world2actuator.translation = position
world2actuator.rotation = orientation
(loc, rot,
_) = (world2actuator.matrix * self.actuator2robot.matrix).decompose()
self.robot_parent.force_pose(loc, rot)
def default_action(self):
""" Change the parent robot pose. """
# Get the Blender object of the parent robot
parent = self.robot_parent.bge_object
# New parent position
position = mathutils.Vector((self.local_data['x'], \
self.local_data['y'], \
self.local_data['z']))
# New parent orientation
orientation = mathutils.Euler([self.local_data['roll'], \
self.local_data['pitch'], \
self.local_data['yaw']])
# Suspend Bullet physics engine, which doesnt like teleport
# or instant rotation done by the Orientation actuator (avoid tilt)
parent.suspendDynamics()
parent.worldPosition = position
parent.worldOrientation = orientation.to_matrix()
parent.restoreDynamics()
def convert_LTP_to_ECEF(P):
"""
converts point in LTP(Blender) to ECEF-r coordinates
"""
x0 = convert_GPS_to_ECEF(P) #P->x0
x0 = mathutils.Vector(x0)
transform_matrix = [[-math.sin(P[0]),
math.cos(P[0]), 0],
[
-math.cos(P[0]) * math.sin(P[1]),
-math.sin(P[1]) * math.sin(P[0]),
math.cos(P[1])
],
[
math.cos(P[1]) * math.cos(P[0]),
math.cos(P[1]) * math.sin(P[0]),
math.sin(P[1])
]]
transform_matrix = mathutils.Matrix(transform_matrix)
transform_matrix.invert()
xe = x0 + transform_matrix * xt #transformed xt -> xe
return xe
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
else:
# previous attitude euler angles as vector
self.patt = mathutils.Vector(self.position_3d.euler)
# previous angular velocity
# 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.info("Attitude Component initialized, runs at %.2f Hz ",
self.frequency)
def set_object_pose(self, object_name, position, orientation):
""" Sets the pose of the object.
:param string object_name: The name of the object.
:param string position: new position of the object [x, y, z]
:param string position: new orientation of the object [qw, qx, qy, qz]
"""
b_obj = get_obj_by_name(object_name)
pos = mathutils.Vector(json.loads(position))
ori = mathutils.Quaternion(json.loads(orientation)).to_matrix()
# Suspend Physics of that object
b_obj.suspendDynamics()
b_obj.setLinearVelocity([0.0, 0.0, 0.0], True)
b_obj.setAngularVelocity([0.0, 0.0, 0.0], True)
b_obj.applyForce([0.0, 0.0, 0.0], True)
b_obj.applyTorque([0.0, 0.0, 0.0], True)
logger.debug("%s goes to %s" % (b_obj, pos))
b_obj.worldPosition = pos
b_obj.worldOrientation = ori
# Reset physics simulation
b_obj.restoreDynamics()
def default_action(self):
"""
Calculates speed and GPS-position
Configurations are the GPS-values for the Blenderorigin
Transforms point from LTP to Geodetic coordinates
Refer to:
- Conversion of Geodetic coordinates to the Local Tangent Plane,
Version 2.01,
http://psas.pdx.edu/CoordinateSystem/Latitude_to_LocalTangent.pdf
- Comparative Analysis of the Performance of Iterative and Non-iterative
Solutions to the Cartesian to Geodetic Coordinate Transformation,
Hok Sum Fok and H. Bâki Iz,
http://www.lsgi.polyu.edu.hk/staff/zl.li/Vol_5_2/09-baki-3.pdf
"""
####
#Speed
####
##copied from accelerometer
# Compute the difference in positions with the previous loop
self.dx = self.p[0] - self.ppx
self.dy = self.p[1] - self.ppy
self.dz = self.p[2] - self.ppz
# Store the position in this instant
self.ppx = self.p[0]
self.ppy = self.p[1]
self.ppz = self.p[2]
# Scale the speeds to the time used by Blender
self.v[0] = self.dx * self.frequency
self.v[1] = self.dy * self.frequency
self.v[2] = self.dz * self.frequency
# Update the data for the velocity
self.pvx = self.v[0]
self.pvy = self.v[1]
self.pvz = self.v[2]
####
#GPS
####
####
#constants in calculations
#a: WGS-84 Earth semimajor axis
#ecc: first eccentricity
####
a = float(6378137)
ecc = 8.181919191e-2
def convert_GPS_to_ECEF(P):
"""
converts gps-data(radians) to ECEF-r coordinates
"""
N = a / math.sqrt(1 - (ecc**2 * (math.sin(P[1])**2)))
h = P[2]
x0 = [(h + N) * math.cos(P[1]) * math.cos(P[0]),
(h + N) * math.cos(P[1]) * math.sin(P[0]),
(h + (1 - ecc**2) * N) * math.sin(P[1])]
return x0
def convert_LTP_to_ECEF(P):
"""
converts point in LTP(Blender) to ECEF-r coordinates
"""
x0 = convert_GPS_to_ECEF(P) #P->x0
x0 = mathutils.Vector(x0)
transform_matrix = [[-math.sin(P[0]),
math.cos(P[0]), 0],
[
-math.cos(P[0]) * math.sin(P[1]),
-math.sin(P[1]) * math.sin(P[0]),
math.cos(P[1])
],
[
math.cos(P[1]) * math.cos(P[0]),
math.cos(P[1]) * math.sin(P[0]),
math.sin(P[1])
]]
transform_matrix = mathutils.Matrix(transform_matrix)
transform_matrix.invert()
xe = x0 + transform_matrix * xt #transformed xt -> xe
return xe
def vermeille_method(xe):
"""
converts point in ECEF-r coordinates into Geodetic (GPS) via
Vermeille's method
"""
#"just intermediary parameters" see FoIz
p = (xe[0]**2 + xe[1]**2) / a**2
q = (1 - ecc**2) / a**2 * xe[2]**2
r = (p + q - ecc**4) / 6
s = ecc**4 * (p * q) / (4 * r**3)
t = (1 + s + math.sqrt(s * (2 + s)))**(1 / 3.0)
u = r * (1 + t + 1 / t)
v = math.sqrt(u**2 + (ecc**4 * q))
w = ecc**2 * ((u + v - q) / (2 * v))
k = math.sqrt(u + v + w**2) - w
D = (k * (math.sqrt(xe[0]**2 + xe[1]**2))) / (k + ecc**2)
gps_coords = [
2 * math.atan(xe[1] / (xe[0] +
(math.sqrt(xe[0]**2 + xe[1]**2)))),
2 * math.atan(xe[2] / (D + math.sqrt(D**2 + xe[2]**2))),
((k + ecc**2 - 1) / k) * math.sqrt(D**2 + xe[2]**2)
]
return gps_coords
#P -> Blender origin in Geodetic coordinates
P = [self.longitude, self.latitude, self.altitude]
#current position
xt = self.position_3d.translation
xt = mathutils.Vector(xt)
#P (in degrees) to radians
for i in range(len(P) - 1):
P[i] = math.radians(P[i])
####
#GPS -> ECEF-r
####
xe = convert_LTP_to_ECEF(P)
####
#ECEF-r -> GPS
####
gps_coords = vermeille_method(xe)
#gps_coords (in radians) to degrees
for i in range(len(gps_coords) - 1):
gps_coords[i] = math.degrees(gps_coords[i])
#compose message as close as possible to a GPS-standardprotocol
self.local_data['longitude'] = gps_coords[0]
self.local_data['latitude'] = gps_coords[1]
self.local_data['altitude'] = gps_coords[2]
self.local_data['velocity'] = self.v
def default_action(self):
""" Apply the positions read to the IK targets of the joints """
# Compute a rotation angle for the whole body, based on the
# angle of the shoulders
world_x_vector = mathutils.Vector([1, 0, 0])
shoulders_vector = mathutils.Vector([
(self._shoulder_empty_l.worldPosition[0] -
self._shoulder_empty_r.worldPosition[0]),
(self._shoulder_empty_l.worldPosition[1] -
self._shoulder_empty_r.worldPosition[1]),
(self._shoulder_empty_l.worldPosition[2] -
self._shoulder_empty_r.worldPosition[2])
])
logger.debug ("Shoulder Vector: [%.4f, %.4f, %.4f]" % \
(shoulders_vector[0], shoulders_vector[1], shoulders_vector[2]))
try:
# Measure the angle with respect to the X axis (in front of the man)
body_rotation = shoulders_vector.angle(world_x_vector)
# Correct the angle
body_rotation -= pi / 2
except ValueError:
# There will be an error if there is no user being tracked
# by the Kinect
body_rotation = 0.0
logger.debug("Angle with Y vector = %.4f" % body_rotation)
# Apply the rotation to the toroso. The rest of the body should follow
self._torso_empty.worldOrientation = [0.0, 0.0, body_rotation]
# Put the ik targets in the correct positions
self._set_object_position(self._head_empty.worldPosition,
self.local_data['head_position'])
self._set_object_position(self._neck_empty.worldPosition,
self.local_data['neck_position'])
self._set_object_position(self._torso_empty.worldPosition,
self.local_data['torso_position'])
self._set_object_position(self._hand_empty_l.worldPosition,
self.local_data['left_hand_position'])
self._set_object_position(self._hand_empty_r.worldPosition,
self.local_data['right_hand_position'])
self._set_object_position(self._elbow_empty_l.worldPosition,
self.local_data['left_elbow_position'])
self._set_object_position(self._elbow_empty_r.worldPosition,
self.local_data['right_elbow_position'])
self._set_object_position(self._shoulder_empty_l.worldPosition,
self.local_data['left_shoulder_position'])
self._set_object_position(self._shoulder_empty_r.worldPosition,
self.local_data['right_shoulder_position'])
self._set_object_position(self._hip_empty_l.worldPosition,
self.local_data['left_hip_position'])
self._set_object_position(self._hip_empty_r.worldPosition,
self.local_data['right_hip_position'])
self._set_object_position(self._knee_empty_l.worldPosition,
self.local_data['left_knee_position'])
self._set_object_position(self._knee_empty_r.worldPosition,
self.local_data['right_knee_position'])
self._set_object_position(self._foot_empty_l.worldPosition,
self.local_data['left_foot_position'])
self._set_object_position(self._foot_empty_r.worldPosition,
self.local_data['right_foot_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)
# Direction of the global vectors
self.world_x_vector = mathutils.Vector([1, 0, 0])
self.world_y_vector = mathutils.Vector([0, 1, 0])
self.world_z_vector = mathutils.Vector([0, 0, 1])
self._destination = self.bge_object.position
self._projection = self.bge_object.position
self._wp_object = None
self._collisions = False
# Variable to store current speed. Used for the stop/resume services
self._previous_speed = 0
# Choose the type of function to move the object
#self._type = 'Velocity'
self._type = 'Position'
self.local_data['x'] = self._destination[0]
self.local_data['y'] = self._destination[1]
self.local_data['z'] = self._destination[2]
# Waypoint tolerance (in meters)
self.local_data['tolerance'] = 0.5
self.local_data['speed'] = self._speed
# Identify an object as the target of the motion
try:
wp_name = self.bge_object['Target']
if wp_name != '':
scene = blenderapi.scene()
self._wp_object = scene.objects[wp_name]
logger.info("Using object '%s' to indicate motion target" %
wp_name)
except KeyError:
self._wp_object = None
# Identify the collision detectors for the sides
if self._obstacle_avoidance:
for child in self.bge_object.children:
if "Radar.R" in child.name:
self._radar_r = child
if "Radar.L" in child.name:
self._radar_l = child
try:
logger.info("Radar Right is '%s'" % self._radar_r.name)
logger.info("Radar Left is '%s'" % self._radar_l.name)
self._collisions = True
except AttributeError:
logger.warning("No radars found attached to the waypoint \
actuator.There will be no obstacle avoidance")
# Convert a string listing the properties to avoid into a list
# Objects with any of these properties will be ignored
# when doing obstacle avoidance
try:
props = self.bge_object['Ignore'].split(',')
self.bge_object['Ignore'] = props
except KeyError:
self.bge_object['Ignore'] = []
logger.info('Component initialized')
def default_action(self):
""" Move the object towards the destination. """
parent = self.robot_parent
speed = self.local_data['speed']
v = 0
rz = 0
self._destination = [
self.local_data['x'], self.local_data['y'], self.local_data['z']
]
self._projection = [
self.local_data['x'], self.local_data['y'],
self.bge_object.worldPosition[2]
]
logger.debug("Robot {0} move status: '{1}'".format(
parent.bge_object.name, parent.move_status))
# Place the target marker where the robot should go
if self._wp_object:
self._wp_object.position = self._destination
# Vectors returned are already normalized
projection_distance, projection_vector, local_vector = \
self.bge_object.getVectTo(self._projection)
true_distance, global_vector, local_vector = \
self.bge_object.getVectTo(self._destination)
# Convert to the Blender Vector object
global_vector = mathutils.Vector(global_vector)
projection_vector = mathutils.Vector(projection_vector)
# if Z is not free, distance is the projection distance
if self._free_z:
distance = true_distance
else:
distance = projection_distance
logger.debug("GOT DISTANCE: xy: %.4f ; xyz: %.4f" %
(projection_distance, true_distance))
logger.debug("Global vector: %.4f, %.4f, %.4f" %
(global_vector[0], global_vector[1], global_vector[2]))
logger.debug("Local vector: %.4f, %.4f, %.4f" %
(local_vector[0], local_vector[1], local_vector[2]))
logger.debug(
"Projection vector: %.4f, %.4f, %.4f" %
(projection_vector[0], projection_vector[1], projection_vector[2]))
# If the target has been reached, change the status
if distance - self.local_data['tolerance'] <= 0:
parent.move_status = "Arrived"
#Do we have a running request? if yes, notify the completion
self.completed(status.SUCCESS, parent.move_status)
logger.debug("TARGET REACHED")
logger.debug("Robot {0} move status: '{1}'".format(
parent.bge_object.name, parent.move_status))
else:
# Do nothing if the speed is zero
if speed == 0:
return
parent.move_status = "Transit"
angle_diff = 0
rotation_direction = 0
# If the projected distance is not null: else computing the
# target angle is not possible!
if projection_distance - self.local_data['tolerance'] / 2 > 0:
### Get the angle of the robot ###
robot_angle = parent.position_3d.yaw
### Get the angle to the target ###
target_angle = projection_vector.angle(self.world_x_vector)
# Correct the direction of the turn according to the angles
dot = projection_vector.dot(self.world_y_vector)
logger.debug("Vector dot product = %.2f" % dot)
if dot < 0:
target_angle = target_angle * -1
### Get the angle that the robot must turn ###
if target_angle < robot_angle:
angle_diff = robot_angle - target_angle
rotation_direction = -1
else:
angle_diff = target_angle - robot_angle
rotation_direction = 1
# Make a correction when the angles change signs
if angle_diff > math.pi:
angle_diff = (2 * math.pi) - angle_diff
rotation_direction = rotation_direction * -1
logger.debug(
"Angles: R=%.4f, T=%.4f Diff=%.4f Direction = %d" %
(robot_angle, target_angle, angle_diff,
rotation_direction))
try:
# Compute the speeds
if self._type == 'Position':
v = speed / self.frequency
rotation_speed = (speed / self.frequency) / 2.0
elif self._type == 'Velocity':
v = speed
rotation_speed = 1.0 #speed / 2.0
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
# Allow the robot to rotate in place if the waypoing is
# to the side or behind the robot
if angle_diff >= math.pi / 3.0:
logger.debug("Turning on the spot!!!")
v = 0
# Collision avoidance using the Blender radar sensor
if self._collisions and v != 0 and self._radar_r['Rcollision']:
# No obstacle avoidance when the waypoint is near
if distance + self.local_data['tolerance'] > \
self._radar_r.sensors["Radar"].distance:
# Ignore obstacles with the properties specified
ignore = False
for prop in self.bge_object['Ignore']:
if prop in self._radar_r.sensors["Radar"].hitObject:
ignore = True
logger.debug(
"Ignoring object '%s' "
"with property '%s'" %
(self._radar_r.sensors["Radar"].hitObject,
prop))
break
if not ignore:
rz = rotation_speed
logger.debug("Obstacle detected to the RIGHT, "
"turning LEFT")
elif self._collisions and v != 0 and self._radar_l['Lcollision']:
# No obstacle avoidance when the waypoint is near
if distance + self.local_data['tolerance'] > \
self._radar_l.sensors["Radar"].distance:
# Ignore obstacles with the properties specified
ignore = False
for prop in self.bge_object['Ignore']:
if prop in self._radar_l.sensors["Radar"].hitObject:
ignore = True
logger.debug("Ignoring object '%s' "
"with property '%s'" % \
(self._radar_l.sensors["Radar"].hitObject, prop))
break
if not ignore:
rz = -rotation_speed
logger.debug("Obstacle detected to the LEFT, "
"turning RIGHT")
# Test if the orientation of the robot is within tolerance
elif -self._angle_tolerance < angle_diff < self._angle_tolerance:
rz = 0
# If not, rotate the robot in the corresponding direction
else:
rz = rotation_speed * rotation_direction
if self._free_z:
vx = math.fabs(v * local_vector.dot(self.world_x_vector))
vz = v * local_vector.dot(self.world_z_vector)
else:
vx = v
vz = 0
logger.debug(
"Applying vx = %.4f, vz = %.4f, rz = %.4f (v = %.4f)" %
(vx, vz, rz, v))
# Give the movement instructions directly to the parent
# The second parameter specifies a "local" movement
if self._type == 'Position':
parent.bge_object.applyMovement([vx, 0, vz], True)
parent.bge_object.applyRotation([0, 0, rz], True)
elif self._type == 'Velocity':
parent.bge_object.setLinearVelocity([vx, 0, vz], True)
parent.bge_object.setAngularVelocity([0, 0, rz], True)
def translation(self):
return mathutils.Vector((self.x, self.y, self.z))
