original_docstring: generate a sequence of walk motion primitives based on a sequence of target points 
docstring: generate a sequence of walk motion primitives based on a sequence of target points
docstring_tokens: generate a sequence of walk motion primitives based on a sequence of target points
code: def generate_locomotion_from_motion_primitives(self, start_node, start_node_type, pose_buffer, action_desc, end_idle=True):
          """generate a sequence of walk motion primitives based on a sequence of target points """
          success = True
          control_points = action_desc["control_points"]
          end_direction = action_desc["end_direction"]
          body_orientation_targets = action_desc["body_orientation_targets"]
          upper_body_gesture = action_desc["upper_body_gesture"]
          velocity_factor = action_desc["velocity_factor"]
  
          upper_body_state = None
          if upper_body_gesture is not None:
              upper_body_state = self.generate_upper_body_state(upper_body_gesture)
          path_state = dict()
          current_node = start_node
          node_type = start_node_type
          path_state["distance"] = np.inf
          path_state["prev_direction_vector"] = None
          path_state["prev_distance"] = np.inf
          path_state["prev_target"] = control_points[0]
          path_state["current_position"] = np.array(pose_buffer[-1][:3])
          print("start processing", control_points)
          add_noise = False
          while len(control_points) > 0 and not self.stop_thread:
              direction_vector, step_distance, path_state, control_points = self.get_direction_from_control_points(control_points, node_type, path_state, pose_buffer, end_direction, add_noise)
              if direction_vector is None:
                  print("target is none", current_node,  len(control_points) )
                  raise Exception("Error: direction_vector is None",current_node)
                  
              if self.collision_boundary is not None and step_distance > 0:
                  points = [np.array(pose_buffer[-1][:3])]
                  points.append(points[0]+step_distance*direction_vector)
                  if self.collision_boundary.check_trajectory(points, 0.0001):
                      print("stop due to collision")
                      control_points = list()
                      step_distance = 0
                      success = False
                      self.state_queue.pop_last_state()
                      last_state = self.state_queue.get_last_state()
                      if last_state is not None:
                          pose_buffer = self.state_queue.get_last_state().pose_buffer
              elif step_distance > 0:
                  print("no collision boundary defined")
              new_frames, current_node, node_type = self.generate_locomotion_step(current_node, node_type, direction_vector, step_distance, pose_buffer, body_orientation_targets, upper_body_state, velocity_factor, end_idle)
              self.state_queue.add_state_queue_entry(current_node, node_type, new_frames, pose_buffer, self.dt)
              path_state["prev_direction_vector"] = direction_vector
              path_state["prev_distance"] = path_state["distance"] 
              time.sleep(self.settings.sleep_time)
              print("generated state")
          return success, np.linalg.norm(path_state["current_position"]  - path_state["prev_target"])
code_tokens: def generate_locomotion_from_motion_primitives ( self , start_node , start_node_type , pose_buffer , action_desc , end_idle = True ) : success = True control_points = action_desc [ "control_points" ] end_direction = action_desc [ "end_direction" ] body_orientation_targets = action_desc [ "body_orientation_targets" ] upper_body_gesture = action_desc [ "upper_body_gesture" ] velocity_factor = action_desc [ "velocity_factor" ] upper_body_state = None if upper_body_gesture is not None : upper_body_state = self . generate_upper_body_state ( upper_body_gesture ) path_state = dict ( ) current_node = start_node node_type = start_node_type path_state [ "distance" ] = np . inf path_state [ "prev_direction_vector" ] = None path_state [ "prev_distance" ] = np . inf path_state [ "prev_target" ] = control_points [ 0 ] path_state [ "current_position" ] = np . array ( pose_buffer [ - 1 ] [ : 3 ] ) print ( "start processing" , control_points ) add_noise = False while len ( control_points ) > 0 and not self . stop_thread : direction_vector , step_distance , path_state , control_points = self . get_direction_from_control_points ( control_points , node_type , path_state , pose_buffer , end_direction , add_noise ) if direction_vector is None : print ( "target is none" , current_node , len ( control_points ) ) raise Exception ( "Error: direction_vector is None" , current_node ) if self . collision_boundary is not None and step_distance > 0 : points = [ np . array ( pose_buffer [ - 1 ] [ : 3 ] ) ] points . append ( points [ 0 ] + step_distance * direction_vector ) if self . collision_boundary . check_trajectory ( points , 0.0001 ) : print ( "stop due to collision" ) control_points = list ( ) step_distance = 0 success = False self . state_queue . pop_last_state ( ) last_state = self . state_queue . get_last_state ( ) if last_state is not None : pose_buffer = self . state_queue . get_last_state ( ) . pose_buffer elif step_distance > 0 : print ( "no collision boundary defined" ) new_frames , current_node , node_type = self . generate_locomotion_step ( current_node , node_type , direction_vector , step_distance , pose_buffer , body_orientation_targets , upper_body_state , velocity_factor , end_idle ) self . state_queue . add_state_queue_entry ( current_node , node_type , new_frames , pose_buffer , self . dt ) path_state [ "prev_direction_vector" ] = direction_vector path_state [ "prev_distance" ] = path_state [ "distance" ] time . sleep ( self . settings . sleep_time ) print ( "generated state" ) return success , np . linalg . norm ( path_state [ "current_position" ] - path_state [ "prev_target" ] )
short_docstring: generate a sequence of walk motion primitives based on a sequence of target points
short_docstring_tokens: generate a sequence of walk motion primitives based on a sequence of target points
comment: """generate a sequence of walk motion primitives based on a sequence of target points """