Exemplo n.º 1
0
    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)
Exemplo n.º 2
0
    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