예제 #1
0
class Leg:
    def __init__(self, b_1_settings=None, cmd_queue=None, group_index=1):
        self.angle_0 = 0.0
        self.angle_1 = 0.0
        self.angle_2 = 0.0
        self.angle_3 = 0.0

        self.cmd_queue = cmd_queue                            
        self.group_index = group_index

        #1
        if b_1_settings is not None:
            self.b_1 = MyFrame(**b_1_settings)
        else:
            self.b_1 = MyFrame()

        self.b_1_box_1 = box(frame=self.b_1,
                            size=(63, 2, 40),
                            pos=(-31.5, 12.69, 0),
                            axis=(1, 0, 0))

        self.b_1_box_2 = box(frame=self.b_1,
                            size=(63, 2, 40),
                            pos=(-31.5, -39.02, 0),
                            axis=(1, 0, 0))


        self.b_1_box_3 = box(frame=self.b_1,
                            size=(2, 53.71, 40),
                            pos=(-1, -13.17, 0),
                            axis=(1, 0, 0))

        #2 
        self.b_2 = MyFrame(frame=self.b_1,
                                pos=(-53, 0, 0))

        self.b_2_celinder = cylinder(frame=self.b_2,
                                        pos=(0, 16.1),
                                        axis=(0,-1),
                                        radius=8,
                                        length=63)
        
        self.b_2_box_1 = box(frame=self.b_2,
                            size=(61.5, 43.61, 26.5),
                            pos=(9.25, -13.21, -1.75),
                            axis=(1, 0, 0))

        self.b_2_box_2 = box(frame=self.b_2,
                            size=(28.81, 60, 43.6),
                            pos=(-35.9, -4.99, -0.3),
                            axis=(1, 0, 0))
        
        #3
        self.b_3 = MyFrame(frame=self.b_2,
                         pos=(-35, -14.99, 0))

        self.b_3_celinder = cylinder(frame=self.b_3,
                                        pos=(0, 0, -29.6),
                                        axis=(0, 0, 1),
                                        radius=8,
                                        length=59.1,
                                        color=(1, 0, 0))

        self.b_3_box_1 = box(frame=self.b_3,
                          size=(42, 20, 2),
                          pos=vector(-11, 0, 25.85),
                          color=(1, 0, 0))

        self.b_3_box_2 = box(frame=self.b_3,
                          size=(42, 20, 2),
                          pos=vector(-11, 0, -25.85),
                          color=(1, 0, 0))        

        self.b_3_box_3 = box(frame=self.b_3,
                          size=(2, 20, 53.7),
                          pos=vector(-31, 0, 0),
                          color=(1, 0, 0))

        self.b_3_box_4 = box(frame=self.b_3,
                          size=(68, 25, 25),
                          pos=vector(-(32 + 34), 0, 0),
                          color=(1, 0, 0))                          

        #4.
        self.b_4 = MyFrame(frame=self.b_3,
                            pos=(-(32 + 34 + 14), 0, 0))

        self.b_4_celinder = cylinder(frame=self.b_4,
                                        pos=(0, 0, -29.6),
                                        axis=(0, 0, 1),
                                        radius=8,
                                        length=59.1,
                                        color=(0, 0, 1))
                                        
        self.b_4_box_1 = box(frame=self.b_4,
                            size=(39.5, 20, 2),
                            pos=(-9.75, 0, 25.5),
                            color=(0, 0, 1))

        self.b_4_box_2 = box(frame=self.b_4,
                            size=(39.5, 20, 2),
                            pos=(-9.75, 0, -25.5),
                            color=(0, 0, 1))

        self.b_4_box_3 = box(frame=self.b_4,
                            size=(2, 20, 53.7),
                            pos=(-28.5, 0, 0),
                            color=(0, 0, 1))

        self.b_4_box_4 = box(frame=self.b_4,
                            size=(55.5, 10, 10),
                            pos=(-57.25, 0, 0),
                            color=(0, 0, 1))
    
    def has_obj(self, obj):
        return obj is self.b_4_box_4

        if obj in self.b_1.objects:
            return True
        if obj in self.b_2.objects:
            return True
        if obj in self.b_3.objects:
            return True
        if obj in self.b_4.objects:
            return True
        return False
                          
    def set_angle_0(self, in_angle):
        border = pi / 2.0
        if in_angle >= border:
            in_angle = border
        elif in_angle < -border:
            in_angle = border
        
        sub_angle = in_angle - self.angle_0

        if sub_angle != 0.0:
            self.angle_0 = in_angle
            self.b_2.rotate(angle=sub_angle, axis=(0,1,0))

            if self.cmd_queue is not None:
                self.cmd_queue.put((self.group_index, 2, (pi - (in_angle + border)) / pi))


    def set_angle_1(self, in_angle):
        sub_angle = in_angle - self.angle_1

        if sub_angle != 0.0:
            self.angle_1 = in_angle
            self.b_3.rotate(angle=sub_angle, axis=(0,0,1))
            
            if self.cmd_queue is not None:
                offset = 140 / 180 * pi 
                self.cmd_queue.put((self.group_index, 1, (offset - in_angle) / pi))
        
       

    def set_angle_2(self, in_angle):
        sub_angle = in_angle - self.angle_2

        if sub_angle != 0.0:
            self.angle_2 = in_angle
            self.b_4.rotate(angle=sub_angle, axis=(0,0,1))

            if self.cmd_queue is not None:
                offset = 122 / 180 * pi            
                self.cmd_queue.put((self.group_index, 0, (offset - in_angle) / pi))
    
    def calc_angles(self, end_pos):
        a = mag(self.b_4.pos)
        b = 85.0
        c = mag(end_pos)
        c = c if c <= a + b else a + b
        
        alfa_1 = math.acos((a * a + c * c - b * b) / (2 * a * c))
        alfa_2 = pi - (math.acos((b * b + a * a - c * c) / (2 * b * a)))
        
        betta = diff_angle((end_pos[0], end_pos[1]), (-1,0))

        if dot((end_pos[0], end_pos[1]), (0, -1)) >= 0:
            betta = betta - alfa_1
        else:
            betta = -betta - alfa_1

        return (betta, alfa_2)

    def set_pos(self, end_pos):
        #1. найдём угол поворота плоскости b_2 относиельно b_1 и знак угла
        v1 = self.b_1.world_to_frame(end_pos) - self.b_2.pos
        v1.y = 0.0

        b_2_angle = diff_angle(v1, (-1, 0, 0))
        b_2_angle = b_2_angle if dot(v1, (0, 0, 1)) >= 0. else -b_2_angle

        #2. перевод end_pos в координаты b_3
        b_3_g_pos = self.b_2.world_to_frame(end_pos) - self.b_3.pos
        angles = self.calc_angles(b_3_g_pos)

        self.set_angle_0(b_2_angle)
        self.set_angle_1(angles[0])
        self.set_angle_2(angles[1])

    def get_pos(self):
        return self.b_4.frame_to_world((-85.0, 0, 0))
