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
# Tick rate is the real measure of time in Blender.
# By default it is set to 60, regardles of the FPS
# If logic tick rate is 60, then: 1 second = 60 ticks
ticks = GameLogic.getLogicTicRate()
# Scale the speeds to the time used by Blender
try:
vx = self.local_data['x'] / ticks
vy = self.local_data['y'] / ticks
rz = self.local_data['w'] / ticks
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
# Get the Blender object of the parent robot
parent = self.robot_parent.blender_obj
# Give the movement instructions directly to the parent
# The second parameter specifies a "local" movement
parent.applyMovement([vx, vy, vz], True)
parent.applyRotation([rx, ry, rz], True)
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.blender_obj.name, parent.move_status))
# Vectors returned are already normalised
distance, global_vector, local_vector = self.blender_obj.getVectTo(self.destination)
logger.debug("My position: {0}".format(self.blender_obj.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"
# Tick rate is the real measure of time in Blender.
# By default it is set to 60, regardles of the FPS
# If logic tick rate is 60, then: 1 second = 60 ticks
ticks = GameLogic.getLogicTicRate()
# Scale the speeds to the time used by Blender
try:
vx = global_vector[0] * self._speed / ticks
vy = global_vector[1] * self._speed / ticks
vz = global_vector[2] * self._speed / ticks
# 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.blender_obj.name, parent.move_status))
# Give the movement instructions directly to the parent
# The second parameter specifies a "local" movement
parent.blender_obj.applyMovement([vx, vy, vz], False)
def default_action(self):
""" Apply rotation to the arm segments """
# Reset movement variables
rx, ry, rz = 0.0, 0.0, 0.0
# Tick rate is the real measure of time in Blender.
# By default it is set to 60, regardles of the FPS
# If logic tick rate is 60, then: 1 second = 60 ticks
ticks = GameLogic.getLogicTicRate()
# Scale the speeds to the time used by Blender
try:
rotation = self._speed / ticks
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
# Use the length of _dofs, since it won't change.
# The length of _segments will change if more objects are attached
# at the end of the arm, as in the case of a hand
for i in range(len(self._dofs)):
key = ('seg%d' % i)
target_angle = morse_math.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 = morse_math.rotation_direction(segment_euler[1], target_angle, self._tolerance, rotation)
elif self._dofs[i] == '-y':
ry = morse_math.rotation_direction(segment_euler[1], -target_angle, self._tolerance, rotation)
elif self._dofs[i] == 'z':
rz = morse_math.rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
logger.debug("PARAMETERS: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, self._tolerance, rotation, rz))
# Give the movement instructions directly to the parent
# The second parameter specifies a "local" movement
segment.applyRotation([rx, ry, rz], True)
# Reset the rotations for the next segment
ry = rz = 0
def default_action(self):
""" Apply rotation angles to the segments of the arm """
# Reset movement variables
rx, ry, rz = 0.0, 0.0, 0.0
# Tick rate is the real measure of time in Blender.
# By default it is set to 60, regardles of the FPS
# If logic tick rate is 60, then: 1 second = 60 ticks
ticks = GameLogic.getLogicTicRate()
# Scale the speeds to the time used by Blender
try:
rotation = self._speed / ticks
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
armature = self.blender_obj
logger.info("The armature is: '%s' (%s)" % (armature, type(armature)))
i = 0
for channel in armature.channels:
segment_angle = channel.joint_rotation
logger.debug("\tChannel '%s': (%.4f, %.4f, %.4f)" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
key = ('seg%d' % i)
# Get the normalised angle for this segment
target_angle = morse_math.normalise_angle(self.local_data[key])
logger.info("%.4f " % target_angle, end='')
# Use the corresponding direction for each rotation
if self._dofs[i] == 'y':
ry = morse_math.rotation_direction(segment_angle[1], target_angle, self._tolerance, rotation)
elif self._dofs[i] == 'z':
rz = morse_math.rotation_direction(segment_angle[2], target_angle, self._tolerance, rotation)
logger.info("[%.4f, %.4f, %.4f] " % (rx, ry, rz))
# Give the movement instructions directly to the parent
# The second parameter specifies a "local" movement
#segment.applyRotation([rx, ry, rz], True)
channel.joint_rotation = [rx, ry, rz]
# Reset the rotations for the next segment
ry = rz = 0
i = i + 1
def update(faceServer, time_diff):
"""
"""
global INFO_PERIOD
cont = G.getCurrentController()
eyes_done = False
# threaded server is thread-safe
for au, value in faceServer.update(time_diff):
if au[0] == '6': # XXX: yes, 6 is an eye prefix (do better ?)
if eyes_done:
continue
# The model is supposed to look towards negative Y values
# Also Up is positive Z values
ax = -faceServer.get_AU('63.5')[3]
az0 = faceServer.get_AU('61.5R')[3]
az1 = faceServer.get_AU('61.5L')[3]
# No ACTION for eyes
G.eye_L.localOrientation = [
[cos(az0), -sin(az0), 0],
[cos(ax)*sin(az0), cos(ax)*cos(az0),-sin(ax)],
[sin(ax)*sin(az0), sin(ax)*cos(az0), cos(ax)] ]
G.eye_R.localOrientation = [
[cos(az1), -sin(az1), 0],
[cos(ax)*sin(az1), cos(ax)*cos(az1),-sin(ax)],
[sin(ax)*sin(az1), sin(ax)*cos(az1), cos(ax)] ]
eyes_done = True
elif au == '26':
# TODO: try with G.setChannel
G.jaw['p26'] = SH_ACT_LEN * value # see always sensor in .blend
elif au[0] == '9': # XXX: yes, 6 is a tongue prefix (do better ?)
G.tongue[au] = SH_ACT_LEN * value
else:
cont.owner['p'+au] = value * SH_ACT_LEN
cont.activate(cont.actuators[au])
INFO_PERIOD += time_diff
if INFO_PERIOD > 5:
print "--- RENDERING INFO ---"
print "BGE logic running at", G.getLogicTicRate(), "fps."
# print "BGE physics running at", G.getPhysicsTicRate(), "fps."
print "BGE graphics currently at", G.getAverageFrameRate(), "fps."
INFO_PERIOD = 0
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)
# Variables to store the previous position
self.ppx = 0.0
self.ppy = 0.0
self.ppz = 0.0
# Variables to store the previous angle position
self.pproll = 0.0
self.pppitch = 0.0
self.ppyaw = 0.0
# Variables to store the previous velocity
self.pvx = 0.0
self.pvy = 0.0
self.pvz = 0.0
# Variables to store the previous angle velocity (in rad)
self.pvroll = 0.0
self.pvpitch = 0.0
self.pvyaw = 0.0
# Make a new reference to the sensor position
self.p = self.blender_obj.position
self.v = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Velocity
self.pv = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Previous Velocity
self.a = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Acceleration
# Tick rate is the real measure of time in Blender.
# By default it is set to 60, regardles of the FPS
# If logic tick rate is 60, then: 1 second = 60 ticks
self.ticks = GameLogic.getLogicTicRate()
self.local_data['distance'] = 0.0
self.local_data['velocity'] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.local_data['acceleration'] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
logger.info('Component initialized')
def default_action(self):
""" Apply rotation to the platine unit """
# Reset movement variables
rx, ry, rz = 0.0, 0.0, 0.0
if self._is_manual_mode:
return
# Update the postition of the base platforms
try:
self._pan_position_3d.update(self._pan_base)
self._tilt_position_3d.update(self._tilt_base)
except AttributeError as detail:
logger.error("Platine is missing the pan and tilt bases. Platine does not work!")
return
# Tick rate is the real measure of time in Blender.
# By default it is set to 60, regardles of the FPS
# If logic tick rate is 60, then: 1 second = 60 ticks
ticks = GameLogic.getLogicTicRate()
try:
normal_speed = self._speed / ticks
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
current_pan = self._pan_position_3d.yaw
current_tilt = self._tilt_position_3d.pitch
if (
abs(current_pan - self.local_data["pan"]) < self._tolerance
and abs(current_tilt - self.local_data["tilt"]) < self._tolerance
):
self.completed((status.SUCCESS))
logger.debug("Platine: pan=%.4f, tilt=%.4f" % (current_pan, current_tilt))
# Get the angles in a range of -PI, PI
target_pan = morse_math.normalise_angle(self.local_data["pan"])
target_tilt = morse_math.normalise_angle(self.local_data["tilt"])
logger.debug("Targets: pan=%.4f, tilt=%.4f" % (target_pan, target_tilt))
# Get the current rotation of the parent robot
parent_pan = self.robot_parent.position_3d.euler.z
parent_tilt = self.robot_parent.position_3d.euler.y
logger.debug("Parent: pan=%.4f, tilt=%.4f" % (parent_pan, parent_tilt))
# Compute the rotation relative to the parent robot
relative_pan = current_pan - parent_pan
correct_pan = morse_math.normalise_angle(relative_pan)
relative_tilt = current_tilt - parent_tilt
correct_tilt = morse_math.normalise_angle(relative_tilt)
# Determine the direction of the rotation, if any
ry = morse_math.rotation_direction(correct_tilt, target_tilt, self._tolerance, normal_speed)
rz = morse_math.rotation_direction(correct_pan, target_pan, self._tolerance, normal_speed)
# Give the rotation instructions directly to the parent
# The second parameter specifies a "local" movement
self._pan_base.applyRotation([0.0, 0.0, rz], True)
self._tilt_base.applyRotation([0.0, ry, 0.0], True)
def default_action(self):
""" Move the object towards the destination. """
parent = self.robot_parent
speed = self.local_data['speed']
vx = 0
rz = 0
self._destination = [ self.local_data['x'], self.local_data['y'], self.local_data['z'] ]
logger.debug("Robot {0} move status: '{1}'".format(parent.blender_obj.name, parent.move_status))
# Place the target marker where the robot should go
if self._wp_object:
self._wp_object.position = self._destination
# Set the z coordiante of the destination equal to that of the robot
# to avoid problems with the terrain.
self._destination[2] = self.blender_obj.worldPosition[2]
# Vectors returned are already normalised
distance, global_vector, local_vector = self.blender_obj.getVectTo(self._destination)
# Convert to the Blender Vector object
global_vector = mathutils.Vector(global_vector)
logger.debug("GOT DISTANCE: %.4f" % (distance))
logger.debug("Global vector: %.4f, %.4f, %.4f" % (global_vector[0], global_vector[1], global_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 runing 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.blender_obj.name, parent.move_status))
else:
# Do nothing if the speed is zero
if speed == 0:
return
parent.move_status = "Transit"
### Get the angle of the robot ###
robot_angle = parent.position_3d.yaw
### Get the angle to the target ###
target_angle = global_vector.angle(self.world_X_vector)
# Correct the direction of the turn according to the angles
dot = global_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))
# Tick rate is the real measure of time in Blender.
# By default it is set to 60, regardles of the FPS
# If logic tick rate is 60, then: 1 second = 60 ticks
ticks = GameLogic.getLogicTicRate()
try:
# Compute the speeds
if self._type == 'Position':
vx = speed / ticks
rotation_speed = (speed / ticks) / 2.0
elif self._type == 'Velocity':
vx = 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!!!")
vx = 0
# Collision avoidance using the Blender radar sensor
if self._collisions and vx != 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:
rz = rotation_speed
logger.debug("Obstacle detected to the RIGHT, turning LEFT")
elif self._collisions and vx != 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:
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
logger.debug("Applying [[vx = %.4f, rz = %.4f]]\n" % (vx, rz))
# Give the movement instructions directly to the parent
# The second parameter specifies a "local" movement
if self._type == 'Position':
parent.blender_obj.applyMovement([vx, 0, 0], True)
parent.blender_obj.applyRotation([0, 0, rz], True)
elif self._type == 'Velocity':
parent.blender_obj.setLinearVelocity([vx, 0, 0], True)
parent.blender_obj.setAngularVelocity([0, 0, rz], True)
def move(contr):
# Get the object data
ob, parent, port_name = setup.ObjectData.get_object_data(contr)
dest_port_name = port_name + '/in'
speed_port_name = port_name + '/speed/in'
destination = []
# Direction tolerance for the movement (in degrees)
angle_tolerance = 5
# Get the dictionary for the robot's state
robot_state_dict = GameLogic.robotDict[parent]
# Speed variable
speed = robot_state_dict['speed']
NED = False
# Get the orientation of the robot
if parent['Orientation'] == 'NED':
NED = True
if ob['Init_OK']:
# Reset movement variables
vx, vy, vz = 0.0, 0.0, 0.0
rx, ry, rz = 0.0, 0.0, 0.0
########################### SPEED ###############################
# Retrieve the port we want to read from
sp = GameLogic.orsConnector.getPort(speed_port_name)
#non-blocking read of the port
speed_msg = sp.read(False)
if speed_msg!=None:
robot_state_dict['speed'] = speed_msg.get(0).asDouble()
print ("SETTING SPEED TO: {0}".format(speed))
########################### DESTINATION ###############################
# Retrieve the port we want to read from
p = GameLogic.orsConnector.getPort(dest_port_name)
#non-blocking read of the port
dest = p.read(False)
if dest!=None:
for i in range(3):
destination.append( dest.get(i).asDouble() )
robot_state_dict['moveStatus'] = "Transit"
print ("STRAIGHT GOT DESTINATION: {0}".format(destination))
print ("Robot {0} move status: '{1}'".format(parent, robot_state_dict['moveStatus']))
robot_state_dict['destination'] = destination
# DEBUGGING:
# Translate the marker to the target destination
scene = GameLogic.getCurrentScene()
target_ob = scene.objects[ob['TargetObject']]
if NED == True:
d=destination[0]
destination[0] = destination[1]
destination[1] = d
destination[2] = -destination[2]
target_ob.position = destination
try:
area_ob = scene.objects['OBWP_Area']
area_ob.scaling = (robot_state_dict['tolerance'], robot_state_dict['tolerance'], 1)
except KeyError:
pass
try:
# Exit the function if there has been no command to move
if not robot_state_dict['moveStatus'] == "Transit":
return
except KeyError:
# Also exit if there is no moveStatus property
return
scene = GameLogic.getCurrentScene()
target_ob = scene.objects[ob['TargetObject']]
destination = target_ob.position
#destination[2] = destination[2]
# Ignore the altitude (Z)
#destination[2] = 0
# Calculate the direction needed
location_V = Mathutils.Vector(ob.position)
# Ignore the altitude (Z)
#location_V[2] = 0
destination_V = Mathutils.Vector(destination)
distance_V = destination_V - location_V
#print ("\nlocation_V {0}".format(location_V))
#print ("destination_V {0}".format(destination_V))
#print ("distance_V {0}".format(distance_V))
distance = distance_V.length - robot_state_dict['tolerance']
#print ("GOT DISTANCE: {0}".format(distance))
# Testing to get the correct transformation for the
# movement of the robot using its local coordinate system
# The results were strange either using local or global coordinates
#rotation_matrix = MorseMath.get_rotation_matrix (parent)
#MorseMath.print_matrix_33(rotation_matrix)
#distance_V = distance_V * rotation_matrix
#print ("ROTATION distance_V {0}".format(distance_V))
#inverted_matrix = MorseMath.invert_rotation_matrix (parent)
#MorseMath.print_matrix_33(inverted_matrix)
#distance_V = inverted_matrix * distance_V
#print ("INVERTED distance_V {0}".format(distance_V))
if distance > 0:
# Move forward
distance_V.normalize()
#fps = GameLogic.getAverageFrameRate()
ticks = GameLogic.getLogicTicRate()
if NED == True:
vx = distance_V[1] * speed/ticks
vy = distance_V[0] * speed/ticks
vz = -distance_V[2] * speed/ticks
else:
vx = distance_V[0] * speed/ticks
vy = distance_V[1] * speed/ticks
vz = distance_V[2] * speed/ticks
# Correction of the movement direction,
# with respect to the object's orientation
#movement_V = Mathutils.Vector(vx, vy, vz)
#(vx, vy, vz) = parent.getAxisVect(movement_V)
# If the target has been reached, change the status
elif distance <= 0:
# Teleport robot to the desired destination
parent.position = target_ob.position
robot_state_dict['moveStatus'] = "Stop"
print ("TARGET REACHED")
print ("Robot {0} move status: '{1}'".format(parent, robot_state_dict['moveStatus']))
"""
robot_state_dict['vx'] = vx
robot_state_dict['vy'] = vy
robot_state_dict['vz'] = vz
msg_act = contr.actuators['Send_update_msg']
msg_act.propName = parent.name
msg_act.subject = 'Speed'
contr.activate(msg_act)
"""
# Give the movement instructions directly to the parent
# The second parameter specifies a "global" movement
parent.applyMovement([vx, vy, vz], False)
