def get_global_yaw_degs(self): bone_uv = self.get_direction_uv() y_projected = Util.Utils().project_on_to_plane(bone_uv, Y_AXIS) yaw = Util.Utils().get_angle_between_degs(Z_AXIS, y_projected) if y_projected[0] < 0.0: return -yaw else: return yaw
def get_global_pitch_degs(self): bone_uv = self.get_direction_uv() x_projected = Util.Utils().project_on_to_plane(bone_uv, X_AXIS) pitch = Util.Utils().get_angle_between_degs(Z_AXIS, x_projected) if x_projected[1] < 0.0: return -pitch else: return pitch
def solve_for_rotations(self, outer_joint_orientation, inner_joint_orientation, bone_number): q1 = outer_joint_orientation q2 = inner_joint_orientation # finding the rotor that express rotation between two orientational frame(between outer and inner joint) rotor = Util.Utils().find_rotation_quaternion(q1, q2) if rotor[0] > 1: rotor[0] = 0.99 if rotor[0] < -1: rotor[0] = -0.99 needed_rotation = math.acos(rotor[0]) * 2 * (180 / np.pi) self.rotations[bone_number] = needed_rotation * (np.pi / 180) if needed_rotation <= self.bone_twist_limit: # if the rotation is inside the limited return Mat.Mat().multiply_two_quaternion(rotor, outer_joint_orientation) else: # the maximum allowed rotation angle theta = (self.bone_twist_limit) * (np.pi / 180) self.rotations[bone_number] = theta # the rotation axis if abs(rotor[0]) == 1: return rotor v1 = np.dot(rotor[1:], (1 / math.sqrt(1 - rotor[0] ** 2))) w = math.cos(theta / 2) x = v1[0] * math.sin(theta / 2) y = v1[1] * math.sin(theta / 2) z = v1[2] * math.sin(theta / 2) return [w, x, y, z]
def set_rotor_base_bone_constraint(self, rotor_type, constraint_axis, angle_degs): # Sanity checking if len(self.chain) == 0: raise Exception( "Chain must contain a basebone before we can specify the basebone constraint type." ) if len(constraint_axis) <= 0.0: raise Exception("Constraint axis cannot be zero.") if angle_degs < 0.0: angle_degs = 0.0 if angle_degs > 180.0: angle_degs = 180.0 # if rotor_type != "GLOBAL_ROTOR" or rotor_type != "LOCAL_ROTOR": # raise Exception("The only valid rotor types for this method are GLOBAL_ROTOR and LOCAL_ROTOR.") # Set the constraint type, axis and angle self.base_bone_constraint_type = rotor_type self.base_bone_constraint_uv = Util.Utils().normalization( constraint_axis) rotor_joint = Joint.Joint3D() rotor_joint.set_as_ball_joint(angle_degs) self.get_bone(0).set_joint(rotor_joint)
def set_hinge_base_bone_constraint( self, hinge_type, hinge_rotation_axis, cw_constraint_degs, acw_constraint_axis, hinge_reference_axis, ): # Sanity checking if len(self.chain) == 0: raise Exception( "Chain must contain a base bone before we can specify the base bone constraint type." ) if Util.Utils.length_calc(hinge_rotation_axis) <= 0.0: raise Exception("Constraint axis cannot be zero.") if Util.length_calc(hinge_reference_axis) <= 0.0: raise Exception("Constraint axis cannot be zero.") # if np.inner(hinge_rotation_axis, hinge_reference_axis) == 0: # # raise Exception( # "The hinge reference-axis must be in the plane of the hinge rotation axis, i.e" # ", they must not be perpendicular") # if hinge_type != "GLOBAL_HINGE" and hinge_type != "LOCAL_HINGE": # raise Exception("The only valid rotor types for this method are GLOBAL_HINGE and LOCAL_HINGE.") # Set the constraint type, axis and angle self.base_bone_constraint_type = hinge_type self.base_bone_constraint_uv = Util.normalization(hinge_rotation_axis) hinge_joint = Joint.Joint3D() if hinge_type == "GLOBAL_HINGE": hinge_joint.set_hinge("GLOBAL_HINGE", hinge_rotation_axis, cw_constraint_degs, acw_constraint_axis, hinge_reference_axis) else: hinge_joint.set_hinge("LOCAL_HINGE", hinge_rotation_axis, cw_constraint_degs, acw_constraint_axis, hinge_reference_axis) self.get_bone(0).set_joint(hinge_joint)
def solve_fabrik_for_Robot(base_bone, chain, target_position, target_orientation): dist_base_to_target = Util.Utils().get_distance_between( chain.get_bone(0).get_start_point_position(), target_position) total_chain_length = 0 for i in range(0, chain.get_chain_length()): total_chain_length += chain.get_bone(i).get_length() if dist_base_to_target <= total_chain_length: if chain.get_chain_length() == 0: raise Exception( "It makes no sense to solve an IK chain with zero bones.") dist_to_target = Util.Utils().get_distance_between( chain.get_bone(chain.get_chain_length() - 1).get_end_point_position(), target_position) counter = 0 m_FABRIK = fabrik.FABRIK(chain.get_chain_length(), target_position, target_orientation, chain.get_bone(0).is_fix_bone(), chain.get_base_bone_constraint_uv(), chain.get_base_location()) while dist_to_target > chain.get_solve_distance_threshold( ) and counter < 10000: chain = m_FABRIK.forward(chain) chain = m_FABRIK.backward(chain) dist_to_target = Util.Utils().get_distance_between( chain.get_bone(chain.get_chain_length() - 1).get_end_point_position(), target_position) counter += 1 # To add the fixed base bone here!! from left of list of bones chain.add_bone(base_bone) # The new bone is added from Left to the chains of bones so needs reordering new_chain = [chain.get_chain()[i] for i in [3, 0, 1, 2]] m_draw = draw_chain.RobotVisualization(target_position, target_orientation, new_chain, m_FABRIK.get_deg(), m_FABRIK.get_rotations()) m_draw.draw_chain() else: print("Target is so far! can't be reached") return
def set_hinge(self, joint_type, rotation_axis, clockwise_constraint_degs, anticlockwise_constraint_degs, reference_axis): dot = Util.Utils().dot_product(rotation_axis, reference_axis) # if Util.approximately_equal(dot, 0.0, 0.01) == 1: # angle_degs = Util.get_angle_between_degs(rotation_axis, reference_axis) # raise Exception( # "The reference axis must be in the plane of the hinge rotation axis - angle between them is currently: " + str( # angle_degs)) self.validate_constraint_angle_degs(clockwise_constraint_degs) self.validate_constraint_angle_degs(anticlockwise_constraint_degs) self.validate_axis(rotation_axis) self.validate_axis(reference_axis) self.hinge_clockwise_constraint_degs = clockwise_constraint_degs self.hinge_anticlockwise_constraint_degs = anticlockwise_constraint_degs self.joint_type = joint_type self.rotation_axis_uv = Util.Utils().normalization(rotation_axis) self.reference_axis_uv = Util.Utils().normalization(reference_axis)
def add_consecutive_freely_rotating_hinged_bone(self, bone_direction_uv, bone_length, joint_type, hinge_rotation_axis, is_fixed, bone_orientation): self.add_consecutive_hinged_bone( bone_direction_uv, bone_length, joint_type, hinge_rotation_axis, 180, 180, Util.Utils().gen_perpendicular_vector_quick(hinge_rotation_axis), is_fixed, bone_orientation)
def add_consecutive_hinged_bone(self, bone_direction_uv, bone_length, joint_type, hinge_rotation_axis, clockwise_degs, anticlockwise_deg, hinge_reference_axis, is_bone_fixed, bone_orientation): Util.Utils().validate_direction(bone_direction_uv) Util.Utils().validate_direction(hinge_rotation_axis) Util.Utils().validate_length(bone_length) if self.chain_length == 0: raise Exception( "You must add a base bone before adding a consectutive bone.") bone_direction_uv = (bone_direction_uv) hinge_rotation_axis = Util.Utils().normalization(hinge_rotation_axis) prev_bone_end = self.get_bone(self.chain_length - 1).get_end_point_position() scale_direction = [i * bone_length for i in bone_direction_uv] bone_end_location = [ x + y for x, y in zip(prev_bone_end, scale_direction) ] m_bone = Bone.Bone3D(prev_bone_end, bone_end_location, bone_direction_uv, bone_length, is_bone_fixed, bone_orientation) m_joint = Joint.Joint3D() if joint_type == "GLOBAL_HINGE": m_joint.set_as_global_hinge(hinge_rotation_axis, clockwise_degs, anticlockwise_deg, hinge_reference_axis) elif joint_type == "LOCAL_HINGE": m_joint.set_as_local_hinge(hinge_rotation_axis, clockwise_degs, anticlockwise_deg, hinge_reference_axis) else: raise Exception( "Hinge joint types may be only JointType.GLOBAL_HINGE or JointType.LOCAL_HINGE." ) m_bone.set_joint(m_joint) self.add_bone(m_bone)
def solve_fabrik_ik(self): dist_base_to_target = Util.Utils().get_distance_between( self.get_bone(0).get_start_point_position(), self.target_position) total_chain_length = 0 for i in range(0, self.chain_length): total_chain_length += self.get_bone(i).get_length() # print("The initial joint angles and diagram. close the figure to see the final configuration\n") # self.draw_chain() if dist_base_to_target <= total_chain_length: if self.get_chain_length == 0: raise Exception( "It makes no sense to solve an IK chain with zero bones.") dist_to_target = Util.Utils().get_distance_between( self.get_bone(self.chain_length - 1).get_end_point_position(), self.target_position) counter = 0 while dist_to_target > self.get_solve_distance_threshold( ) and counter < 10000: self.forward(self.target_position, self.target_orientation) self.backward() dist_to_target = Util.Utils().get_distance_between( self.get_bone(self.get_chain_length() - 1).get_end_point_position(), self.target_position) counter += 1 # self.draw_chain() # after finding these joint position we can do anything with them. # Here I calculate the joints_angle: # print("Final joint angles:\n") self.draw_chain() # self.output_joint_angles() else: print("Target is so far! can't be reached") return
def angles(self): angles = [] # for base bone rotation base_bone = self.chain[0] base_bone_orientation = base_bone.get_bone_orientation() rotations_base_bone = Util.Utils().find_rotation_quaternion(self.chain[1].bone_orientation, base_bone_orientation) # number 3 belongs to the rotation matrix which is 0 for bone 3 rotation, 1 for bone 5 rotation, # 2 for bone 7 rotation, 3 for bone 0 rotation # self.solve_for_orientation(base_bone_orientation, self.fixed_base_orientation, 3) angles.append(math.acos(rotations_base_bone[0]) * 2) for i in range(0,len(self.deg)): angles.append(self.deg[i]) angles.append(self.rotations[i]) s = ','.join([str(n) for n in angles]) print(s)
def Franka_robot_definition(default_target_position, default_target_orientation): # This is an example of using this code for solving inverse kinematic of FRANKA robot # Step 1: Define the specification of Base-bone: In this case it only twist and rotate around itself. # Bone number 1 (Base bone) base_bone_start_location = [0, 0, 0] base_bone_direction = [0, 0, 1] base_bone_length = 0.333 joint_type_1 = "twist_only" base_bone_rotation_axis = [0, 0, 1] cw_base_bone_constraint_rads = 2.8973 cw_base_bone_constraint_degs = cw_base_bone_constraint_rads * 180 / math.pi acw_base_bone_constraint_rads = 2.8973 acw_base_bone_constraint_degs = acw_base_bone_constraint_rads * 180 / math.pi base_bone_orientation = [ 1, 0, 0, 0 ] # this means no orientational frame rotation happened from global coordinate. # Define the specification of consecutive bones in this case they are two group the one # rotate around themselves(bone 3, 5 7) # and the one working as a hinge (bone 2,4,6) # Shoulder: # Bone number 2 bone_direction_2 = [0, 0, 1] bone_length_2 = 0.316 joint_type_2 = "LOCAL_HINGE" hinge_rotation_axis_2 = [0, 1, 0] hinge_constraint_reference_axis_2 = Util.Utils( ).gen_perpendicular_vector_quick(hinge_rotation_axis_2) cw_rad_2 = 1.7628 cw_deg_2 = cw_rad_2 * 180 / math.pi acw_rad_2 = 1.7628 acw_deg_2 = acw_rad_2 * 180 / math.pi bone_2_orientation = [1, 0, 0, 0] is_bone_2_fixed = 0 # Bone number 3 bone_direction_3 = [1, 0, 0] bone_length_3 = 0.088 is_bone_3_fixed = 0 joint_type_3 = "twist_only" hinge_rotation_axis_3 = [0, 0, 1] hinge_constraint_reference_axis_3 = [0, 0, 1] cw_rad_3 = 2.8972 cw_deg_3 = cw_rad_3 * 180 / math.pi acw_rad_3 = 2.8973 acw_deg_3 = acw_rad_3 * 180 / math.pi bone_3_orientation = [1, 0, 0, 0] # elbow # Bone number 4 bone_direction_4 = [0, 0, 1] bone_length_4 = 0.088 joint_type_4 = "LOCAL_HINGE" hinge_rotation_axis_4 = [0, 1, 0] hinge_constraint_reference_axis_4 = [0, 0, 1] cw_rad_4 = 3.0718 cw_deg_4 = cw_rad_4 * 180 / math.pi acw_rad_4 = 0.0698 acw_deg_4 = acw_rad_4 * 180 / math.pi bone_4_orientation = [1, 0, 0, 0] is_bone_4_fixed = 0 # Bone number 5 bone_direction_5 = [0, 0, 1] bone_length_5 = 0.384 is_bone_5_fixed = 0 joint_type_5 = "twist_only" hinge_rotation_axis_5 = [0, 0, 1] hinge_constraint_reference_axis_5 = [0, 0, 1] cw_rad_5 = 2.8973 cw_deg_5 = cw_rad_5 * 180 / math.pi acw_rad_5 = 2.8973 acw_deg_5 = acw_rad_5 * 180 / math.pi bone_5_orientation = [1, 0, 0, 0] # wrist # Bone number 6 bone_direction_6 = [1, 0, 0] bone_length_6 = 0.088 joint_type_6 = "LOCAL_HINGE" hinge_rotation_axis_6 = [0, 1, 0] hinge_constraint_reference_axis_6 = [0, 0, 1] cw_rad_6 = 0.0175 # cw_deg_6 = cw_rad_6 * 180 / math.pi acw_rad_6 = 3.7525 # acw_deg_6 = acw_rad_6 * 180 / math.pi bone_6_orientation = [0.707, 0, -0.707, 0] is_bone_6_fixed = 0 # Bone number 7 bone_direction_7 = [0, 0, -1] bone_length_7 = 0.107 is_bone_7_fixed = 0 joint_type_7 = "twist_only" hinge_rotation_axis_7 = [0, 0, 1] hinge_constraint_reference_axis_7 = [0, 0, 1] cw_rad_7 = 2.8973 cw_deg_7 = cw_rad_7 * 180 / math.pi acw_rad_7 = 2.8973 acw_deg_7 = acw_rad_7 * 180 / math.pi bone_7_orientation = [0.707, 0, -0.707, 0] ###### Make the Robot Chain # The FRANKA consist of four main part that in each part there is a joint responsible for hinge duties and # a consecutive bone responsible for twisting In below these four part being made # by the above information about joints and bones # the First part:review create a chain by defining one bone that is fixed in its place and only able to twist(base bone) is_base_bone_fixed = 0 m_chain = Chain.Chain3d(is_base_bone_fixed, base_address="./output") # scale_direction_base = [i * (base_bone_length) for i in base_bone_direction] # base_bone_end_location = [x + y for x, y in zip(base_bone_start_location, scale_direction_base)] # m_bone = Bone.Bone3D(base_bone_start_location,base_bone_end_location,base_bone_direction, # base_bone_length,is_base_bone_fixed,base_bone_orientation) # # m_chain.add_bone(m_bone) # Defining second part that consist of bone 2(able to work as a local hinge) and bone 3 that only # rotate around itself and responsible for rotations. scale_direction = [i * base_bone_length for i in base_bone_direction] bone_2_start_location = [ x + y for x, y in zip(base_bone_start_location, scale_direction) ] scale_direction = [ i * (bone_length_2 + bone_length_3) for i in bone_direction_2 ] bone_2_end_location = [ x + y for x, y in zip(bone_2_start_location, scale_direction) ] m_bone = Bone.Bone3D(bone_2_start_location, bone_2_end_location, bone_direction_2, bone_length_2 + bone_length_3, is_bone_2_fixed, bone_2_orientation) m_chain.add_bone(m_bone) m_chain.set_rotor_base_bone_constraint("BALL", base_bone_rotation_axis, cw_deg_2) # Third part belongs to bone 4(able to work as a local hinge) and bone 5 that only # rotate around itself and responsible for twists. m_chain.add_consecutive_hinged_bone(bone_direction_4, bone_length_4 + bone_length_5, joint_type_4, hinge_rotation_axis_4, cw_deg_4, acw_deg_4, hinge_constraint_reference_axis_4, is_bone_4_fixed, bone_4_orientation) # Fourth part belongs to bone 6(able to work as a local hinge) and bone 7 that only # rotate around itself and responsible for twists. m_chain.add_consecutive_hinged_bone(bone_direction_6, bone_length_6 + bone_length_7, joint_type_6, hinge_rotation_axis_6, cw_deg_6, acw_deg_6, hinge_constraint_reference_axis_6, is_bone_6_fixed, bone_6_orientation) # In this part the target is set for the chain and whole chain is going to be solved m_chain.set_target(default_target_position, default_target_orientation) return m_chain
def get_direction_uv(self): start_point = [i * -1 for i in self.start_point] return Util.Utils().normalization( [x + y for x, y in zip(self.end_point, start_point)])
print("Target is so far! can't be reached") return if __name__ == "__main__": # x = float(sys.argv[1]) # y = float(sys.argv[2]) # z = float(sys.argv[3]) # default_target_position = [x, y, z] # default_target_position = [0.3,0.2,0.5] default_target_position = [0.28, -0.199904, 0.6] # default_target_position = [0.41, 0.09, 0.82] # default_target_orientation = [0, 1, 0, 0, 0] # rotation_matrix = [[1,-0.000308427, 0.000562747], # [0.000308962,1,-0.000950381], # [-0.000562454, 0.000950554,0.999999]] rotation_matrix = [[0.998752, 0.0479043, -0.0141034], [-0.0126917, -0.029647, -0.99948], [-0.0482975, 0.998412, -0.029002]] default_target_orientation = Util.Utils().quaternion_from_rotation_matrix( rotation_matrix) chain = Franka_robot_definition(default_target_position, default_target_orientation) base_bone = FRANKA_base_bone() solve_fabrik_for_Robot(base_bone, chain, default_target_position, default_target_orientation)
def set_freely_rotating_local_hinged_base_bone(self, hinge_rotation_axis): self.set_hinge_base_bone_constraint( "LOCAL_HINGE", hinge_rotation_axis, 180, 180, Util.Utils().gen_perpendicular_vector_quick(hinge_rotation_axis))
def validate_axis(self, axis): if Util.Utils().length_calc(axis) <= 0: raise Exception( "Provided axis is illegal - it has a magnitude of zero.")
def forward(self,chain): for loop in range(self.chain_length - 1, -1, -1): # Get the length of the bone we're working on this_bone = chain.get_bone(loop) this_bone_length = this_bone.get_length() this_bone_joint = this_bone.get_joint() this_bone_joint_type = this_bone_joint.get_joint_type() # If we are NOT working on the end effector bone if loop != (self.chain_length - 1): if this_bone.is_fix_bone() == 1: this_bone_outer_to_inner_uv = Util.Utils().negated(this_bone.get_fixed_bone_direction_uv()) else: # Get the outer-to-inner unit vector of the bone further out outer_bone_outer_to_inner_uv = Util.Utils().negated( chain.get_bone(loop+1).get_direction_uv()) # Get the outer-to-inner unit vector of this bone this_bone_outer_to_inner_uv = Util.Utils().negated( chain.get_bone(loop).get_direction_uv()) next_bone_orientation = chain.get_bone(loop+1).get_bone_orientation() this_bone_orientation = chain.get_bone(loop+1).get_bone_orientation() this_bone.set_bone_orientation( self.solve_for_rotations(next_bone_orientation, this_bone_orientation, loop)) # Get the joint type for this bone and handle constraints on thisBoneOuterToInnerUV if this_bone_joint_type == "BALL": # Constrain to relative angle between this bone and the outer bone if required angle_between_degs = Util.Utils().get_angle_between_degs(outer_bone_outer_to_inner_uv, this_bone_outer_to_inner_uv) constrain_angle_degs = this_bone_joint.get_ball_joint_constraint_degs() if angle_between_degs > constrain_angle_degs: this_bone_outer_to_inner_uv = Util.Utils().get_angle_limited_uv(this_bone_outer_to_inner_uv, outer_bone_outer_to_inner_uv, constrain_angle_degs) elif this_bone_joint_type == "GLOBAL_HINGE": # Project this bone outer-to-inner direction onto the hinge rotation axis this_bone_outer_to_inner_uv = Util.Utils().project_on_to_plane(this_bone_outer_to_inner_uv, this_bone_joint.get_hinge_rotation_axis()) elif this_bone_joint_type == "LOCAL_HINGE": # Not a base bone? Then construct a rotation matrix based on the previous bones # inner-to-outer direction... if loop > 0: m = Util.Utils().create_rotation_matrix(chain.get_bone(loop-1).get_direction_uv()) relative_hinge_rotation_axis = Util.Utils().normalization( Util.Utils().times(m, this_bone_joint.get_hinge_rotation_axis())) # transform the hinge rotation axis into the previous bones frame of reference. # Project this bone's outer-to-inner direction onto the plane described by the relative hinge rotation axis this_bone_outer_to_inner_uv = Util.Utils().project_on_to_plane(this_bone_outer_to_inner_uv, relative_hinge_rotation_axis) else: raise Exception("The base bone joint can't be LOCAL HINGE") scale = [i * this_bone_length for i in this_bone_outer_to_inner_uv] end_location = this_bone.get_end_point_position() new_start_location = [x + y for x, y in zip(end_location, scale)] this_bone.set_start_point_position(new_start_location) # If we are not working on the basebone, then we also set the end joint location of # the previous bone in the chain if loop > 0: chain.get_bone(loop-1).set_end_point_position(new_start_location) # If we ARE working on the end effector bone.. else: # put end effector end location to the target this_bone.set_end_point_position(self.target_position) this_bone.set_bone_orientation( self.solve_for_rotations(self.target_orientation, this_bone.get_bone_orientation(), loop)) if this_bone.is_fix_bone() == 1: this_bone_outer_to_inner_uv = Util.Utils().negated(this_bone.get_fixed_bone_direction_uv()) else: this_bone_outer_to_inner_uv = Util.Utils().negated(this_bone.get_direction_uv()) if this_bone_joint_type == "BALL": i = 0 elif this_bone_joint_type == "GLOBAL_HINGE": this_bone_outer_to_inner_uv = Util.Utils().project_on_to_plane(this_bone_outer_to_inner_uv, this_bone_joint.get_hinge_rotation_axis()) elif this_bone_joint_type == "LOCAL_HINGE": m = Util.Utils().create_rotation_matrix(chain.get_bone(loop-1).get_direction_uv()) relative_hinge_rotation_axis = Util.Utils().normalization( Util.Utils().times(m, this_bone_joint.get_hinge_rotation_axis())) # Project this bone's outer-to-inner direction onto the plane described by the relative hinge # rotation axis this_bone_outer_to_inner_uv = Util.Utils().project_on_to_plane(this_bone_outer_to_inner_uv, relative_hinge_rotation_axis) scale = [i * this_bone_length for i in this_bone_outer_to_inner_uv] end_location = this_bone.get_end_point_position() new_start_location = [x + y for x, y in zip(end_location, scale)] this_bone.set_start_point_position(new_start_location) # If we are not working on the base bone, then we also set the end joint location of # the previous bone in the chain if loop > 0: chain.get_bone(loop-1).set_end_point_position(new_start_location) return chain
def set_base_bone_constraint_uv(self, constraint_uv): if len(constraint_uv) == 0: raise Exception("direction unit vector cannot be zero") self.base_bone_constraint_uv = Util.Utils().normalization( constraint_uv)
def backward(self,chain): for loop in range(self.chain_length): this_bone = chain.get_bone(loop) this_bone_length = chain.get_bone(loop).get_length() # If we are not working on the base bone if loop != 0: if this_bone.is_fix_bone() == 1: this_bone_inner_to_outer_uv = this_bone.get_fixed_bone_direction_uv() else: this_bone_inner_to_outer_uv = this_bone.get_direction_uv() prev_bone_inner_to_outer_uv = chain.get_bone(loop-1).get_direction_uv() this_bone_joint = this_bone.get_joint() this_bone_joint_type = this_bone_joint.get_joint_type() if this_bone_joint_type == "BALL": angle_between_degs = Util.Utils().get_angle_between_degs(prev_bone_inner_to_outer_uv, this_bone_inner_to_outer_uv) constraint_angle_degs = this_bone_joint.get_ball_joint_constraint_degs() self.deg[loop] = angle_between_degs if angle_between_degs > constraint_angle_degs: this_bone_inner_to_outer_uv = Util.Utils().get_angle_limited_uv(this_bone_inner_to_outer_uv, prev_bone_inner_to_outer_uv, constraint_angle_degs) self.deg[loop] = constraint_angle_degs elif this_bone_joint_type == "GLOBAL_HINGE": # Get the hinge rotation axis and project our inner-to-outer UV onto it this_bone_inner_to_outer_uv = Util.Utils().project_on_to_plane(this_bone_inner_to_outer_uv, this_bone_joint.get_hinge_rotation_axis()) # If there are joint constraints, then we must honour them... cw_constraint_degs = -this_bone_joint.get_hinge_clockwise_constraint_degs() acw_constraint_degs = this_bone_joint.get_hinge_anticlockwise_constraint_degs() if not Util.Utils().approximately_equal(cw_constraint_degs, -this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(), 0.001) and not Util.Utils().approximately_equal( acw_constraint_degs, this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(), 0.001): hinge_reference_axis = this_bone_joint.get_reference_axis() hinge_rotation_axis = this_bone_joint.get_hinge_rotation_axis() # Get the signed angle (about the hinge rotation axis) between the hinge reference axis and the hinge-rotation aligned bone UV signed_angle_degs = Util.Utils().get_signed_angle_between_degs(hinge_reference_axis, this_bone_inner_to_outer_uv, hinge_rotation_axis) self.deg[loop] = signed_angle_degs * math.pi / 180 # Make our bone inner-to-outer UV the hinge reference axis rotated by its maximum clockwise or anticlockwise rotation as required if signed_angle_degs > acw_constraint_degs: this_bone_inner_to_outer_uv = Util.Utils().normalization( Mat.Mat().rotate_about_axis(hinge_reference_axis, acw_constraint_degs, hinge_rotation_axis)) self.deg[loop] = acw_constraint_degs * math.pi / 180 elif signed_angle_degs < cw_constraint_degs: this_bone_inner_to_outer_uv = Util.Utils().normalization( Mat.Mat().rotate_about_axis(hinge_reference_axis, cw_constraint_degs, hinge_rotation_axis)) self.deg[loop] = cw_constraint_degs * math.pi / 180 elif this_bone_joint_type == "LOCAL_HINGE": # Transform the hinge rotation axis to be relative to the previous bone in the chain hinge_rotation_axis = this_bone_joint.get_hinge_rotation_axis() m = Util.Utils().create_rotation_matrix(prev_bone_inner_to_outer_uv) relative_hinge_rotation_axis = Util.Utils().normalization( Util.Utils().times(m, hinge_rotation_axis)) this_bone_inner_to_outer_uv = Util.Utils().project_on_to_plane(this_bone_inner_to_outer_uv, relative_hinge_rotation_axis) # Constrain rotation about reference axis if required cw_constraint_degs = -this_bone_joint.get_hinge_clockwise_constraint_degs() acw_constraint_degs = this_bone_joint.get_hinge_anticlockwise_constraint_degs() if not Util.Utils().approximately_equal(cw_constraint_degs, -this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(), 0.001) and not Util.Utils().approximately_equal( acw_constraint_degs, this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(), 0.001): relative_hinge_reference_axis = Util.Utils().normalization( Util.Utils().times(m, this_bone_joint.get_reference_axis())) signed_angle_degs = Util.Utils().get_signed_angle_between_degs( relative_hinge_reference_axis, this_bone_inner_to_outer_uv, relative_hinge_rotation_axis) self.deg[loop] = signed_angle_degs * math.pi / 180 if signed_angle_degs > acw_constraint_degs: this_bone_inner_to_outer_uv = Util.Utils().normalization( Mat.Mat().rotate_about_axis(relative_hinge_reference_axis, acw_constraint_degs, relative_hinge_rotation_axis)) self.deg[loop] = acw_constraint_degs * math.pi / 180 elif signed_angle_degs < cw_constraint_degs: this_bone_inner_to_outer_uv = Util.Utils().normalization( Mat.Mat().rotate_about_axis(relative_hinge_reference_axis, cw_constraint_degs, relative_hinge_rotation_axis)) self.deg[loop] = cw_constraint_degs * math.pi / 180 # twisted = np.cross(this_bone_inner_to_outer_uv,relative_hinge_rotation_axis) # print("bone"+str(loop)) # print(twisted) scale = [i * this_bone_length for i in this_bone_inner_to_outer_uv] start_location = this_bone.get_start_point_position() new_end_location = [x + y for x, y in zip(start_location, scale)] this_bone.set_end_point_position(new_end_location) if loop < self.chain_length - 1: chain.get_bone(loop+1).set_start_point_position(new_end_location) # If we ARE working on the basebone... else: chain.get_bone(0).set_start_point_position(self.fixed_base_location) if self.is_base_bone_fixed == 1: chain.get_bone(0).set_end_point_position(self.fixed_base_location_2) if self.chain_length > 1: chain.get_bone(1).set_start_point_position(self.fixed_base_location_2) else: this_bone_joint = this_bone.get_joint() this_bone_joint_type = this_bone_joint.get_joint_type() if this_bone_joint_type == "GLOBAL_HINGE": hinge_rotation_axis = this_bone_joint.get_hinge_rotation_axis() cw_constraint_degs = -this_bone_joint.get_hinge_clockwise_constraint_degs() acw_constraint_degs = this_bone_joint.get_hinge_anticlockwise_constraint_degs() this_bone_inner_to_outer_uv = Util.Utils().project_on_to_plane(this_bone.get_direction_uv(), hinge_rotation_axis) # If we have a global hinge which is not freely rotating then we must constrain about the reference axis if not Util.Utils().approximately_equal(cw_constraint_degs, -this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(), 0.001) and not Util.Utils().approximately_equal( acw_constraint_degs, this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(), 0.001): hinge_reference_axis = this_bone_joint.get_reference_axis() signed_angle_degs = Util.Utils().get_signed_angle_between_degs(hinge_reference_axis, this_bone_inner_to_outer_uv, hinge_rotation_axis) self.deg[loop] = signed_angle_degs * math.pi / 180 if signed_angle_degs > acw_constraint_degs: this_bone_inner_to_outer_uv = Util.Utils().normalization( Mat.Mat().rotate_about_axis(hinge_reference_axis, acw_constraint_degs, hinge_rotation_axis)) self.deg[loop] = acw_constraint_degs * math.pi / 180 elif signed_angle_degs < cw_constraint_degs: this_bone_inner_to_outer_uv = Util.Utils().normalization( Mat.Mat().rotate_about_axis(hinge_reference_axis, cw_constraint_degs, hinge_rotation_axis)) self.deg[loop] = cw_constraint_degs * math.pi / 180 # twisted = np.cross(this_bone_inner_to_outer_uv, hinge_rotation_axis) # print("bone" + str(loop)) # print(twisted) scale = [i * this_bone_length for i in this_bone_inner_to_outer_uv] start_location = this_bone.get_start_point_position() new_end_location = [x + y for x, y in zip(start_location, scale)] this_bone.set_end_point_position(new_end_location) if self.chain_length > 1: chain.get_bone(1).set_start_point_position(new_end_location) if this_bone_joint_type == "BALL": this_bone_inner_to_outer_uv = this_bone.get_direction_uv() angle_between_degs = Util.Utils().get_angle_between_degs(self.base_bone_constraint_uv, this_bone_inner_to_outer_uv) constraint_angle_degs = this_bone.get_ball_joint_constraint_degs() self.deg[loop] = angle_between_degs * math.pi / 180 if angle_between_degs > constraint_angle_degs: this_bone_inner_to_outer_uv = Util.Utils().get_angle_limited_uv(this_bone_inner_to_outer_uv, self.base_bone_constraint_uv, constraint_angle_degs) self.deg[loop] = constraint_angle_degs * math.pi / 180 scale = [i * this_bone_length for i in this_bone_inner_to_outer_uv] start_location = this_bone.get_start_point_position() new_end_location = [x + y for x, y in zip(start_location, scale)] this_bone.set_end_point_position(new_end_location) if self.chain_length > 1: chain.get_bone(1).set_start_point_position(new_end_location) else: this_bone_inner_to_outer_uv = this_bone.get_direction_uv() scale = [i * this_bone_length for i in this_bone_inner_to_outer_uv] start_location = this_bone.get_start_point_position() new_end_location = [x + y for x, y in zip(start_location, scale)] this_bone.set_end_point_position(new_end_location) if self.chain_length > 1: chain.get_bone(1).set_start_point_position(new_end_location) return chain
def get_live_length(self): return Util.get_distance_between(self.start_point, self.end_point)