def omegadot(self, m, n, t):
(tf, x, y, theta, v, omega, J, dx_dtau, dy_dtau, \
dtheta_dtau, dv_dtau, domega_dtau, dJ_dtau, \
F_control, T_control, F_controldot, T_controldot, \
F_virtual, T_virtual, dF_control_dtau, dT_control_dtau) = getPhaseVariables(m, n, t)
return m.I * (domega_dtau) == tf * (T_virtual)
def TorqueControlDot(self, m, n, t):
(tf, x, y, theta, v, omega, J, dx_dtau, dy_dtau, \
dtheta_dtau, dv_dtau, domega_dtau, dJ_dtau, \
F_control, T_control, F_controldot, T_controldot, \
F_virtual, T_virtual, dF_control_dtau, dT_control_dtau) = getPhaseVariables(m, n, t)
return dT_control_dtau == tf * T_controldot
def ellipseX(self, m, t, n):
(tf, x, y, theta, v, omega, J, dx_dtau, dy_dtau, \
dtheta_dtau, dv_dtau, domega_dtau, dJ_dtau, \
F_control, T_control, F_controldot, T_controldot, \
F_virtual, T_virtual, dF_control_dtau, dT_control_dtau) = getPhaseVariables(m, n, t)
return 1 / (self.b / 2)**2 * (y - self.c[1])**2 + 1 / (
self.a / 2)**2 * (x - self.c[0])**2 >= 1
def JdotTotalTime(self, m, n, t):
(tf, x, y, theta, v, omega, J, dx_dtau, dy_dtau, \
dtheta_dtau, dv_dtau, domega_dtau, dJ_dtau, \
F_control, T_control, F_controldot, T_controldot, \
F_virtual, T_virtual, dF_control_dtau, dT_control_dtau) = getPhaseVariables(m, n, t)
## Penalize total time:
return dJ_dtau == tf * 1
def JdotTravelTime(self, m, n, t):
(tf, x, y, theta, v, omega, J, dx_dtau, dy_dtau, \
dtheta_dtau, dv_dtau, domega_dtau, dJ_dtau, \
F_control, T_control, F_controldot, T_controldot, \
F_virtual, T_virtual, dF_control_dtau, dT_control_dtau) = getPhaseVariables(m, n, t)
## Penalize total travel in (x,y):
return dJ_dtau == tf * ((x - m.x1[0])**2 + (y - m.y1[0])**2)
def TorqueControl(self, m, n, t):
(tf, x, y, theta, v, omega, J, dx_dtau, dy_dtau, \
dtheta_dtau, dv_dtau, domega_dtau, dJ_dtau, \
F_control, T_control, F_controldot, T_controldot, \
F_virtual, T_virtual, dF_control_dtau, dT_control_dtau) = getPhaseVariables(m, n, t)
(cv, cvw) = self.getCvCvw(v)
T_aero = 0.2 * (theta) + cvw * (v)
return T_control == -T_aero + T_virtual
def ForceControl(self, m, n, t):
(tf, x, y, theta, v, omega, J, dx_dtau, dy_dtau, \
dtheta_dtau, dv_dtau, domega_dtau, dJ_dtau, \
F_control, T_control, F_controldot, T_controldot, \
F_virtual, T_virtual, dF_control_dtau, dT_control_dtau) = getPhaseVariables(m, n, t)
(cv, cvw) = self.getCvCvw(v)
F_aero = 0.1 * (omega) + cv * (v)
return F_control == -F_aero + F_virtual