Ejemplo n.º 1
0
    def update_home_position(self):
        h_dist_home = (Leg.OUTER_WS_LIMIT + Leg.INNER_WS_LIMIT) / 2
        roll_dist_home = h_dist_home * cosd(self.cs.roty)

        self.home_position_leg = Position(self.cs, roll_dist_home * sind(-self.home_angle), roll_dist_home * cosd(-self.home_angle), 0)
        self.home_position_leg.z = -38

        self.home_position_thorax = self.cs.parent.to_this(self.home_position_leg)

        return self.home_position_leg
Ejemplo n.º 2
0
    def update_swings_inwards(self):
        # Indicates whether the swing movement is towards inner limit of workspace

        coxa = self.get_node("coxa")
        current_foot = self.get_node("foot")
        moved_foot = current_foot.clone
        moved_foot.x += .0001 * sind(-1 * self.task_cs.rotz)
        moved_foot.y += .0001 * cosd(-1 * self.task_cs.rotz)

        self.swings_inwards = coxa.distance_to(current_foot) > coxa.distance_to(moved_foot)
Ejemplo n.º 3
0
 def x_y(l, coxa_angle):
     x = l * sind(-coxa_angle)
     y = l * cosd(-coxa_angle)
     return x, y
Ejemplo n.º 4
0
    def store_node(self, name):

        # noinspection PyShadowingNames
        def x_y(l, coxa_angle):
            x = l * sind(-coxa_angle)
            y = l * cosd(-coxa_angle)
            return x, y

        if name == "foot":
            segment_coxa = self.segments["coxa"]
            coxa_len = segment_coxa.length
            coxa_angle = segment_coxa.current_angle

            segment_femur = self.segments["femur"]
            femur_angle = segment_femur.current_angle
            femur_projection_h = segment_femur.length * cosd(femur_angle)
            femur_projection_v = segment_femur.length * sind(femur_angle)

            segment_tibia = self.segments["tibia"]
            tibia_angle = segment_tibia.current_angle
            tibia_projection_h = segment_tibia.length * cosd(femur_angle + tibia_angle)
            tibia_projection_v = segment_tibia.length * sind(femur_angle + tibia_angle)

            segment_tarsus = self.segments["tarsus"]
            tarsus_angle = segment_tarsus.current_angle
            tarsus_projection_h = segment_tarsus.length * sind(90 + (tibia_angle + femur_angle + tarsus_angle) - 180)
            tarsus_projection_v = segment_tarsus.length * cosd(90 + (tibia_angle + femur_angle + tarsus_angle) - 180)

            l = coxa_len + femur_projection_h + tibia_projection_h + tarsus_projection_h
            z = femur_projection_v + tibia_projection_v + tarsus_projection_v
            x, y = x_y(l, coxa_angle)
            node_pos_converted = Position(self.cs)
            node_pos_converted.x, node_pos_converted.y, node_pos_converted.z = x, y, z

        elif name == "foot-odom":
            foot_thorax = self.get_node("foot-thorax")
            node_pos_converted = self.cs.parent.parent.to_this(foot_thorax)

        elif name == "coxa-thorax":
            segment_coxa = self.segments["coxa"]
            node_pos = Position(segment_coxa.cs)
            node_pos_converted = self.cs.parent.to_this(node_pos)

        elif name == "foot-thorax":
            foot = self.get_node("foot")
            node_pos_converted = self.cs.parent.to_this(foot)

        elif name == "tibia-thorax":
            segment_coxa = self.segments["coxa"]
            coxa_len = segment_coxa.length
            coxa_angle = segment_coxa.current_angle

            segment_femur = self.segments["femur"]
            femur_angle = segment_femur.current_angle
            femur_projection_h = segment_femur.length * cosd(femur_angle)
            femur_projection_v = segment_femur.length * sind(femur_angle)

            l = coxa_len + femur_projection_h
            z = femur_projection_v
            x, y = x_y(l, coxa_angle)
            node_pos_converted = Position(self.cs)
            node_pos_converted.x, node_pos_converted.y, node_pos_converted.z = x, y, z

        else:
            node_pos = Position(self.segments[name].cs)
            node_pos_converted = self.cs.to_this(node_pos)

        self.stored_node_positions[name] = node_pos_converted

        return node_pos_converted