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)