diff --git a/source/move.py b/source/move.py index 79c72bd..b19bdc0 100644 --- a/source/move.py +++ b/source/move.py @@ -1,20 +1,29 @@ import bpy +import math from random import random from mathutils import Matrix, Quaternion -from typing import Tuple +from typing import Tuple, Iterable +class RandomBounds: + def __init__(self, lower_bound, upper_bound): + self.lower_bound = lower_bound + self.upper_bound = upper_bound + + def random(self): + return random() * (self.upper_bound - self.lower_bound) + self.lower_bound class ArmGraspController: """Class to help with controlling a single arm in blender. """ - def __init__(self, armature_name: str, wrist_object_name: str, + def __init__(self, armature_name: str, wrist_controller_name: str, + shoulder_controller_name: str, start_pos: Tuple[float] = (1.6766993999481201, 0.3146645724773407, -1.3483989238739014)): """Constructor for ArmGraspController Args: armature_name (str): The exact name of the blender armature to control. wrist_object_name (str): The exact name of the blender object which is used to control the arm's IK solutions. - start_pos (Tuple[float]): 3D wrist position to start in. + start_pos (Tuple[float]): 3D wrist position to start in. (This position is relative to the shoulder.) """ self.armature_name = armature_name self.arm = bpy.data.objects[self.armature_name] @@ -23,9 +32,18 @@ class ArmGraspController: self.thumb_rot_name = "thumb_3" self.upper_arm_name = "upper_arm" self.upper_arm_bone = self.arm.pose.bones[self.upper_arm_name] - self.wrist_object_name = wrist_object_name - self.wrist_object = bpy.data.objects[self.wrist_object_name] - self.start_pos = start_pos + self.lower_arm_name = "lower_arm" + self.lower_arm_bone = self.arm.pose.bones[self.lower_arm_name] + self.wrist_controller_name = wrist_controller_name + self.wrist_controller = bpy.data.objects[self.wrist_controller_name] + self.shoulder_controller_name = shoulder_controller_name + self.shoulder_controller = bpy.data.objects[self.shoulder_controller_name] + + self.optimal_range_factor = 0.95 # Used to modify the max range so that the arm never fully stretches. + self.arm_range = (self.upper_arm_bone.length + self.lower_arm_bone.length) * self.optimal_range_factor + + self.start_pos_wrist = start_pos + self.start_pos_shoulder = self.get_shoulder_pos().copy() def deselect_all_bones(self): """Deselects all bones in the armature (the arm). @@ -33,18 +51,64 @@ class ArmGraspController: for bone in self.arm.pose.bones: # Deselect all selected bones bone.bone.select = False - def move(self, new_pos: Tuple[float]): + @staticmethod + def vec_add(vec1: Tuple[float], vec2: Tuple[float]): + return [x+y for (x,y) in zip(vec1, vec2)] + + @staticmethod + def distance(vec: Iterable[float]): + return math.sqrt(sum(i**2 for i in vec)) + + @staticmethod + def vec_diff(vec1, vec2): + return [x-y for (x,y) in zip(vec1, vec2)] + + @staticmethod + def normalize_vec(vec): + dist = ArmGraspController.distance(vec) + return [i/dist for i in vec] + + def get_shoulder_pos(self): + return self.shoulder_controller.location + + def distance_from_shoulder(self, pos): + return self.distance(self.vec_diff(pos, self.get_shoulder_pos())) + + def move_wrist(self, new_pos: Tuple[float]): """Moves wrist to new position. Args: - new_pos (Tuple[float]): New wrist position. Tuple size: 3 floats. + new_pos (Tuple[float]): New wrist position, relative to shoulder. Tuple size: 3 floats. """ - self.wrist_object.location = new_pos - self.wrist_object.keyframe_insert(data_path='location', index=-1) + # self.wrist_object.location = self.vec_add(new_pos, self.upper_arm_bone.head) + self.wrist_controller.location = new_pos + self.wrist_controller.keyframe_insert(data_path='location', index=-1) def move_shoulder(self, new_pos: Tuple[float]): - self.upper_arm_bone.location = new_pos - self.upper_arm_bone.keyframe_insert(data_path='location') + self.shoulder_controller.location = new_pos + self.shoulder_controller.keyframe_insert(data_path='location', index=-1) + + def set_translation_matrix(self, bone, new_pos): + for i in range(3): + bone.matrix[i][3] = new_pos[i] + + def move_arm(self, new_pos: Tuple[float]): + if(self.distance_from_shoulder(new_pos) <= self.arm_range): + # Do standard move + self.move_wrist(new_pos) + else: + # Combine both shoulder and arm move + shoulder_pos = self.get_shoulder_pos() + diff = self.vec_diff(new_pos, shoulder_pos) + diff_2 = self.vec_diff(diff, [i*self.arm_range for i in self.normalize_vec(diff)]) + move = self.vec_add(shoulder_pos, diff_2) + self.move_shoulder(move) + + self.move_wrist(new_pos) + + def move_back(self): + self.move_shoulder(self.start_pos_shoulder) + self.move_wrist(self.start_pos_wrist) def open_fingers(self): self.grab_movement(0.8) @@ -102,19 +166,14 @@ frame_number = 0 max_dist = 0.5 #start_location = ob.location.copy() start_location = (1.6766993999481201, 0.3146645724773407, -1.3483989238739014) -controller = ArmGraspController("Armature", "Sphere") +controller = ArmGraspController("Armature", "WristController", "ShoulderController") +new_pos = [1.7, 2.5, -1.3483989238739014] -for i in range(12): +x_rand = RandomBounds(0, 2) +y_rand = RandomBounds(1, 5) +z_rand = RandomBounds(-3, 2) + +for i in range(10): bpy.context.scene.frame_set(frame_number) - - x = random()*max_dist - y = random()*max_dist - z = random()*max_dist - controller.move(( - start_location[0] + x, - start_location[1] + y, - start_location[2] + z - )) - - controller.move_fingers() - frame_number += 20 \ No newline at end of file + controller.move_arm([x_rand.random(), y_rand.random(), z_rand.random()]) + frame_number += 20