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)
