def update_Y_forward(self, obj):
"""
Update the transformation3D to reflect the tranformation
between obj (a blender object) and the blender world origin.
In this case, the robot moves forwad along the Y axis.
Change the values of yaw, pitch, roll for Blender vehicles
Robots that use the Blender vehicle constrainst move in the
direction of the Y axis, contrary to most of the MORSE components
that move along the X axis.
"""
rot_matrix = obj.orientation
self.matrix = mathutils.Matrix((rot_matrix[0], \
rot_matrix[1], \
rot_matrix[2]))
self.matrix = self.matrix * self.correction_matrix
self.matrix.resize_4x4()
pos = obj.worldPosition
for i in range(0, 3):
if blenderapi.version() < (2, 62, 0):
self.matrix[3][i] = pos[i]
else:
self.matrix[i][3] = pos[i]
self.matrix[3][3] = 1
self.euler = self.matrix.to_euler()
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(VideoCamera, self).__init__(obj, parent)
# Prepare the exportable data of this sensor
self.local_data['image'] = ''
# Prepare the intrinsic matrix for this camera.
# Note that the matrix is stored in row major
intrinsic = mathutils.Matrix()
intrinsic.identity()
alpha_u = self.image_width * \
self.image_focal / BLENDER_HORIZONTAL_APERTURE
intrinsic[0][0] = alpha_u
intrinsic[1][1] = alpha_u
intrinsic[0][2] = self.image_width / 2.0
intrinsic[1][2] = self.image_height / 2.0
self.local_data['intrinsic_matrix'] = intrinsic
self.capturing = False
self._n = -1
# Variable to indicate this is a camera
self.camera_tag = True
# Position of the robot where the last shot is taken
self.robot_pose = copy.copy(self.robot_parent.position_3d)
logger.info("Component initialized, runs at %.2f Hz ", self.frequency)
def __init__(self, obj):
"""
Construct a transformation3d. Generate the identify
transformation if no object is associated to it. Otherwise,
returns the transformation3D between this object and the world
reference
"""
self.matrix = mathutils.Matrix(
([1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]))
self.euler = mathutils.Euler([0, 0, 0])
if obj is not None:
self.update(obj)
# For use only by robots moving along the Y axis
self.correction_matrix = mathutils.Matrix(
([0.0, 1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, 1.0]))
def _create_transform_matrix():
""" Construct the transformation matrix
from the Kinect to the Blender frame of reference
"""
global transformation_matrix
# Y X Z Y
# | / | /
# KinCam |/ ____ Z , World |/_____X
# Transformation of the Kinect frame of reference to that of Blender
kinect_matrix = mathutils.Matrix((
[0.0, 1.0, 0.0, 0.0], \
[0.0, 0.0, 1.0, 0.0], \
[1.0, 0.0, 0.0, 0.0], \
[0.0, 0.0, 0.0, 1.0]))
# Additional rotation of the physical sensor, with respect to the Blender world
# Currently set to 25.5 degrees around the Y axis
kinect_rotation = mathutils.Matrix((
[1.0, 0.0, -0.545, 0.0], \
[0.0, 1.0, 0.0, 0.0], \
[0.545, 0.0, 1.0, 0.0], \
[0.0, 0.0, 0.0, 1.0]))
# Spin the positions around the Z axis, to match with the Blender human
rotation_matrix = mathutils.Matrix((
[-1.0, 0.0, 0.0, 0.0], \
[0.0, -1.0, 0.0, 0.0], \
[0.0, 0.0, 1.0, 0.0], \
[0.0, 0.0, 0.0, 1.0]))
# Position of the kinect with respect to the human.
# XXX: Make this adjustable from the real position in the scene
kinect_position = [2.0, 0.0, 2.0]
#logger.error("Kinect position: [%.4f, %.4f, %.4f]" % (kinect_position[0], kinect_position[1], kinect_position[2]))
transformation_matrix = kinect_matrix * kinect_rotation * rotation_matrix
# Add the position of the Kinect sensor
transformation_matrix[0][3] = kinect_position[0]
transformation_matrix[1][3] = kinect_position[1]
transformation_matrix[2][3] = kinect_position[2]
def set_camarafp_projection_matrix(self, projection_matrix):
""" Set the CamaraFP (MORSE' environment camera) 4x4 projection matrix
:param projection_matrix: The camera's 4x4 projection matrix.
:type projection_matrix: 4x4 Matrix [[float]]
"""
try:
blender_object = get_obj_by_name('CameraFP')
blender_object.projection_matrix = mathutils.Matrix(projection_matrix)
return projection_matrix
except SystemError: # if the matrix is not 4x4 numpy raises a SystemError
raise MorseRPCInvokationError( "The Matrix must be 4x4 [[float]]" )
def set_camarafp_transform(self, transform):
""" Set the CamaraFP (MORSE' environment camera) world space transform matrix.
:param transform: The camera's world space transform matrix.
:type transform: 4x4 Matrix [[float]]
"""
try:
blender_object = get_obj_by_name('CameraFP')
blender_object.worldTransform = mathutils.Matrix(transform)
return transform
except SystemError: # if the matrix is not 4x4 numpy raises a SystemError
raise MorseRPCInvokationError( "The Matrix must be 4x4 [[float]]" )
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 update(self, obj):
"""
Update the transformation3D to reflect the tranformation
between obj (a blender object) and the blender world origin
"""
rot_matrix = obj.orientation
self.matrix = mathutils.Matrix((rot_matrix[0], \
rot_matrix[1], \
rot_matrix[2]))
self.matrix.resize_4x4()
pos = obj.worldPosition
for i in range(0, 3):
if blenderapi.version() < (2, 62, 0):
self.matrix[3][i] = pos[i]
else:
self.matrix[i][3] = pos[i]
self.matrix[3][3] = 1
self.euler = self.matrix.to_euler()
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 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
import logging; logger = logging.getLogger("morse." + __name__)
from morse.middleware.pocolibs_datastream import PocolibsDataStreamInput
from niut.struct import *
from morse.core import mathutils
# Define a transformation matrix for the position of the Kinect/Xtion sensor
transformation_matrix = mathutils.Matrix()
class NiutPoster(PocolibsDataStreamInput):
_type_name = "NIUT_HUMAN_LIST"
_type_url = "http://trac.laas.fr/git/niut-genom/tree/niutStruct.h#n82"
def initialize(self):
PocolibsDataStreamInput.initialize(self, NIUT_HUMAN_LIST)
self.couples = \
[('head_position', NIUT_HEAD),
('neck_position', NIUT_NECK),
('torso_position', NIUT_TORSO),
('left_hand_position', NIUT_LEFT_HAND),
('right_hand_position', NIUT_RIGHT_HAND),
('left_elbow_position', NIUT_LEFT_ELBOW),
('right_elbow_position', NIUT_RIGHT_ELBOW),
('left_shoulder_position', NIUT_LEFT_SHOULDER),
('right_shoulder_position', NIUT_RIGHT_SHOULDER),
('left_hip_position', NIUT_LEFT_HIP),
('right_hip_position', NIUT_RIGHT_HIP),
('left_knee_position', NIUT_LEFT_KNEE),
('right_knee_position', NIUT_RIGHT_KNEE),
('left_foot_position', NIUT_LEFT_FOOT),
def default_action(self):
""" Apply rotation to the arm segments """
# Get the reference to the Sound actuator
if self._sound is None:
logger.debug ("ACTIVATING THE SOUND ACTUATOR")
contr = blenderapi.controller()
self._sound = contr.actuators['Sound']
contr.activate(self._sound)
self._sound.stopSound()
# Reset movement variables
rx, ry, rz = 0.0, 0.0, 0.0
# Scale the speeds to the time used by Blender
try:
rotation = self._speed / self.frequency
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
self._moving = False
for i in range(6):
key = ('seg%d' % i)
target_angle = normalise_angle(self.local_data[key])
# Get the next segment
segment = self._segments[i]
# Extract the angles
rot_matrix = segment.localOrientation
segment_matrix = mathutils.Matrix((rot_matrix[0], rot_matrix[1], rot_matrix[2]))
segment_euler = segment_matrix.to_euler()
# Use the corresponding direction for each rotation
if self._dofs[i] == 'y':
ry = rotation_direction(segment_euler[1], target_angle, self._tolerance, rotation)
#logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, _tolerance, rotation, ry))
elif self._dofs[i] == 'z':
rz = rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
#logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, _tolerance, rotation, rz))
logger.debug("ry = %.4f, rz = %.4f" % (ry, rz))
# Give the movement instructions directly to the parent
# The second parameter specifies a "local" movement
segment.applyRotation([rx, ry, rz], True)
if ry != 0.0 or rz != 0.0:
self._moving = True
# Reset the rotations for the next segment
ry = rz = 0
if self._moving:
self._sound.startSound()
logger.debug("STARTING SOUND")
else:
self._sound.stopSound()
logger.debug("STOPPING SOUND")
