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)
