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()