예제 #2
0
class Pauk:
    def __init__(self, cmd_queue=None):
        self.f_1 = MyFrame()
        
        self.f_2 = MyFrame(frame=self.f_1, pos=(0, -20, 0))
        self.f_2.rotate(axis=(0, 1, 0), angle=-pi / 4.0)
        
        self.base_box = box(frame=self.f_2,
                            size=(50, 40, 50),
                            color=(1, 0, 0))
        
        self.legs = []
        self.legs.append(Leg({"pos": (25, 20 - 13.7, 0), "axis": (-1, 0, 0), "frame": self.f_2}, cmd_queue, 0))
        self.legs.append(Leg({"pos": (-25, 20 - 13.7, 0), "axis": (1, 0, 0), "frame": self.f_2}, cmd_queue, 1))
        self.legs.append(Leg({"pos": (0, 20 - 13.7, 25.), "axis": (0, 0, -1), "frame": self.f_2}, cmd_queue, 2))
        self.legs.append(Leg({"pos": (0, 20 - 13.7, -25.), "axis": (0, 0, 1), "frame": self.f_2}, cmd_queue, 3))
        self.time = 0.0
        
        self.base_height = 120
        self.set_pos((0, self.base_height, 0))
        self.leg_len = 160

        for id, point in enumerate(self.get_init_legs_points()):
            self.legs[id].set_pos(point)

        self.can_move = True
        self.first_step = True

        self.move_direction = vector(0, 0, 0)
        self.move_time = 0.0
        self.move_angle = 0.0
        self.angle = 0.0

    def set_angle(self, angle):
        da = angle - self.angle
        
        if da != 0.0:
            self.f_1.rotate(angle=da, axis=self.f_1.up)
            self.angle = angle
    
    def get_angle(self):
        return self.angle
            
    def get_init_legs_points(self):
        res = []

        for leg in self.legs:
            pos = self.f_1.world_to_frame(leg.b_2.frame_to_world((0, 0, 0)))
            pos.y = 0
            pos = norm(pos) * self.leg_len + vector(0, -1, 0) * self.base_height
            print pos
            res.append(self.f_1.frame_to_world(pos))
        return res

    def get_data_by_time(self, animation, time):
        if len(animation) == 0:
            return vector(0, 0, 0)
        elif len(animation) == 1:
            return vector(animation[0][1])
        else:
            if time < animation[0][0]:
                return animation[0][1]
            elif time >= animation[-1][0]:
                return animation[-1][1]

            for i in range(1, len(animation)):
                if animation[i][0] >= time:
                    return vector(animation[i-1][1]) + (vector(animation[i][1]) - vector(animation[i-1][1])) *\
                                 (time - animation[i-1][0]) / (animation[i][0] - animation[i - 1][0])
 
    def set_next_move(self, direction, angle, time):
        if not self.can_move:
            self.move_direction = self.f_1.frame_to_world(direction) - self.f_1.pos
            self.move_angle = angle
            self.move_time = time
            self.can_move = True
            
    def get_leg_by_obj(self, obj):
        for leg in self.legs:
            if leg.has_obj(obj):
                return leg
        return None
    
    def is_body(self, obj):
        return obj is self.base_box
        
    def get_pos(self):
        return self.f_1.pos
    
    def set_pos(self, pos):
        self.f_1.pos = pos

    #сделать шиг в заданном направление за заданное время.
    def update(self, rate=30.0):
        #разрешено движение.
        if self.can_move:
            #рассчёт анимации ног.
            if self.first_step:
                max_up = 20
                leg_move_time = self.move_time / 4.0
                dt = 0.0

                #инимация перемещения
                if self.move_angle == 0:
                    #определим последовательность ног
                    sequence = [0, 1, 2, 3]
                    dot_x = dot(self.f_1.axis, self.move_direction)
                    dot_z = dot(vector(-self.f_1.axis.z, self.f_1.axis.y, self.f_1.axis.x), self.move_direction)
                    
                    if dot_x >= 0.0 and dot_z >= 0.0:
                        sequence = [0, 1, 2, 3]
                    elif dot_x < 0.0 and dot_z >= 0.0:
                        sequence = [2, 3, 0, 1]
                    elif dot_x < 0.0 and dot_z < 0.0:
                        sequence = [1, 0, 3, 2]
                    else:
                        sequence = [3, 2, 1, 0]

                    #ходьба 1
                    if 1:
                        #создание набора анимации
                        for id in sequence:
                            leg = self.legs[id]
                            leg.anim = [(0.0 + dt, vector(leg.get_pos())),
                                        (leg_move_time / 2.0 + dt, (leg.get_pos() + self.move_direction / 2) + self.f_1.up * max_up),
                                        (leg_move_time + dt, (leg.get_pos() + self.move_direction))]
                            dt = dt + leg_move_time

                        self.body_pos_anim = [(0.0, vector(self.get_pos())),
                                              (self.move_time, self.get_pos() + self.move_direction)]

                        self.body_angle_anim = [(0.0, vector(self.angle, 0.0, 0.0))]
                        self.first_step = False
                        self.time = 0.0
                        self.max_time = self.move_time
                    #ходьба 2
                    else:
                        dt = self.move_time / 2
                        
                        leg = self.legs[sequence[0]]
                        leg.anim = [(0.0, vector(leg.get_pos())),
                                    (dt / 2, (leg.get_pos() + self.move_direction / 2 + self.f_1.up * max_up)),
                                    (dt, (leg.get_pos() + self.move_direction))]

                        leg = self.legs[sequence[1]]
                        leg.anim = [(0.0, vector(leg.get_pos())),
                                    (dt / 2, (leg.get_pos() + self.move_direction / 2 + self.f_1.up * max_up)),
                                    (dt, (leg.get_pos() + self.move_direction))]
                                    
                        leg = self.legs[sequence[2]]
                        leg.anim = [(dt, vector(leg.get_pos())),
                                    (dt + dt / 2.0, (leg.get_pos() + self.move_direction / 2 + self.f_1.up * max_up)),
                                    (dt + dt, (leg.get_pos() + self.move_direction))]

                        leg = self.legs[sequence[3]]
                        leg.anim = [(dt, vector(leg.get_pos())),
                                    (dt + dt / 2.0, (leg.get_pos() + self.move_direction / 2 + self.f_1.up * max_up)),
                                    (dt + dt, (leg.get_pos() + self.move_direction))]

                        dt = self.move_time / 4
                        dv = self.move_direction / 4
                        dv_left = rotate(dv, angle=pi / 2,   axis=self.f_1.up)
                        dv_rigth = rotate(dv, angle=-pi / 2, axis=self.f_1.up)
                        
                        self.body_pos_anim = [(dt * 0, self.get_pos()),
                                              (dt * 1, self.get_pos() + dv * 1 + dv_left + self.f_1.up * max_up),
                                              (dt * 2, self.get_pos() + dv * 2),
                                              (dt * 3, self.get_pos() + dv * 3 + dv_rigth + self.f_1.up * max_up),
                                              (dt * 4, self.get_pos() + dv * 4)]

                        self.body_angle_anim = [(0.0, vector(self.angle, 0.0, 0.0))]
                        self.first_step = False
                        self.time = 0.0
                        self.max_time = self.move_time
                        
                else:
                    #определим последовательность ног
                    for id in [0, 1, 2, 3]:
                        leg = self.legs[id]
                        new_pos_1 = self.get_pos() + rotate(vector=(leg.get_pos() - self.get_pos()),
                                                            angle=self.move_angle / 2,
                                                            axis=self.f_1.up)

                        new_pos_2 = self.get_pos() + rotate(vector=(leg.get_pos() - self.get_pos()),
                                                            angle=self.move_angle,
                                                            axis=self.f_1.up)

                        leg.anim = [(0.0 + dt, vector(leg.get_pos())),
                                    (leg_move_time / 2.0 + dt, new_pos_1 + self.f_1.up * max_up),
                                    (leg_move_time + dt, new_pos_2)]

                        dt = dt + leg_move_time

                    self.body_pos_anim = [(0.0, vector(self.get_pos()))]
                    self.body_angle_anim = [(0.0, vector(self.angle, 0.0, 0.0)),
                                            (self.move_time, vector(self.angle + self.move_angle, 0.0, 0.0))]
                    
                    self.first_step = False
                    self.time = 0.0
                    self.max_time = self.move_time

            self.time = self.time + 1.0 / rate
            
            #анимация тела
            self.set_pos(self.get_data_by_time(self.body_pos_anim, self.time))
            self.set_angle(self.get_data_by_time(self.body_angle_anim, self.time)[0])

            #анимация ног
            for leg in self.legs:
                leg.set_pos(self.get_data_by_time(leg.anim, self.time))

            #проверка конца анимации.
            if self.time >= self.max_time:
                self.can_move = False
                self.first_step = True