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("The PTU is missing the pan and/or tilt bases. Discarding action.")
return
try:
normal_speed = self._speed / self.frequency
# 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
logger.debug("PTU: 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)
# Store the variables to acces as a service:
self._current_pan = correct_pan
self._current_tilt = correct_tilt
if (abs(target_pan - correct_pan) < self._tolerance and \
abs(target_tilt - correct_tilt) < self._tolerance):
self.completed((status.SUCCESS))
# 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):
""" Apply rotation angles to the segments of the arm """
armature = self.blender_obj
logger.debug("The armature is: '%s' (%s)" % (armature, type(armature)))
for channel in armature.channels:
segment_angle = channel.joint_rotation
logger.debug("Channel '%s' st: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
# Get the normalised angle for this segment
target_angle = morse_math.normalise_angle(self.local_data[channel.name])
# Use the corresponding direction for each rotation
if self._dofs[channel.name][1] == 1:
#ry = morse_math.rotation_direction(segment_angle[1], target_angle, self._tolerance, rotation)
segment_angle[1] = target_angle
elif self._dofs[channel.name][2] == 1:
#rz = morse_math.rotation_direction(segment_angle[2], target_angle, self._tolerance, rotation)
segment_angle[2] = target_angle
logger.debug("Channel '%s' fn: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
channel.joint_rotation = segment_angle
armature.update()
def default_action(self):
""" Apply rotation angles to the segments of the arm """
armature = self.blender_obj
logger.debug("The armature is: '%s' (%s)" % (armature, type(armature)))
for channel in armature.channels:
segment_angle = channel.joint_rotation
logger.debug("Channel '%s' st: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
# Get the normalised angle for this segment
target_angle = morse_math.normalise_angle(self.local_data[channel.name])
# Use the corresponding direction for each rotation
if self._dofs[channel.name][1] == 1:
#ry = morse_math.rotation_direction(segment_angle[1], target_angle, self._tolerance, rotation)
segment_angle[1] = target_angle
elif self._dofs[channel.name][2] == 1:
#rz = morse_math.rotation_direction(segment_angle[2], target_angle, self._tolerance, rotation)
segment_angle[2] = target_angle
logger.debug("Channel '%s' fn: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
channel.joint_rotation = segment_angle
armature.update()
def default_action(self):
""" Compute the relative position and rotation of the robot
The measurements are taken with respect to the previous position
and orientation of the robot
"""
# Compute the difference in positions with the previous loop
self.local_data['dx'] = self.robot_parent.position_3d.x - self.previous_position[0]
self.local_data['dy'] = self.robot_parent.position_3d.y - self.previous_position[1]
self.local_data['dz'] = self.robot_parent.position_3d.z - self.previous_position[2]
# Compute the difference in orientation with the previous loop
dyaw = self.robot_parent.position_3d.yaw - self.previous_orientation[0]
dpitch = self.robot_parent.position_3d.pitch - self.previous_orientation[1]
droll = self.robot_parent.position_3d.roll - self.previous_orientation[2]
self.local_data['dyaw'] = morse_math.normalise_angle(dyaw)
self.local_data['dpitch'] = morse_math.normalise_angle(dpitch)
self.local_data['droll'] = morse_math.normalise_angle(droll)
# Store the 'new' previous data
self.previous_position = [self.robot_parent.position_3d.x, self.robot_parent.position_3d.y, self.robot_parent.position_3d.z]
self.previous_orientation = [self.robot_parent.position_3d.yaw, self.robot_parent.position_3d.pitch, self.robot_parent.position_3d.roll]
def default_action(self):
""" Compute the relative position and rotation of the robot
The measurements are taken with respect to the previous position
and orientation of the robot
"""
# Compute the difference in positions with the previous loop
self.local_data['dx'] = self.robot_parent.position_3d.x - self.previous_position[0]
self.local_data['dy'] = self.robot_parent.position_3d.y - self.previous_position[1]
self.local_data['dz'] = self.robot_parent.position_3d.z - self.previous_position[2]
# Compute the difference in orientation with the previous loop
dyaw = self.robot_parent.position_3d.yaw - self.previous_orientation[0]
dpitch = self.robot_parent.position_3d.pitch - self.previous_orientation[1]
droll = self.robot_parent.position_3d.roll - self.previous_orientation[2]
self.local_data['dyaw'] = morse_math.normalise_angle(dyaw)
self.local_data['dpitch'] = morse_math.normalise_angle(dpitch)
self.local_data['droll'] = morse_math.normalise_angle(droll)
# Store the 'new' previous data
self.previous_position = [self.robot_parent.position_3d.x, self.robot_parent.position_3d.y, self.robot_parent.position_3d.z]
self.previous_orientation = [self.robot_parent.position_3d.yaw, self.robot_parent.position_3d.pitch, self.robot_parent.position_3d.roll]
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 default_action(self):
""" Apply rotation to the arm segments """
# Get the reference to the Sound actuator
if self._sound == None:
logger.debug ("ACTIVATING THE SOUND ACTUATOR")
contr = bge.logic.getCurrentController()
self._sound = contr.actuators['Sound']
contr.activate(self._sound)
self._sound.stopSound()
# 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 = bge.logic.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
self._moving = False
for i in range(6):
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)
#logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, self._tolerance, rotation, ry))
elif self._dofs[i] == 'z':
rz = morse_math.rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
#logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, self._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")
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 = bge.logic.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
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)
# Store the variables to acces as a service:
self._current_pan = correct_pan
self._current_tilt = correct_tilt
if (abs(target_pan - correct_pan) < self._tolerance and \
abs(target_tilt - correct_tilt) < self._tolerance ):
self.completed((status.SUCCESS))
# 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):
""" Apply rotation to the arm segments """
# Get the reference to the Sound actuator
if self._sound == None:
logger.debug("ACTIVATING THE SOUND ACTUATOR")
contr = bge.logic.getCurrentController()
self._sound = contr.actuators['Sound']
contr.activate(self._sound)
self._sound.stopSound()
# 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 = bge.logic.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
self._moving = False
for i in range(6):
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)
#logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, self._tolerance, rotation, ry))
elif self._dofs[i] == 'z':
rz = morse_math.rotation_direction(segment_euler[2],
target_angle,
self._tolerance, rotation)
#logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, self._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")
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)
