mems_ctrnn/visual_agent.py at master · rafaie/mems_ctrnn · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
from vagent_mems_ctrnn import VAgent_MEMS_CTRNN
from vagent_mems_ctrnn_acc import VAgent_MEMS_CTRNN_ACC
from visual_object import Ray
import math


class VisualAgent:

    # diameter of agent
    BODY_SIZE = 30.0
    ENV_WIDTH = 400.0
    MAX_RAY_LENGTH = 220.0
    INPUT_GAIN = 10.0
    VISUAL_ANGLE = math.pi/6
    VEL_GAIN = 5

    def __init__(self, ix=0.0, iy=0.0, num_rays_=7, agent_vel_x=None,
                 stability_acc=0.001,
                 stability_hist_bucket=3,
                 stability_min_iteration=7,
                 stability_max_iteration=150,
                 is_type_acc=False):

        self.cx = 0
        self.cy = 0
        self.vx = 0

        self.stability_acc = stability_acc
        self.stability_hist_bucket = stability_hist_bucket
        self.stability_min_iteration = stability_min_iteration
        self.stability_max_iteration = stability_max_iteration

        self.num_rays = num_rays_
        self.rays = [Ray() for i in range(self.num_rays)]

        v = VAgent_MEMS_CTRNN(stability_acc=stability_acc,
                              stability_hist_bucket=stability_hist_bucket,
                              stability_min_iteration=stability_min_iteration,
                              stability_max_iteration=stability_max_iteration)
        if is_type_acc is True:
            v = VAgent_MEMS_CTRNN_ACC(stability_acc=stability_acc,
                            stability_hist_bucket=stability_hist_bucket,
                            stability_min_iteration=stability_min_iteration,
                            stability_max_iteration=stability_max_iteration)

        self.nervous_system = v

        self.reset(ix, iy)

        if agent_vel_x is not None:
            VisualAgent.VEL_GAIN = agent_vel_x

    # Accessors
    def positionX(self):
        return self.cx

    def set_positionX(self, new_x):
        self.cx = new_x
        self.reset_rays()

    def positionY(self):
        return self.cy

    def reset(self, ix, iy, rs=None, randomize=0):
        self.cx = ix
        self.cy = iy
        self.vx = 0.0
        self.reset_rays()

    def step(self, step_size, object, mem_step_size=1, show_details=False,
             use_defelection_feedback=False,
             return_states_info=False):
        self.reset_rays()
        for i in range(self.num_rays):
            object.ray_intersection(self.rays[i])
            external_input = VisualAgent.INPUT_GAIN * \
                (VisualAgent.MAX_RAY_LENGTH -
                 self.rays[i].length)/VisualAgent.MAX_RAY_LENGTH
            if show_details is True:
                print('==> ', i, external_input)
                print(self.rays[i])
                print(object)

            self.nervous_system.set_neuron_external_input(i, external_input)

        # Step nervous system
        st = self.nervous_system.euler_step(step_size=mem_step_size,
                                            use_defelection_feedback=
                                            use_defelection_feedback,
                                            return_states_info=
                                            return_states_info)

        # Update agent state
        self.vx = VisualAgent.VEL_GAIN * (self.nervous_system.outputs[0] -
                                          self.nervous_system.outputs[1])

        self.cx += step_size * self.vx

        if self.cx < -VisualAgent.ENV_WIDTH/2:
            self.cx = -VisualAgent.ENV_WIDTH/2
        elif self.cx > VisualAgent.ENV_WIDTH/2:
            self.cx = VisualAgent.ENV_WIDTH/2

        return st

    def reset_ray(self, ray, theta, cx, cy):
        if abs(theta) < 0.0000001:
            # special case, vertical ray
            ray.m = math.inf
        else:
            ray.m = 1 / math.tan(theta)

        ray.b = cy - ray.m * cx
        ray.length = VisualAgent.MAX_RAY_LENGTH

        # Set starting coordinates (i.e. on upper perimeter of agent body)
        if ray.m == math.inf:
            ray.startX = cx
            ray.startY = cy + VisualAgent.BODY_SIZE / 2
            return

        ray.startX = cx + (VisualAgent.BODY_SIZE / 2) * math.sin(theta)
        ray.startY = cy + (VisualAgent.BODY_SIZE / 2) * math.cos(theta)

    def reset_rays(self):
        theta = - VisualAgent.VISUAL_ANGLE / 2
        for i in range(self.num_rays):
            self.reset_ray(self.rays[i], theta, self.cx, self.cy)
            theta += VisualAgent.VISUAL_ANGLE/(self.num_rays - 1)