def r_contract(self):
lower_limit = third_side(self.segments["femur"].length, self.segments["tibia"].length, 180 + self.segments["tibia"].min_angle)
upper_limit = 100000000
transition_zone = 20
try:
tarsus = self.stored_node_positions["tarsus"]
except:
tarsus = self.store_node("tarsus")
try:
femur = self.stored_node_positions["femur"]
except:
femur = self.store_node("femur")
parameter = femur.distance_to(tarsus)
return exp_r(parameter, lower_limit, upper_limit, transition_zone)
def calc_joint_angles(self, target_position, best_effort):
general_calculation = True
position_leg_cs = self.cs.to_this(target_position)
angle_coxa = -1 * d(atan2(position_leg_cs.x, position_leg_cs.y))
femur_to_position_projection = 0
femur_segment = self.segments["femur"]
femur_length = femur_segment.length
tibia_segment = self.segments["tibia"]
tibia_length = tibia_segment.length
tarsus_segment = self.segments["tarsus"]
tarsus_length = tarsus_segment.length
coxa_segment = self.segments["coxa"]
if best_effort:
tgt_tarsus = position_leg_cs.clone
tgt_tarsus.z += tarsus_length
femur = self.get_node("femur")
# check if we can actually reach target_position
distance = femur.distance_to(tgt_tarsus)
max_dist = femur_length + tibia_length
min_dist = third_side(femur_length, tibia_length, 180 + tibia_segment.min_angle)
dz = femur.z - tgt_tarsus.z
# are we too far?
if distance > max_dist:
# yep, too far
# calculate femur angle to target
femur_to_position_projection = sqrt(max_dist ** 2 - dz ** 2)
general_calculation = False
elif distance < min_dist:
# we are too close
femur_to_position_projection = sqrt(min_dist ** 2 - dz ** 2)
general_calculation = False
else:
# we are not too close and not too far
general_calculation = True
if general_calculation:
# works either when not best_effort or when best_effort and distance is ok
coxa_to_position_projection = sqrt(position_leg_cs.x ** 2 + position_leg_cs.y ** 2)
femur_to_position_projection = coxa_to_position_projection - coxa_segment.length
distance_to_tarsus_top = sqrt(femur_to_position_projection ** 2 + (position_leg_cs.z + tarsus_length) ** 2)
if position_leg_cs.z + tarsus_length == 0:
angle_down_to_tarsus_top = 0
else:
angle_down_to_tarsus_top = 90 - angle_ab(-1 * (position_leg_cs.z + tarsus_length), distance_to_tarsus_top, femur_to_position_projection)
angle_femur = angle_ab(femur_length, distance_to_tarsus_top, tibia_length) - angle_down_to_tarsus_top
angle_tibia = -180 + angle_ac(femur_length, distance_to_tarsus_top, tibia_length)
angle_tarsus = -180 + 90 - angle_femur - angle_tibia
if coxa_segment.min_angle > angle_coxa:
if best_effort:
angle_coxa = coxa_segment.min_angle
else:
raise Exception(self.name + " coxa angle too low")
elif coxa_segment.max_angle < angle_coxa:
if best_effort:
angle_coxa = coxa_segment.max_angle
else:
raise Exception(self.name + " coxa angle too high")
if femur_segment.min_angle > angle_femur:
if best_effort:
angle_femur = femur_segment.min_angle
else:
raise Exception(self.name + " femur angle too low")
elif femur_segment.max_angle < angle_femur:
if best_effort:
angle_femur = femur_segment.max_angle
else:
raise Exception(self.name + " femur angle too high")
if tibia_segment.min_angle > angle_tibia:
if best_effort:
angle_tibia = tibia_segment.min_angle
else:
raise Exception(self.name + " tibia angle too low")
elif tibia_segment.max_angle < angle_tibia:
if best_effort:
angle_tibia = tibia_segment.max_angle
else:
raise Exception(self.name + " tibia angle too high")
if tarsus_segment.min_angle > angle_tarsus:
if best_effort:
angle_tarsus = tarsus_segment.min_angle
else:
raise Exception(self.name + " tarsus angle too low")
elif tarsus_segment.max_angle < angle_tarsus:
if best_effort:
angle_tarsus = tarsus_segment.max_angle
else:
raise Exception(self.name + " tarsus angle too high")
return angle_coxa, angle_femur, angle_tibia, angle_tarsus
