def default_action(self):
""" Main loop of the actuator.
Implements the component behaviour
"""
l_orientation = mathutils.Euler([self.local_data['left'], 0.0, 0.0])
self.left_eye.orientation = l_orientation.to_matrix()
r_orientation = mathutils.Euler([self.local_data['right'], 0.0, 0.0])
self.right_eye.orientation = r_orientation.to_matrix()
def reflectAttributeValues(self,
object,
attributes,
tag,
order,
transport,
time=None,
retraction=None):
scene = blenderapi.scene()
obj_name = self._rtia.getObjectInstanceName(object)
logger.debug("RAV %s", obj_name)
try:
obj = scene.objects[obj_name]
if self.in_position in attributes:
pos, offset = MorseVector.unpack(attributes[self.in_position])
# Update the positions of the robots
obj.worldPosition = pos
if self.in_orientation in attributes:
ori, offset = MorseVector.unpack(
attributes[self.in_orientation])
# Update the orientations of the robots
obj.worldOrientation = mathutils.Euler(ori).to_matrix()
except KeyError as detail:
logger.debug("Robot %s not found in this simulation scenario," + \
"but present in another node. Ignoring it!", obj_name)
def default_action(self):
""" Change the parent robot orientation. """
if self._speed == float('inf'):
# New parent orientation
orientation = mathutils.Euler([self.local_data['roll'],
self.local_data['pitch'],
self.local_data['yaw']])
self.robot_parent.force_pose(None, orientation.to_matrix())
else:
goal = [self.local_data['roll'], self.local_data['pitch'], self.local_data['yaw']]
current_rot = [self.position_3d.roll, self.position_3d.pitch, self.position_3d.yaw]
cmd = [0.0, 0.0, 0.0]
for i in range(0, 3):
diff = goal[i] - current_rot[i]
diff = normalise_angle(diff)
diff_abs = abs(diff)
if diff_abs < self._tolerance:
cmd[i] = 0.0
else:
sign = diff_abs / diff
if diff_abs > self._speed / self._frequency:
cmd[i] = sign * self._speed
else:
cmd[i] = diff_abs * self._frequency
if self._type == 'Position':
cmd[i] /= self._frequency
self.robot_parent.apply_speed(self._type, [0.0, 0.0, 0.0], cmd)
def default_action(self):
""" Change the parent robot orientation. """
# New parent orientation
orientation = mathutils.Euler([self.local_data['roll'],
self.local_data['pitch'],
self.local_data['yaw']])
self.robot_parent.force_pose(None, orientation.to_matrix())
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
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):
""" Change the parent robot orientation. """
# Get the Blender object of the parent robot
parent = self.robot_parent.bge_object
# 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.worldOrientation = orientation.to_matrix()
parent.restoreDynamics()
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 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):
""" 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 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
last_yaw = self.arc_pos.yaw
# 200 steps in the circle (one step is 1.8 degree)
self.bearing_index += 1
if self.bearing_index == self.nSteps:
self.bearing_index = 0
self.local_data['bearing'] = self.bearing_index #* self.stepSize
self.arc_pos.rotation = mathutils.Euler(
[0.0, 0.0, self.bearing_index * self.stepSize])
self.local_data['scan_list'] = [0 for i in range(self.nBins)]
for ray in self._ray_list:
# Transform the ray to the current position and rotation
# of the sensor
mat = self.arc_pos.transformation3d_with(self.position_3d)
correct_ray = mat.matrix * ray
# Shoot a ray towards the target
target, point, normal = self.bge_object.rayCast(
correct_ray, None, self.laser_range, "", 0, 0, 0)
#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
ray_vec = self.position_3d.translation - point
ray_vec.normalize()
echo_intensity = ray_vec.dot(normal)
if echo_intensity < 0:
print(echo_intensity)
bin = int((distance / self.laser_range) * self.nBins)
self.local_data['scan_list'][
bin] += echo_intensity * self.rayNormalizationGain
#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]
correct_ray.normalize()
new_point = inverse * (correct_ray * self.laser_range)
# Save the information gathered
self.local_data['point_list'][index] = new_point[:]
self.local_data['range_list'][index] = distance
index += 1
# Modify arc drawing
self.change_arc()