def _add_motion_primitive(self, action_data_key, structure_key, motion_primitive_name): #extract mp key name from file name splitted_name = (motion_primitive_name[:-self.type_offset]).split("_") action_offset = len(splitted_name[0]) + 1 mp_data_key = motion_primitive_name[action_offset:-self.type_offset] self.graph_data[action_data_key]["nodes"][mp_data_key] = {} self.graph_data[action_data_key]["nodes"][mp_data_key][ "name"] = motion_primitive_name[:-self.type_offset] mm_string = self.zip_file.read( self._get_motion_primitive_file_path( structure_key, motion_primitive_name)).decode("utf-8") mm_data = json.loads(mm_string) self.graph_data[action_data_key]["nodes"][mp_data_key]["mm"] = mm_data if self.verbose: write_message_to_log( "\t" + "Load motion primitive " + motion_primitive_name, LOG_MODE_INFO) statsfile = structure_key + "/" + ( motion_primitive_name[:-self.type_offset] + ".stats") self._add_stats(action_data_key, mp_data_key, statsfile) space_partition_file = self._get_space_partitioning_file_path( structure_key, motion_primitive_name) #print space_partition_file self._add_space_partitioning_data_structure(action_data_key, mp_data_key, space_partition_file)
def get_best_transition_node(self): options, next_node_type = self.get_transition_options(self.state) n_transitions = len(options) if n_transitions == 1: next_node = options[0] elif n_transitions > 1: if self.trajectory is not None: next_node = self.select_next_step( self.state, options, add_orientation=self.constrain_transition_orientation) else: # use random transition if there is no path to follow random_index = random.randrange(0, n_transitions, 1) next_node = options[random_index] else: n_outgoing_edges = len(self.motion_state_graph.nodes[ self.state.current_node].outgoing_edges) write_message_to_log( "Error: Could not find a transition from state " + str(self.state.current_node) + " " + str(n_outgoing_edges)) next_node = self.node_group.get_random_start_state() next_node_type = self.motion_state_graph.nodes[next_node].node_type if next_node is None: write_message_to_log( "Error: Could not find a transition of type " + next_node_type + " from state " + str(self.state.current_node)) return next_node, next_node_type
def init_from_dict(self, action_name, desc): self.name = desc["name"] self.action_name = action_name write_message_to_log("Init motion state " + self.name, LOG_MODE_DEBUG) skeleton = self.motion_state_group.motion_state_graph.mgrd_skeleton self._initialize_from_json( skeleton, desc["mm"], self.motion_state_group.motion_state_graph.animated_joints) if "space_partition_pickle" in list(desc.keys()): self.cluster_tree = desc["space_partition_pickle"] if "space_partition_json" in list(desc.keys()): tree_data = desc["space_partition_json"] self.cluster_tree = FeatureClusterTree(None, None, None, None, None) data = np.array(tree_data["data"]) features = np.array(tree_data["features"]) options = tree_data["options"] self.cluster_tree.build_node_from_json(data, features, options, tree_data["root"]) if "stats" in list(desc.keys()): self.parameter_bb = desc["stats"]["pose_bb"] self.cartesian_bb = desc["stats"]["cartesian_bb"] self.velocity_data = desc["stats"]["pose_velocity"]
def __init__(self, service_config_file, json_path_expressions): # Load configuration files service_config = load_json_file(service_config_file) update_data_using_jsonpath(service_config, json_path_expressions) algorithm_config_file = "config" + os.sep + service_config[ "algorithm_settings"] + "_algorithm.config" if os.path.isfile(algorithm_config_file): write_message_to_log( "Load algorithm configuration from " + algorithm_config_file, LOG_MODE_INFO) algorithm_config = load_json_file(algorithm_config_file) else: write_message_to_log( "Did not find algorithm configuration file " + algorithm_config_file, LOG_MODE_INFO) algorithm_config = DEFAULT_ALGORITHM_CONFIG # Construct morphable graph from files self.application = MGRestApplication( service_config, algorithm_config, [ (r"/run_morphablegraphs", GenerateMotionHandler), #legacy (r"/config_morphablegraphs", SetConfigurationHandler), (r"/generate_motion", GenerateMotionHandler), (r"/get_skeleton", GetSkeletonHandler) ]) self.port = service_config["port"]
def post(self): global context try: mg_input = json.loads(self.request.body.decode("utf-8")) except Exception as e: error_string = "Error: Could not decode request body as JSON." + str( e.args) write_message_to_log(error_string, LOG_MODE_ERROR) self.write(error_string) return try: id = context.request_count print("start task", id) context.request_count += 1 fut = pool.submit(generate_motion, context, mg_input) while not fut.done(): yield gen.sleep(0.2) # start process and wait until it is done result_str = fut.result() print("end task", id) self.write(result_str) except Exception as e: print("caught exception in get") self.write("Caught an exception: %s" % e) raise finally: self.finish()
def get_random_action_transition(self, graph_walk, action_name, is_cycle=False): """ Get random start state based on edge from previous elementary action if possible """ next_node = None if graph_walk.step_count > 0: prev_node_key = graph_walk.steps[-1].node_key if prev_node_key in list(self.nodes.keys()): next_node = self.nodes[ prev_node_key].generate_random_action_transition( action_name, is_cycle) write_message_to_log( "Generate start from transition of last action " + str(prev_node_key) + str(next_node), mode=LOG_MODE_DEBUG) # if there is no previous elementary action or no action transition # use transition to random start state if next_node is None or next_node not in self.node_groups[ action_name].nodes: #print next_node, "not in", action_name next_node = self.node_groups[action_name].get_random_start_state() write_message_to_log("Generate random start" + str(next_node), mode=LOG_MODE_DEBUG) return next_node
def optimize_time_parameters_over_graph_walk(self, graph_walk): for idx, ea in enumerate(graph_walk.elementary_action_list): #print "evaluate", ea.action_name,ea.start_step,ea.end_step prev_action_idx = max( idx - (self.optimized_actions_for_time_constraints - 1), 0) start_step = graph_walk.elementary_action_list[ prev_action_idx].start_step end_step = ea.end_step time_constraints = TimeConstraintsBuilder( graph_walk, start_step, end_step).build(self.motion_primitive_graph, graph_walk) if time_constraints is not None: data = ( self.motion_primitive_graph, graph_walk, time_constraints, self._algorithm_config["global_time_optimization_settings"] ["error_scale_factor"], self._algorithm_config["global_time_optimization_settings"] ["quality_scale_factor"]) self.time_error_minimizer.set_objective_function_parameters( data) #initial_guess = graph_walk.get_global_time_parameter_vector(start_step) initial_guess = time_constraints.get_initial_guess(graph_walk) #print "initial_guess",prev_action_idx, time_constraints.start_step, end_step, initial_guess, time_constraints.constraint_list optimal_parameters = self.time_error_minimizer.run( initial_guess) #print "result ",optimal_parameters graph_walk.update_time_parameters(optimal_parameters, start_step, end_step) #graph_walk.convert_graph_walk_to_quaternion_frames(start_step, use_time_parameters=True) else: write_message_to_log( "No time constraints for action " + str(idx), LOG_MODE_DEBUG) return graph_walk
def __init__(self, motion_state_graph, mg_input, algorithm_config, start_pose=None, create_ca_vis_data=False): self.elementary_action_list = [] self.steps = [] self.motion_state_graph = motion_state_graph self.step_count = 0 self.mg_input = mg_input self._algorithm_config = algorithm_config self.motion_vector = MotionVector(self.motion_state_graph.skeleton, algorithm_config) if start_pose is None: start_pose = mg_input.get_start_pose() self.motion_vector.start_pose = start_pose smoothing_settings = algorithm_config["smoothing_settings"] self.spatial_smoothing_method = "smoothing" self.apply_smoothing = smoothing_settings[ "spatial_smoothing"] # set whether the exported motion is smoothed at transitions if "spatial_smoothing_method" in smoothing_settings: self.spatial_smoothing_method = smoothing_settings[ "spatial_smoothing_method"] self.motion_vector.apply_spatial_smoothing = False # deactivate smoothing during the synthesis self.use_time_parameters = algorithm_config["activate_time_variation"] self.constrain_place_orientation = algorithm_config[ "inverse_kinematics_settings"]["constrain_place_orientation"] write_message_to_log( "Use time parameters" + str(self.use_time_parameters), LOG_MODE_DEBUG) self.keyframe_event_list = KeyframeEventList(create_ca_vis_data) self.place_action_list = DEFAULT_PLACE_ACTION_LIST
def update_data_using_jsonpath(data, expressions, split_str="="): """ Takes a dictionary and a list of expressions in the form JSONPath=value, e.g. "$.write_log=True". Expressions and values should not contain the split_str="=" """ for expr in expressions: expr_t = expr.split(split_str) json_path_str = expr_t[0] value = expr_t[1] match = search_for_path(data, json_path_str) if match is not None: before = match path_list = iter(get_path_from_string(json_path_str)) value_type = get_type_of_string(value) update_json(data, path_list, TYPE_CONVERTER[value_type](value)) match = search_for_path(data, json_path_str) message = "set value of " + json_path_str + " from " + str( before) + " to " + str(match) + " with " + str(type(match)) write_message_to_log(message, LOG_MODE_DEBUG) else: write_message_to_log( "Warning: Did not find JSONPath " + json_path_str + " in data", LOG_MODE_ERROR)
def get_average_keyframe_constraint_error(self): keyframe_constraint_errors = [] step_index = 0 prev_frames = None for step_idx, step in enumerate(self.steps): quat_frames = self.motion_state_graph.nodes[ step.node_key].back_project( step.parameters, use_time_parameters=False).get_motion_vector() skeleton = self.motion_vector.skeleton aligned_frames = align_and_concatenate_frames( skeleton, skeleton.aligning_root_node, quat_frames, prev_frames, self.motion_vector.start_pose, 0) for c_idx, constraint in enumerate( step.motion_primitive_constraints.constraints): if constraint.constraint_type in [SPATIAL_CONSTRAINT_TYPE_KEYFRAME_POSITION, SPATIAL_CONSTRAINT_TYPE_TWO_HAND_POSITION] and\ not "generated" in list(constraint.semantic_annotation.keys()): error = constraint.evaluate_motion_sample(aligned_frames) write_message_to_log( "Error of Keyframe constraint " + str(step_idx) + "-" + str(c_idx) + ": " + str(error), LOG_MODE_DEBUG) keyframe_constraint_errors.append(error) prev_frames = aligned_frames step_index += 1 if len(keyframe_constraint_errors) > 0: return np.average(keyframe_constraint_errors) else: return -1
def _create_ik_constraints2(self): ik_constraints = collections.OrderedDict() for idx, action in enumerate(self.elementary_action_list): write_message_to_log( "Create IK constraints for action" + " " + str(idx) + " " + str(action.start_step) + " " + str(self.steps[action.start_step].start_frame), LOG_MODE_DEBUG) if not self.constrain_place_orientation and action.action_name in self.place_action_list: constrain_orientation = False else: constrain_orientation = True start_step = action.start_step end_step = action.end_step frame_offset = self.steps[start_step].start_frame for step in self.steps[start_step:end_step + 1]: time_function = None if self.use_time_parameters and self.motion_state_graph.nodes[ step.node_key].get_n_time_components() > 0: time_function = self.motion_state_graph.nodes[ step.node_key].back_project_time_function( step.parameters) step_constraints = step.motion_primitive_constraints.convert_to_ik_constraints( self.motion_state_graph, frame_offset, time_function, constrain_orientation, version=2) ik_constraints.update(step_constraints) frame_offset += step.end_frame - step.start_frame + 1 return ik_constraints
def optimize_for_collision_avoidance_constraints(self, graph_walk, action_constraints, start_step=0): reduced_motion_vector = deepcopy(graph_walk.motion_vector) reduced_motion_vector.reduce_frames( graph_walk.steps[start_step].start_frame) write_message_to_log( "start frame " + str(graph_walk.steps[start_step].start_frame), LOG_MODE_DEBUG) step_index = start_step n_steps = len(graph_walk.steps) print(reduced_motion_vector.n_frames, graph_walk.get_num_of_frames(), reduced_motion_vector.n_frames - graph_walk.get_num_of_frames()) while step_index < n_steps: node = self.motion_primitive_graph.nodes[ graph_walk.steps[step_index].node_key] print(graph_walk.steps[step_index].node_key, node.n_canonical_frames, graph_walk.steps[step_index].start_frame) motion_primitive_constraints = MotionPrimitiveConstraints() active_constraint = False for trajectory in action_constraints.collision_avoidance_constraints: if reduced_motion_vector.frames is not None: trajectory.set_min_arc_length_from_previous_frames( reduced_motion_vector.frames) else: trajectory.min_arc_length = 0.0 trajectory.set_number_of_canonical_frames( node.n_canonical_frames) #discrete_trajectory = trajectory.create_discrete_trajectory(original_frames[step.start_frame:step.end_frame]) motion_primitive_constraints.constraints.append(trajectory) active_constraint = True if active_constraint: data = (node, motion_primitive_constraints, reduced_motion_vector.frames, self._algorithm_config["local_optimization_settings"] ["error_scale_factor"], self._algorithm_config["local_optimization_settings"] ["quality_scale_factor"]) self.collision_avoidance_error_minimizer.set_objective_function_parameters( data) graph_walk.steps[ step_index].parameters = self.collision_avoidance_error_minimizer.run( graph_walk.steps[step_index].parameters) motion_primitive_sample = node.back_project( graph_walk.steps[step_index].parameters, use_time_parameters=False) reduced_motion_vector.append_quat_frames( motion_primitive_sample.get_motion_vector()) step_index += 1 write_message_to_log( "start frame " + str(step_index) + " " + str(len(graph_walk.steps)), LOG_MODE_DEBUG) assert (len(graph_walk.motion_vector.frames)) == len( reduced_motion_vector.frames), (str( len(graph_walk.motion_vector.frames))) + "," + str( len(reduced_motion_vector.frames)) graph_walk.motion_vector = reduced_motion_vector return graph_walk
def main(): parser = argparse.ArgumentParser( description="Start the MorphableGraphs REST-interface") parser.add_argument( "-set", nargs='+', default=[], help="JSONPath expression, e.g. -set $.model_data=path/to/data") parser.add_argument("-config_file", nargs='?', default=SERVICE_CONFIG_FILE, help="Path to default config file") parser.add_argument("-target_skeleton", nargs='?', default=None, help="Path to target skeleton file") parser.add_argument("-skeleton_scale", nargs='?', default=1.0, help="Scale applied to the target skeleton offsets") args = parser.parse_args() if os.path.isfile(args.config_file): mg_service = MGRESTInterface(args.config_file, args.set) if args.target_skeleton is not None: mg_service.set_target_skeleton(args.target_skeleton, scale_factor=args.skeleton_scale) mg_service.start() else: write_message_to_log( "Error: could not open service or algorithm configuration file", LOG_MODE_ERROR)
def update_spatial_parameters(self, parameter_vector, start_step=0): write_message_to_log("Update spatial parameters", LOG_MODE_DEBUG) offset = 0 for step in self.steps[start_step:]: new_alpha = parameter_vector[offset:offset + step.n_spatial_components] step.parameters[:step.n_spatial_components] = new_alpha offset += step.n_spatial_components
def _generate_action(self, action_constraints): """ Extends the graph walk with an action based on the given constraints. Parameters --------- * action_constraints: ActionConstraints Constraints for the action """ self.mp_generator = MotionPrimitiveGenerator(action_constraints, self._algorithm_config) self.mp_constraints_builder.set_action_constraints(action_constraints) action_state = MotionGeneratorState(self._algorithm_config) if action_constraints.root_trajectory is not None: max_arc_length = action_constraints.root_trajectory.full_arc_length else: max_arc_length = np.inf action_state.initialize_from_previous_graph_walk( self.graph_walk, max_arc_length, action_constraints.cycled_next) arc_length_of_end = self.get_end_step_arc_length(action_constraints) optimization_steps = self.graph_walk_optimizer._global_spatial_optimization_steps self.graph_walk_planner.set_state(self.graph_walk, self.mp_generator, action_state, action_constraints, arc_length_of_end) node_key = self.graph_walk_planner.get_best_start_node() self._generate_motion_primitive(action_constraints, node_key, action_state) while not action_state.is_end_state(): self.graph_walk_planner.set_state(self.graph_walk, self.mp_generator, action_state, action_constraints, arc_length_of_end) node_key, next_node_type = self.graph_walk_planner.get_best_transition_node( ) self._generate_motion_primitive(action_constraints, node_key, action_state, next_node_type == NODE_TYPE_END) if self.activate_global_optimization and action_state.temp_step % optimization_steps == 0: start_step = action_state.temp_step - optimization_steps self.graph_walk_optimizer.optimize_spatial_parameters_over_graph_walk( self.graph_walk, self.graph_walk.step_count + start_step) self.graph_walk.step_count += action_state.temp_step self.graph_walk.update_frame_annotation( action_constraints.action_name, action_state.action_start_frame, self.graph_walk.get_num_of_frames()) self.graph_walk = self.graph_walk_optimizer.optimize( self.graph_walk, action_state, action_constraints) self.graph_walk.add_entry_to_action_list( action_constraints.action_name, action_state.start_step, len(self.graph_walk.steps) - 1, action_constraints) write_message_to_log( "Reached end of elementary action " + action_constraints.action_name, LOG_MODE_INFO)
def _construct_space_partition(self, cluster_file_name, reconstruct=False): self.cluster_tree = None if not reconstruct and os.path.isfile(cluster_file_name + "cluster_tree.pck"): write_message_to_log( "Load space partitioning data structure" + self.name, LOG_MODE_DEBUG) self.cluster_tree = ClusterTree() self.cluster_tree.load_from_file_pickle(cluster_file_name + "cluster_tree.pck")
def start(self): """ Start the web server loop """ if self.application.is_initiated(): write_message_to_log("Start listening to port " + str(self.port), LOG_MODE_INFO) self.application.listen(self.port) tornado.ioloop.IOLoop.instance().start() else: write_message_to_log("Error: Could not initiate MG REST service", LOG_MODE_ERROR)
def get_action_from_keyframe(self, keyframe): found_action_index = -1 step_index = self.get_step_from_keyframe(keyframe) write_message_to_log("Found keyframe in step " + str(step_index), LOG_MODE_DEBUG) if step_index < 0: return found_action_index for action_index, action in enumerate(self.elementary_action_list): if action.start_step <= step_index <= action.end_step: found_action_index = action_index return found_action_index
def build(self, motion_primitive_graph, graph_walk): if self.n_time_constraints > 0: write_message_to_log( "Found " + str(self.n_time_constraints) + " time constraints", LOG_MODE_DEBUG) return TimeConstraints(motion_primitive_graph, graph_walk, self.start_step, self.end_step, self.time_constraint_list) else: write_message_to_log("Did not find time constraints", LOG_MODE_DEBUG) return None
def _load_mgrd_model_from_mgrd_json(self, skeleton, mm_data, use_mgrd_mixture_model=True, scale=None): write_message_to_log("Init motion primitive model with semantic annotation", LOG_MODE_INFO) sspm = MGRDQuaternionSplineModel.load_from_json(skeleton, mm_data['sspm']) tspm = MGRDTemporalSplineModel.load_from_json(mm_data['tspm']) mixture_model = MotionPrimitiveModelWrapper.load_mixture_model({ 'gmm_covars': mm_data['gmm']['covars'], 'gmm_means': mm_data['gmm']['means'], 'gmm_weights': mm_data['gmm']['weights'] }, use_mgrd_mixture_model) if scale is not None: self._pre_scale_root_translation(sspm, scale) return MGRDMotionPrimitiveModel(skeleton, sspm, tspm, mixture_model)
def retarget_motion_vector(src_motion_vector, target_skeleton, scale_factor=1): write_message_to_log("Start retargeting...", LOG_MODE_INFO) target_frames = retarget_from_src_to_target(src_motion_vector.skeleton, target_skeleton, src_motion_vector.frames, GAME_ENGINE_TO_ROCKETBOX_MAP, None, scale_factor) target_skeleton.frame_time = src_motion_vector.skeleton.frame_time target_motion_vector = AnnotatedMotionVector(target_skeleton) target_motion_vector.frame_time = src_motion_vector.frame_time target_motion_vector.frames = target_frames return target_motion_vector
def get_transition_type_for_action(self, graph_walk, action_constraint): prev_node = graph_walk.steps[-1].node_key n_standard_transitions = len( self.get_n_standard_transitions(prev_node)) if n_standard_transitions > 0: next_node_type = NODE_TYPE_STANDARD else: next_node_type = NODE_TYPE_END write_message_to_log( "Generate " + str(next_node_type) + " transition without trajectory " + str(n_standard_transitions) + " " + str(prev_node), LOG_MODE_DEBUG) if action_constraint.cycled_next and next_node_type == NODE_TYPE_END: next_node_type = NODE_TYPE_CYCLE_END return next_node_type
def _export_motion_to_file(self, bvh_string, motion_vector): bvh_filename = self.application.service_config[ "output_dir"] + os.sep + self.application.service_config[ "output_filename"] if self.application.add_timestamp_to_filename: bvh_filename += "_" + str(datetime.now().strftime("%d%m%y_%H%M%S")) write_message_to_log("export motion to file " + bvh_filename, LOG_MODE_DEBUG) with open(bvh_filename + ".bvh", "wb") as out_file: out_file.write(bvh_string) if motion_vector.mg_input is not None: write_to_json_file(bvh_filename + "_input.json", motion_vector.mg_input.mg_input_file) if motion_vector.keyframe_event_list is not None: motion_vector.keyframe_event_list.export_to_file(bvh_filename)
def _create_ik_constraints(self): ik_constraints = [] for idx, action in enumerate(self.elementary_action_list): write_message_to_log( "Create IK constraints for action" + " " + str(idx) + " " + str(action.start_step) + " " + str(self.steps[action.start_step].start_frame), LOG_MODE_DEBUG) if not self.constrain_place_orientation and action.action_name in self.place_action_list: constrain_orientation = False else: constrain_orientation = True start_step = action.start_step end_step = action.end_step elementary_action_ik_constraints = dict() elementary_action_ik_constraints["keyframes"] = dict() elementary_action_ik_constraints["trajectories"] = list() elementary_action_ik_constraints["collision_avoidance"] = list() frame_offset = self.steps[start_step].start_frame for step in self.steps[start_step:end_step + 1]: time_function = None if self.use_time_parameters and self.motion_state_graph.nodes[ step.node_key].get_n_time_components() > 0: time_function = self.motion_state_graph.nodes[ step.node_key].back_project_time_function( step.parameters) step_keyframe_constraints = step.motion_primitive_constraints.convert_to_ik_constraints( self.motion_state_graph, frame_offset, time_function, constrain_orientation) elementary_action_ik_constraints["keyframes"].update( step_keyframe_constraints) elementary_action_ik_constraints[ "collision_avoidance"] += step.motion_primitive_constraints.get_ca_constraints( ) frame_offset += step.end_frame - step.start_frame + 1 if self._algorithm_config[ "collision_avoidance_constraints_mode"] == "ik": elementary_action_ik_constraints[ "trajectories"] += self._create_ik_trajectory_constraints_from_ca_trajectories( idx) elementary_action_ik_constraints[ "trajectories"] += self._create_ik_trajectory_constraints_from_annotated_trajectories( idx) ik_constraints.append(elementary_action_ik_constraints) return ik_constraints
def post(self): try: mg_input = json.loads(self.request.body.decode("utf-8")) except Exception as e: error_string = "Error: Could not decode request body as JSON." + str( e.args) write_message_to_log(error_string, LOG_MODE_ERROR) self.write(error_string) return motion_vector = self.application.generate_motion(mg_input, False) if motion_vector is not None: self._handle_result(mg_input, motion_vector) else: error_string = "Error: Could not generate motion." self.write(error_string)
def _load_mgrd_model_from_legacy_json(self, mgrd_skeleton, data, use_mgrd_mixture_model, animated_joints): write_message_to_log("Init motion primitive model without semantic annotation", LOG_MODE_DEBUG) mm = MotionPrimitiveModelWrapper.load_mixture_model(data, use_mgrd_mixture_model) tspm = LegacyTemporalSplineModel(data) sspm = MGRDQuaternionSplineModel.load_from_json(mgrd_skeleton, { 'eigen': np.asarray(data['eigen_vectors_spatial']), 'mean': np.asarray(data['mean_spatial_vector']), 'n_coeffs': data['n_basis_spatial'], 'n_dims': data['n_dim_spatial'], 'knots': np.asarray(data['b_spline_knots_spatial']), 'degree': self.SPLINE_DEGREE, 'translation_maxima': np.asarray(data['translation_maxima']), 'animated_joints': animated_joints }) self._pre_scale_root_translation(sspm, data['translation_maxima']) return MGRDMotionPrimitiveModel(mgrd_skeleton, sspm, tspm, mm)
def _get_best_fit_sample_using_cluster_tree(self, graph_node, constraints, prev_frames, n_candidates=-1): """ Directed search in precomputed hierarchical space partitioning data structure """ n_candidates = self.n_cluster_search_candidates if n_candidates < 1 else n_candidates data = graph_node, constraints, prev_frames distance, s = graph_node.search_best_sample(self.objective, data, n_candidates) write_message_to_log( "Found best sample with distance: " + str(distance), LOG_MODE_DEBUG) constraints.min_error = distance return np.array(s)
def select_next_step(self, state, options, add_orientation=False): #next_node = self._look_one_step_ahead(state, options, add_orientation) mp_constraints = self._generate_node_evaluation_constraints( state, add_orientation) if state.current_node is None or True: errors, s_vectors = self._evaluate_options(state, mp_constraints, options) else: errors, s_vectors = self._evaluate_options_looking_ahead( state, mp_constraints, options, add_orientation) min_idx = np.argmin(errors) next_node = options[min_idx] write_message_to_log( "####################################Next node is" + str(next_node), LOG_MODE_DEBUG) return next_node
def _init_start_pose(self, mg_input): """ Sets the pose at the beginning of the elementary action sequence Estimates the optimal start orientation from the constraints if none is given. """ self.start_pose = mg_input.get_start_pose() if self.start_pose["orientation"] is None: root_trajectories = self._create_trajectory_constraints_for_joint( 0, self.motion_state_graph.skeleton.root) self.start_pose["orientation"] = [0, 0, 0] if len(root_trajectories) > 0: if root_trajectories[0] is not None: self.start_pose[ "orientation"] = self.get_start_orientation_from_trajectory( root_trajectories[0]) write_message_to_log( "Set start orientation from trajectory to" + str(self.start_pose["orientation"]), LOG_MODE_DEBUG)
def _post_process_motion(self, motion_vector, complete_motion_vector): """ Applies inverse kinematics constraints on a annotated motion vector and adds values for static DOFs that are not part of the motion model. Parameters ---------- * motion_vector : AnnotatedMotionVector Contains motion but also the constraints * complete_motion_vector: bool Sets DOFs that are not modelled by the motion model using default values. Returns ------- * motion_vector : AnnotatedMotionVector Contains a list of quaternion frames and their annotation based on actions. """ ik_settings = self._algorithm_config["inverse_kinematics_settings"] has_model = self._motion_state_graph.skeleton.skeleton_model is not None if self._algorithm_config[ "activate_motion_grounding"] and has_model and self.scene_interface is not None and "motion_grounding_settings" in self._algorithm_config: self.run_motion_grounding(motion_vector, ik_settings) #self.run_motion_grounding(motion_vector, ik_settings) if self._algorithm_config["activate_inverse_kinematics"]: me = MotionEditing( self._motion_state_graph.skeleton, self._algorithm_config["inverse_kinematics_settings"]) version = 1 if "version" in self._algorithm_config[ "inverse_kinematics_settings"]: version = self._algorithm_config[ "inverse_kinematics_settings"]["version"] write_message_to_log( "Modify using inverse kinematics" + str(version), LOG_MODE_INFO) if version == 1: me.modify_motion_vector(motion_vector) #me.fill_rotate_events(motion_vector) elif version == 2: me.modify_motion_vector2(motion_vector) if complete_motion_vector: motion_vector.frames = self._motion_state_graph.skeleton.add_fixed_joint_parameters_to_motion( motion_vector.frames)