human-aware-legibility/NavDynamicsAuto.py at master · HARPLab/human-aware-legibility · 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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
import numpy as np
import theano.tensor as T
from ilqr.dynamics import FiniteDiffDynamics, tensor_constrain, constrain, BatchAutoDiffDynamics, AutoDiffDynamics


class NavDynamicsAuto(AutoDiffDynamics):

    """Inverted pendulum auto-differentiated dynamics model."""

    def __init__(self,
                 exp,
                 constrain=True,
                 min_bounds=-0.001, #-1, 1
                 max_bounds=0.001,
                 **kwargs):
        """Constructs an InvertedPendulumDynamics model.

        Args:
            dt: Time step [s].
            constrain: Whether to constrain the action space or not.
            min_bounds: Minimum bounds for action [N m].
            max_bounds: Maximum bounds for action [N m].
            m: Pendulum mass [kg].
            l: Pendulum length [m].
            g: Gravity acceleration [m/s^2].
            **kwargs: Additional key-word arguments to pass to the
                BatchAutoDiffDynamics constructor.

        Note:
            state: [sin(theta), cos(theta), theta']
            action: [torque]
        """
        k = exp.get_dist_scalar_k()

        self.constrained    = constrain
        self.min_bounds     = min_bounds  #* exp.get_dt() * k * .1 # .3
        self.max_bounds     = max_bounds    # * exp.get_dt() * k * .1 #.3

        print("DYNAM: min max bounds")
        print(self.min_bounds, self.max_bounds)

        self.exp            = exp
        self._has_hessians  = True

        dt = self.exp.get_dt()

        x_x = T.dscalar("x_x")
        x_y = T.dscalar("x_y")
        x_x_prev = T.dscalar("x_x_prev")
        x_y_prev = T.dscalar("x_y_prev")

        u_x = T.dscalar("u_x")
        u_y = T.dscalar("u_y")

        min_bounds, max_bounds = self.min_bounds, self.max_bounds

        # # Constrain action space.
        if False: #self.constrained:
            print("speed limit")
            # print([u[0], u[1]])
            u = tensor_constrain([u_x, u_y], min_bounds, max_bounds)
            # print(u)
            u_x = u[0]
            u_y = u[1]

        f = T.stack([
                x_x + u_x,
                x_y + u_y,
                x_x,
                x_y
            ])

        x_inputs = [x_x, x_y, x_x_prev, x_y_prev]
        u_inputs = [u_x, u_y]
        super(NavDynamicsAuto, self).__init__(f, x_inputs, u_inputs, hessians=True,
                                                **kwargs)

        # super(NavDynamics, self).__init__(f,    state_size=4,
        #                                         action_size=2,
        #                                         **kwargs)


    # def f_x(self, x, u, i):
    #     """Partial derivative of dynamics model with respect to x.

    #     Args:
    #         x: Current state [state_size].
    #         u: Current control [action_size].
    #         i: Current time step.

    #     Returns:
    #         df/dx [state_size, state_size].
    #     """
    #     z = np.hstack([x, u, i])
    #     return self._f_x(*z)

    # def f_u(self, x, u, i):
    #     """Partial derivative of dynamics model with respect to u.

    #     Args:
    #         x: Current state [state_size].
    #         u: Current control [action_size].
    #         i: Current time step.

    #     Returns:
    #         df/du [state_size, action_size].
    #     """
    #     z = np.hstack([x, u, i])
    #     return self._f_u(*z)

    # def f_xx(self, x, u, i):
    #     """Second partial derivative of dynamics model with respect to x.

    #     Args:
    #         x: Current state [state_size].
    #         u: Current control [action_size].
    #         i: Current time step.

    #     Returns:
    #         d^2f/dx^2 [state_size, state_size, state_size].
    #     """
    #     if not self._has_hessians:
    #         raise NotImplementedError

    #     z = np.hstack([x, u, i])
    #     return self._f_xx(*z)

    # def f_ux(self, x, u, i):
    #     """Second partial derivative of dynamics model with respect to u and x.

    #     Args:
    #         x: Current state [state_size].
    #         u: Current control [action_size].
    #         i: Current time step.

    #     Returns:
    #         d^2f/dudx [state_size, action_size, state_size].
    #     """
    #     if not self._has_hessians:
    #         raise NotImplementedError

    #     z = np.hstack([x, u, i])
    #     return self._f_ux(*z)

    # def f_uu(self, x, u, i):
    #     """Second partial derivative of dynamics model with respect to u.

    #     Args:
    #         x: Current state [state_size].
    #         u: Current control [action_size].
    #         i: Current time step.

    #     Returns:
    #         d^2f/du^2 [state_size, action_size, action_size].
    #     """
    #     if not self._has_hessians:
    #         raise NotImplementedError

    #     z = np.hstack([x, u, i])
    #     return self._f_uu(*z)