def boat_dynamics(cls, s, u):
'''
Calculates the dynamics, i.e.:
\dot{state} = f(state,u)
for the rocket + two planets system.
:param state: numpy array, length 4, comprising state of system:
[x, y, \dot{x}, \dot{y}]
:param u: numpy array, length 2, comprising control input for system:
[\ddot{x}_u, \ddot{y}_u]
Note that this is only the added acceleration, note the total acceleration.
:return: numpy array, length 4, comprising the time derivative of the system state:
[\dot{x}, \dot{y}, \ddot{x}, \ddot{y}]
'''
derivs = np.zeros_like(s)
derivs[0] = cos(s[2]) * s[3] - sin(s[2]) * s[4];
derivs[1] = sin(s[2]) * s[3] + cos(s[2]) * s[4];
derivs[2] = s[5];
derivs[3] = -cls.d11 / cls.m11 * s[3] + u[0] / cls.m11 + u[1] / cls.m11;
derivs[4] = -cls.d22 / cls.m22 * s[4] + u[2] / cls.m22 + u[3] / cls.m22;
derivs[5] = -cls.d33 / cls.m33 * s[5] + cls.width / (2 * cls.m33) * u[0] - cls.width / (2 * cls.m33) * u[1] + cls.height / (2 * cls.m33) * u[2] - cls.height / (2 * cls.m33) * u[3];
return derivs
def CalcClosestLocationQuadBall(q_quad, q_ball):
if not isinstance(q_quad[2], Variable):
R_i = np.array([[np.cos(q_quad[2]) , -np.sin(q_quad[2])],
[np.sin(q_quad[2]) , np.cos(q_quad[2])]])
else:
R_i = np.array([[cos(q_quad[2]) , -sin(q_quad[2])],
[sin(q_quad[2]) , cos(q_quad[2])]])
R_inv_i = R_i.T
#
q_dash = R_inv_i.dot(q_ball - q_quad[:2])
p_closest = np.clip(q_dash, [-w_quad, -h_quad], [w_quad, h_quad])
# Handle case where q_ball is inside the quadrotor base
if p_closest[0] > -w_quad and p_closest[0] < w_quad and p_closest[1] > -h_quad and p_closest[1] < h_quad:
if p_closest[1] >= 0 and p_closest[1] >= h_quad -w_quad - p_closest[0] and p_closest[1] >= h_quad -w_quad + p_closest[0]:
p_closest[1] = h_quad
elif p_closest[1] < 0 and p_closest[1] <= -h_quad -w_quad + p_closest[0] and p_closest[1] <= -h_quad -w_quad - p_closest[0]:
p_closest[1] = -h_quad
elif p_closest[0] < -w_quad+h_quad:
p_closest[0] = -w_quad
else: # p_closest[0] > w_quad-h_quad
p_closest[0] = w_quad
return q_quad[:2] + R_i.dot(p_closest)
def get_ddots(Fx, Fy, F_leg, u0):
alpha = (self.l1 * Fy * sym.sin(self.x[2]) -
self.l1 * Fx * sym.cos(self.x[2]) - u0)
A = sym.cos(self.x[2]) * alpha - R * (
Fx - F_leg * sym.sin(self.x[2]) -
self.m_l * self.l1 * self.x[7]**2 * sym.sin(self.x[2]))
B = sym.sin(self.x[2]) * alpha + R * (
self.m_l * self.l1 * self.x[7]**2 * sym.cos(self.x[2]) + Fy -
F_leg * sym.cos(self.x[2]) - self.m_l * self.g)
C = sym.cos(self.x[2]) * alpha + R * F_leg * sym.sin(
self.x[2]) + self.m * R * (
self.x[4] * self.x[7]**2 * sym.sin(self.x[2]) +
self.l2 * self.x[8]**2 * sym.sin(self.x[3]) -
2 * self.x[9] * self.x[7] * sym.cos(self.x[2]))
D = sym.sin(self.x[2]) * alpha - R * (
F_leg * sym.cos(self.x[2]) - self.m * self.g) - self.m * R * (
2 * self.x[9] * self.x[7] * sym.sin(self.x[2]) +
self.x[4] * self.x[7]**2 * sym.cos(self.x[2]) +
self.l2 * self.x[8]**2 * sym.cos(self.x[3]))
E = self.l2 * sym.cos(self.x[2] - self.x[3]) * alpha - R * (
self.l2 * F_leg * sym.sin(self.x[3] - self.x[2]) + u0)
return np.asarray([(A*b1*c2*d4*e2 - A*b1*c3*d4*e1 - A*b1*c4*d2*e2 + A*b1*c4*d3*e1 + A*b2*c4*d1*e2 - B*a2*c4*d1*e2 - C*a2*b1*d4*e2 + D*a2*b1*c4*e2 + E*a2*b1*c3*d4 - E*a2*b1*c4*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
(A*b2*c1*d4*e2 + B*a1*c2*d4*e2 - B*a1*c3*d4*e1 - B*a1*c4*d2*e2 + B*a1*c4*d3*e1 - B*a2*c1*d4*e2 - C*a1*b2*d4*e2 + D*a1*b2*c4*e2 + E*a1*b2*c3*d4 - E*a1*b2*c4*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
-(A*b1*c1*d4*e2 - B*a1*c4*d1*e2 - C*a1*b1*d4*e2 + D*a1*b1*c4*e2 + E*a1*b1*c3*d4 - E*a1*b1*c4*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
(A*b1*c1*d4*e1 - B*a1*c4*d1*e1 - C*a1*b1*d4*e1 + D*a1*b1*c4*e1 + E*a1*b1*c2*d4 - E*a1*b1*c4*d2 + E*a1*b2*c4*d1 - E*a2*b1*c1*d4)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
(A*b1*c1*d2*e2 - A*b1*c1*d3*e1 - A*b2*c1*d1*e2 - B*a1*c2*d1*e2 + B*a1*c3*d1*e1 + B*a2*c1*d1*e2 - C*a1*b1*d2*e2 + C*a1*b1*d3*e1 + C*a1*b2*d1*e2 + D*a1*b1*c2*e2 - D*a1*b1*c3*e1 - D*a2*b1*c1*e2 - E*a1*b1*c2*d3 + E*a1*b1*c3*d2 - E*a1*b2*c3*d1 + E*a2*b1*c1*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2)])
def get_plant_and_pos():
""""Set up plant and position systems."""
# Inputs
kappa_F = sym.Variable("kappa_F")
kappa_R = sym.Variable("kappa_R")
delta = sym.Variable("delta")
# Outputs
r = sym.Variable("r")
r_dot = sym.Variable("r_dot")
theta = sym.Variable("theta")
theta_dot = sym.Variable("theta_dot")
phi = sym.Variable("phi")
phi_dot = sym.Variable("phi_dot")
# v = r_dot r_hat + r theta_dot theta_hat
v_r_hat = r_dot
v_theta_hat = r * theta_dot
beta = sym.atan2(v_r_hat, v_theta_hat)
# Slip angles
alpha_F = phi - delta - beta
alpha_R = phi - beta
F_xf = sym.cos(delta) * S_FL * kappa_F - sym.sin(delta) * S_FC * alpha_F
F_xr = S_RL * kappa_R
F_yf = sym.sin(delta) * S_FL * kappa_F + sym.cos(delta) * S_FC * alpha_F
F_yr = S_RC * alpha_R
F_x = F_xf + F_xr
F_y = F_yr + F_yf
plant_state = np.array([r, r_dot, theta_dot, phi, phi_dot])
theta_ddot = (F_x*sym.cos(phi) - F_y*sym.sin(phi)) / \
(m*r) - 2*r_dot * theta_dot/r
plant_dynamics = np.array([
r_dot,
(F_x * sym.sin(phi) + F_y * sym.cos(phi)) / m + r * theta_dot**2,
theta_ddot, phi_dot, theta_ddot - (l_F * F_yf - l_R * F_yr) / Iz
])
plant_input = [kappa_F, kappa_R, delta]
plant_vector_system = SymbolicVectorSystem(state=plant_state,
input=plant_input,
dynamics=plant_dynamics,
output=plant_state)
position_state = [theta]
position_dynamics = [theta_dot]
position_system = SymbolicVectorSystem(state=position_state,
input=plant_state,
dynamics=position_dynamics,
output=position_state)
return plant_vector_system, position_system
def y_expr_prism(state):
# type: (List[float])->list[float]
""" Return the y coordinate of the prism (floating base( walker as a float
I.E. using sym.cos
TODO more documentation
"""
y1_in = sym.cos(state[2])
y2_in = y1_in + sym.cos(state[2] + state[3])
y4_in = y2_in + sym.cos(state[2] + state[3] + state[5])
y5_in = y4_in + sym.cos(state[2] + state[3] + state[5] + state[6])
return -y5_in
def y_expr(state):
# type: (List[float])->list[float]
""" Return the y coordinate of the drake_walkers toe as an expression
I.E. using pydrake.cos
TODO more documentation
"""
y1_in = sym.cos(state[0])
y2_in = y1_in + sym.cos(state[0] + state[1])
y4_in = y2_in + sym.cos(state[0] + state[1] + state[3])
y5_in = y4_in + sym.cos(state[0] + state[1] + state[3] + state[4])
return -y5_in
def test_non_method_jacobian(self):
# Jacobian([x * cos(y), x * sin(y), x ** 2], [x, y]) returns
# the following 3x2 matrix:
#
# = |cos(y) -x * sin(y)|
# |sin(y) x * cos(y)|
# | 2 * x 0|
J = sym.Jacobian([x * sym.cos(y), x * sym.sin(y), x**2], [x, y])
self._check_scalar(J[0, 0], sym.cos(y))
self._check_scalar(J[1, 0], sym.sin(y))
self._check_scalar(J[2, 0], 2 * x)
self._check_scalar(J[0, 1], -x * sym.sin(y))
self._check_scalar(J[1, 1], x * sym.cos(y))
self._check_scalar(J[2, 1], 0)
def test_non_method_jacobian(self):
# Jacobian([x * cos(y), x * sin(y), x ** 2], [x, y]) returns
# the following 3x2 matrix:
#
# = |cos(y) -x * sin(y)|
# |sin(y) x * cos(y)|
# | 2 * x 0|
J = sym.Jacobian([x * sym.cos(y), x * sym.sin(y), x ** 2], [x, y])
self._check_scalar(J[0, 0], sym.cos(y))
self._check_scalar(J[1, 0], sym.sin(y))
self._check_scalar(J[2, 0], 2 * x)
self._check_scalar(J[0, 1], - x * sym.sin(y))
self._check_scalar(J[1, 1], x * sym.cos(y))
self._check_scalar(J[2, 1], 0)
def test_non_method_jacobian(self):
# Jacobian([x * cos(y), x * sin(y), x ** 2], [x, y]) returns
# the following 3x2 matrix:
#
# = |cos(y) -x * sin(y)|
# |sin(y) x * cos(y)|
# | 2 * x 0|
J = sym.Jacobian([x * sym.cos(y), x * sym.sin(y), x ** 2], [x, y])
npc.assert_equal(J[0, 0], sym.cos(y))
npc.assert_equal(J[1, 0], sym.sin(y))
npc.assert_equal(J[2, 0], 2 * x)
npc.assert_equal(J[0, 1], - x * sym.sin(y))
npc.assert_equal(J[1, 1], x * sym.cos(y))
npc.assert_equal(J[2, 1], sym.Expression(0))
def test_non_method_jacobian(self):
# Jacobian([x * cos(y), x * sin(y), x ** 2], [x, y]) returns
# the following 3x2 matrix:
#
# = |cos(y) -x * sin(y)|
# |sin(y) x * cos(y)|
# | 2 * x 0|
J = sym.Jacobian([x * sym.cos(y), x * sym.sin(y), x**2], [x, y])
numpy_compare.assert_equal(J[0, 0], sym.cos(y))
numpy_compare.assert_equal(J[1, 0], sym.sin(y))
numpy_compare.assert_equal(J[2, 0], 2 * x)
numpy_compare.assert_equal(J[0, 1], -x * sym.sin(y))
numpy_compare.assert_equal(J[1, 1], x * sym.cos(y))
numpy_compare.assert_equal(J[2, 1], sym.Expression(0))
def test_functions_with_float(self):
# TODO(eric.cousineau): Use concrete values once vectorized methods are
# supported.
v_x = 1.0
v_y = 1.0
self.assertEqualStructure(sym.abs(v_x), np.abs(v_x))
self.assertNotEqualStructure(sym.abs(v_x), 0.5 * np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.exp(v_x), np.exp(v_x))
self._check_scalar(sym.sqrt(v_x), np.sqrt(v_x))
self._check_scalar(sym.pow(v_x, v_y), v_x**v_y)
self._check_scalar(sym.sin(v_x), np.sin(v_x))
self._check_scalar(sym.cos(v_x), np.cos(v_x))
self._check_scalar(sym.tan(v_x), np.tan(v_x))
self._check_scalar(sym.asin(v_x), np.arcsin(v_x))
self._check_scalar(sym.acos(v_x), np.arccos(v_x))
self._check_scalar(sym.atan(v_x), np.arctan(v_x))
self._check_scalar(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self._check_scalar(sym.sinh(v_x), np.sinh(v_x))
self._check_scalar(sym.cosh(v_x), np.cosh(v_x))
self._check_scalar(sym.tanh(v_x), np.tanh(v_x))
self._check_scalar(sym.min(v_x, v_y), min(v_x, v_y))
self._check_scalar(sym.max(v_x, v_y), max(v_x, v_y))
self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
self._check_scalar(sym.floor(v_x), np.floor(v_x))
self._check_scalar(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y), v_x, v_y),
v_x if v_x > v_y else v_y)
def test_functions_with_float(self):
v_x = 1.0
v_y = 1.0
self.assertEqual(sym.abs(v_x), np.abs(v_x))
self.assertEqual(sym.exp(v_x), np.exp(v_x))
self.assertEqual(sym.sqrt(v_x), np.sqrt(v_x))
self.assertEqual(sym.pow(v_x, v_y), v_x**v_y)
self.assertEqual(sym.sin(v_x), np.sin(v_x))
self.assertEqual(sym.cos(v_x), np.cos(v_x))
self.assertEqual(sym.tan(v_x), np.tan(v_x))
self.assertEqual(sym.asin(v_x), np.arcsin(v_x))
self.assertEqual(sym.acos(v_x), np.arccos(v_x))
self.assertEqual(sym.atan(v_x), np.arctan(v_x))
self.assertEqual(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self.assertEqual(sym.sinh(v_x), np.sinh(v_x))
self.assertEqual(sym.cosh(v_x), np.cosh(v_x))
self.assertEqual(sym.tanh(v_x), np.tanh(v_x))
self.assertEqual(sym.min(v_x, v_y), min(v_x, v_y))
self.assertEqual(sym.max(v_x, v_y), max(v_x, v_y))
self.assertEqual(sym.ceil(v_x), np.ceil(v_x))
self.assertEqual(sym.floor(v_x), np.floor(v_x))
self.assertEqual(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y), v_x, v_y),
v_x if v_x > v_y else v_y)
def test_functions_with_float(self):
# TODO(eric.cousineau): Use concrete values once vectorized methods are
# supported.
v_x = 1.0
v_y = 1.0
self.assertEqualStructure(sym.abs(v_x), np.abs(v_x))
self.assertNotEqualStructure(sym.abs(v_x), 0.5*np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.exp(v_x), np.exp(v_x))
self._check_scalar(sym.sqrt(v_x), np.sqrt(v_x))
self._check_scalar(sym.pow(v_x, v_y), v_x ** v_y)
self._check_scalar(sym.sin(v_x), np.sin(v_x))
self._check_scalar(sym.cos(v_x), np.cos(v_x))
self._check_scalar(sym.tan(v_x), np.tan(v_x))
self._check_scalar(sym.asin(v_x), np.arcsin(v_x))
self._check_scalar(sym.acos(v_x), np.arccos(v_x))
self._check_scalar(sym.atan(v_x), np.arctan(v_x))
self._check_scalar(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self._check_scalar(sym.sinh(v_x), np.sinh(v_x))
self._check_scalar(sym.cosh(v_x), np.cosh(v_x))
self._check_scalar(sym.tanh(v_x), np.tanh(v_x))
self._check_scalar(sym.min(v_x, v_y), min(v_x, v_y))
self._check_scalar(sym.max(v_x, v_y), max(v_x, v_y))
self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
self._check_scalar(sym.floor(v_x), np.floor(v_x))
self._check_scalar(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y),
v_x, v_y),
v_x if v_x > v_y else v_y)
def dynamics(self, quad_q, quad_u, dt):
# Quadrotor
a_f = quad_u[0] * 1.0 / self.quad_mass
r_ddot = quad_u[1]
tang = np.array([cos(quad_q[2]), sin(quad_q[2])])
norm = np.array([-sin(quad_q[2]), cos(quad_q[2])])
pos_ddot = norm * a_f + self.g_vec
quad_next = np.zeros_like(quad_q)
quad_next[0] = quad_q[0] + quad_q[3] * dt + 0.5 * pos_ddot[0] * dt**2.0
quad_next[1] = quad_q[1] + quad_q[4] * dt + 0.5 * pos_ddot[1] * dt**2.0
quad_next[2] = quad_q[2] + quad_q[5] * dt + 0.5 * r_ddot * dt**2.0
quad_next[3] = quad_q[3] + pos_ddot[0] * dt
quad_next[4] = quad_q[4] + pos_ddot[1] * dt
quad_next[5] = quad_q[5] + r_ddot * dt
return quad_next
def nonlinear_dynamics_constraints(prog, decision_vars, x_bar, u_bar, N, dt):
''' Add nonlinear dynamics constraints in original form '''
x, u, _, _ = decision_vars
for n in range(N-1):
psi = x[2, n]
v = x[3, n]
steer = x[4, n]
prog.AddConstraint(x[0, n+1] == x[0, n] + dt*v*sym.cos(psi))
prog.AddConstraint(x[1, n+1] == x[1, n] + dt*v*sym.sin(psi))
prog.AddConstraint(x[2, n+1] == x[2, n] + dt*v*sym.tan(steer))
prog.AddConstraint(x[3, n+1] == x[3, n] + dt*u[0, n])
prog.AddConstraint(x[4, n+1] == x[4, n] + dt*u[1, n])
return prog
def test_functions_with_variable(self):
self.assertEqual(str(sym.abs(x)), "abs(x)")
self.assertEqual(str(sym.exp(x)), "exp(x)")
self.assertEqual(str(sym.sqrt(x)), "sqrt(x)")
self.assertEqual(str(sym.pow(x, y)), "pow(x, y)")
self.assertEqual(str(sym.sin(x)), "sin(x)")
self.assertEqual(str(sym.cos(x)), "cos(x)")
self.assertEqual(str(sym.tan(x)), "tan(x)")
self.assertEqual(str(sym.asin(x)), "asin(x)")
self.assertEqual(str(sym.acos(x)), "acos(x)")
self.assertEqual(str(sym.atan(x)), "atan(x)")
self.assertEqual(str(sym.atan2(x, y)), "atan2(x, y)")
self.assertEqual(str(sym.sinh(x)), "sinh(x)")
self.assertEqual(str(sym.cosh(x)), "cosh(x)")
self.assertEqual(str(sym.tanh(x)), "tanh(x)")
self.assertEqual(str(sym.min(x, y)), "min(x, y)")
self.assertEqual(str(sym.max(x, y)), "max(x, y)")
self.assertEqual(str(sym.ceil(x)), "ceil(x)")
self.assertEqual(str(sym.floor(x)), "floor(x)")
self.assertEqual(str(sym.if_then_else(x > y, x, y)),
"(if (x > y) then x else y)")
def test_functions_with_expression(self):
self.assertEqual(str(sym.abs(e_x)), "abs(x)")
self.assertEqual(str(sym.exp(e_x)), "exp(x)")
self.assertEqual(str(sym.sqrt(e_x)), "sqrt(x)")
self.assertEqual(str(sym.pow(e_x, e_y)), "pow(x, y)")
self.assertEqual(str(sym.sin(e_x)), "sin(x)")
self.assertEqual(str(sym.cos(e_x)), "cos(x)")
self.assertEqual(str(sym.tan(e_x)), "tan(x)")
self.assertEqual(str(sym.asin(e_x)), "asin(x)")
self.assertEqual(str(sym.acos(e_x)), "acos(x)")
self.assertEqual(str(sym.atan(e_x)), "atan(x)")
self.assertEqual(str(sym.atan2(e_x, e_y)), "atan2(x, y)")
self.assertEqual(str(sym.sinh(e_x)), "sinh(x)")
self.assertEqual(str(sym.cosh(e_x)), "cosh(x)")
self.assertEqual(str(sym.tanh(e_x)), "tanh(x)")
self.assertEqual(str(sym.min(e_x, e_y)), "min(x, y)")
self.assertEqual(str(sym.max(e_x, e_y)), "max(x, y)")
self.assertEqual(str(sym.ceil(e_x)), "ceil(x)")
self.assertEqual(str(sym.floor(e_x)), "floor(x)")
self.assertEqual(str(sym.if_then_else(e_x > e_y, e_x, e_y)),
"(if (x > y) then x else y)")
def test_functions_with_variable(self):
self.assertEqual(str(sym.abs(x)), "abs(x)")
self.assertEqual(str(sym.exp(x)), "exp(x)")
self.assertEqual(str(sym.sqrt(x)), "sqrt(x)")
self.assertEqual(str(sym.pow(x, y)), "pow(x, y)")
self.assertEqual(str(sym.sin(x)), "sin(x)")
self.assertEqual(str(sym.cos(x)), "cos(x)")
self.assertEqual(str(sym.tan(x)), "tan(x)")
self.assertEqual(str(sym.asin(x)), "asin(x)")
self.assertEqual(str(sym.acos(x)), "acos(x)")
self.assertEqual(str(sym.atan(x)), "atan(x)")
self.assertEqual(str(sym.atan2(x, y)), "atan2(x, y)")
self.assertEqual(str(sym.sinh(x)), "sinh(x)")
self.assertEqual(str(sym.cosh(x)), "cosh(x)")
self.assertEqual(str(sym.tanh(x)), "tanh(x)")
self.assertEqual(str(sym.min(x, y)), "min(x, y)")
self.assertEqual(str(sym.max(x, y)), "max(x, y)")
self.assertEqual(str(sym.ceil(x)), "ceil(x)")
self.assertEqual(str(sym.floor(x)), "floor(x)")
self.assertEqual(str(sym.if_then_else(x > y, x, y)),
"(if (x > y) then x else y)")
def test_functions_with_float(self):
# TODO(eric.cousineau): Use concrete values once vectorized methods are
# supported.
v_x = 1.0
v_y = 1.0
# WARNING: If these math functions have `float` overloads that return
# `float`, then `assertEqual`-like tests are meaningful (current state,
# and before `math` overloads were introduced).
# If these math functions implicitly cast `float` to `Expression`, then
# `assertEqual` tests are meaningless, as it tests `__nonzero__` for
# `Formula`, which will always be True.
self.assertEqual(sym.abs(v_x), 0.5*np.abs(v_x))
self.assertNotEqual(str(sym.abs(v_x)), str(0.5*np.abs(v_x)))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.exp(v_x), np.exp(v_x))
self._check_scalar(sym.sqrt(v_x), np.sqrt(v_x))
self._check_scalar(sym.pow(v_x, v_y), v_x ** v_y)
self._check_scalar(sym.sin(v_x), np.sin(v_x))
self._check_scalar(sym.cos(v_x), np.cos(v_x))
self._check_scalar(sym.tan(v_x), np.tan(v_x))
self._check_scalar(sym.asin(v_x), np.arcsin(v_x))
self._check_scalar(sym.acos(v_x), np.arccos(v_x))
self._check_scalar(sym.atan(v_x), np.arctan(v_x))
self._check_scalar(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self._check_scalar(sym.sinh(v_x), np.sinh(v_x))
self._check_scalar(sym.cosh(v_x), np.cosh(v_x))
self._check_scalar(sym.tanh(v_x), np.tanh(v_x))
self._check_scalar(sym.min(v_x, v_y), min(v_x, v_y))
self._check_scalar(sym.max(v_x, v_y), max(v_x, v_y))
self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
self._check_scalar(sym.floor(v_x), np.floor(v_x))
self._check_scalar(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y),
v_x, v_y),
v_x if v_x > v_y else v_y)
def __init__(self, m=5, J=500, m_l=1, J_l=0.5, l1=0.0, l2=0.0, k_g=2e3, b_g=20, \
g=9.8, flight_step_size = 1e-2, contact_step_size = 5e-3, descend_step_size_switch_threshold=2e-2, \
ground_height_function=lambda x: 0, initial_state=np.asarray([0.,0.,0.,1.5,1.0,0.,0.,0.,0.,0.])):
'''
2D hopper with actuated piston at the end of the leg.
The model of the hopper follows the one described in "Hopping in Legged Systems" (Raibert, 1984)
'''
self.m = m
self.J = J
self.m_l = m_l
self.J_l = J_l
self.l1 = l1
self.l2 = l2
self.k_g_y = k_g
self.k_g_x = 2e3
self.b_g_x = 200
self.b_g = b_g
self.g = g
self.ground_height_function = ground_height_function
self.r0 = 1.5
# state machine for touchdown detection
self.xTD = sym.Variable('xTD')
self.was_in_contact = False
# Symbolic variables
# State variables are s = [x_ft, y_ft, theta, phi, r]
# self.x = [s, sdot]
# Inputs are self.u = [tau, chi]
self.x = np.array([sym.Variable('x_' + str(i)) for i in range(10)])
self.u = np.array([sym.Variable('u_' + str(i)) for i in range(2)])
# Initial state
self.initial_env = {}
for i, state in enumerate(initial_state):
self.initial_env[self.x[i]] = state
self.initial_env[self.xTD] = 0
self.k0 = 800
self.b_leg = 2
self.k0_stabilize = 40
self.b0_stabilize = 10
self.k0_restore = 60
self.b0_restore = 15
# self.flight_step_size = flight_step_size
# self.contact_step_size = contact_step_size
# self.descend_step_size_switch_threshold = descend_step_size_switch_threshold
# self.hover_step_size_switch_threshold=-0.75
# print(self.initial_env)
# Dynamic modes
Fx_contact = -self.k_g_x * (self.x[0] -
self.xTD) - self.b_g_x * self.x[5]
Fx_flight = 0.
Fy_contact = -self.k_g_y * (self.x[1] - self.ground_height_function(
self.x[0])) - self.b_g * self.x[6] * (1 - np.exp(self.x[1] * 16))
Fy_flight = 0.
R = self.x[4] - self.l1
# EOM is obtained from Russ Tedrake's Thesis
a1 = -self.m_l * R
a2 = (self.J_l - self.m_l * R * self.l1) * sym.cos(self.x[2])
b1 = self.m_l * R
b2 = (self.J_l - self.m_l * R * self.l1) * sym.sin(self.x[2])
c1 = self.m * R
c2 = (self.J_l + self.m * R * self.x[4]) * sym.cos(self.x[2])
c3 = self.m * R * self.l2 * sym.cos(self.x[3])
c4 = self.m * R * sym.sin(self.x[2])
d1 = -self.m * R
d2 = (self.J_l + self.m * R * self.x[4]) * sym.sin(self.x[2])
d3 = self.m * R * self.l2 * sym.sin(self.x[3])
d4 = -self.m * R * sym.cos(self.x[2])
e1 = self.J_l * self.l2 * sym.cos(self.x[2] - self.x[3])
e2 = -self.J * R
self.b_r_ascend = 0.
r_diff = self.x[4] - self.r0
F_leg_flight = -self.k0_restore * r_diff - self.b0_restore * self.x[9]
F_leg_ascend = -self.u[1] * r_diff - self.b_r_ascend * self.x[
9] #self.u[1] * (1-np.exp(10*r_diff_upper)/(np.exp(10*r_diff_upper)+1))#+(- self.k0_stabilize * r_diff_upper - self.b0_stabilize * self.x[9])*(np.exp(10*r_diff_upper)/(np.exp(10*r_diff_upper)+1))
F_leg_descend = -self.k0 * r_diff - self.b_leg * self.x[9]
# F_leg_descend = F_leg_ascend
self.tau_p = 400.
self.tau_d = 10.
hip_x_dot = self.x[5] + self.x[9] * sym.sin(
self.x[2]) + self.x[4] * sym.cos(self.x[2]) * self.x[7]
hip_y_dot = self.x[6] + self.x[9] * sym.cos(
self.x[2]) - self.x[4] * sym.sin(self.x[2]) * self.x[7]
alpha_des_ascend = 0.6 * sym.atan(hip_x_dot / (
-hip_y_dot - 1e-6)) #-sym.atan(self.x[5]/self.x[6]) # point toward
alpha_des_descend = 0.6 * sym.atan(
hip_x_dot / (hip_y_dot + 1e-6)) # point toward landing point
tau_leg_flight_ascend = (self.tau_p * (alpha_des_ascend - self.x[2]) -
self.tau_d * self.x[7]) * -1
tau_leg_flight_descend = (self.tau_p *
(alpha_des_descend - self.x[2]) -
self.tau_d * self.x[7]) * -1
tau_leg_contact = self.u[0]
def get_ddots(Fx, Fy, F_leg, u0):
alpha = (self.l1 * Fy * sym.sin(self.x[2]) -
self.l1 * Fx * sym.cos(self.x[2]) - u0)
A = sym.cos(self.x[2]) * alpha - R * (
Fx - F_leg * sym.sin(self.x[2]) -
self.m_l * self.l1 * self.x[7]**2 * sym.sin(self.x[2]))
B = sym.sin(self.x[2]) * alpha + R * (
self.m_l * self.l1 * self.x[7]**2 * sym.cos(self.x[2]) + Fy -
F_leg * sym.cos(self.x[2]) - self.m_l * self.g)
C = sym.cos(self.x[2]) * alpha + R * F_leg * sym.sin(
self.x[2]) + self.m * R * (
self.x[4] * self.x[7]**2 * sym.sin(self.x[2]) +
self.l2 * self.x[8]**2 * sym.sin(self.x[3]) -
2 * self.x[9] * self.x[7] * sym.cos(self.x[2]))
D = sym.sin(self.x[2]) * alpha - R * (
F_leg * sym.cos(self.x[2]) - self.m * self.g) - self.m * R * (
2 * self.x[9] * self.x[7] * sym.sin(self.x[2]) +
self.x[4] * self.x[7]**2 * sym.cos(self.x[2]) +
self.l2 * self.x[8]**2 * sym.cos(self.x[3]))
E = self.l2 * sym.cos(self.x[2] - self.x[3]) * alpha - R * (
self.l2 * F_leg * sym.sin(self.x[3] - self.x[2]) + u0)
return np.asarray([(A*b1*c2*d4*e2 - A*b1*c3*d4*e1 - A*b1*c4*d2*e2 + A*b1*c4*d3*e1 + A*b2*c4*d1*e2 - B*a2*c4*d1*e2 - C*a2*b1*d4*e2 + D*a2*b1*c4*e2 + E*a2*b1*c3*d4 - E*a2*b1*c4*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
(A*b2*c1*d4*e2 + B*a1*c2*d4*e2 - B*a1*c3*d4*e1 - B*a1*c4*d2*e2 + B*a1*c4*d3*e1 - B*a2*c1*d4*e2 - C*a1*b2*d4*e2 + D*a1*b2*c4*e2 + E*a1*b2*c3*d4 - E*a1*b2*c4*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
-(A*b1*c1*d4*e2 - B*a1*c4*d1*e2 - C*a1*b1*d4*e2 + D*a1*b1*c4*e2 + E*a1*b1*c3*d4 - E*a1*b1*c4*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
(A*b1*c1*d4*e1 - B*a1*c4*d1*e1 - C*a1*b1*d4*e1 + D*a1*b1*c4*e1 + E*a1*b1*c2*d4 - E*a1*b1*c4*d2 + E*a1*b2*c4*d1 - E*a2*b1*c1*d4)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
(A*b1*c1*d2*e2 - A*b1*c1*d3*e1 - A*b2*c1*d1*e2 - B*a1*c2*d1*e2 + B*a1*c3*d1*e1 + B*a2*c1*d1*e2 - C*a1*b1*d2*e2 + C*a1*b1*d3*e1 + C*a1*b2*d1*e2 + D*a1*b1*c2*e2 - D*a1*b1*c3*e1 - D*a2*b1*c1*e2 - E*a1*b1*c2*d3 + E*a1*b1*c3*d2 - E*a1*b2*c3*d1 + E*a2*b1*c1*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2)])
flight_ascend_dynamics = np.hstack(
(self.x[5:],
get_ddots(Fx_flight, Fy_flight, F_leg_flight,
tau_leg_flight_ascend)))
flight_descend_dynamics = np.hstack(
(self.x[5:],
get_ddots(Fx_flight, Fy_flight, F_leg_flight,
tau_leg_flight_descend)))
contact_descend_dynamics = np.hstack(
(self.x[5:],
get_ddots(Fx_contact, Fy_contact, F_leg_descend,
tau_leg_contact)))
contact_ascend_dynamics = np.hstack(
(self.x[5:],
get_ddots(Fx_contact, Fy_contact, F_leg_ascend, tau_leg_contact)))
flight_ascend_conditions = np.asarray([
self.x[1] > self.ground_height_function(self.x[0]), hip_y_dot > 0
])
flight_descend_conditions = np.asarray([
self.x[1] > self.ground_height_function(self.x[0]), hip_y_dot <= 0
])
contact_descend_coditions = np.asarray([
self.x[1] <= self.ground_height_function(self.x[0]), self.x[9] < 0
])
contact_ascend_coditions = np.asarray([
self.x[1] <= self.ground_height_function(self.x[0]), self.x[9] >= 0
])
self.f_list = np.asarray([
flight_ascend_dynamics, flight_descend_dynamics,
contact_descend_dynamics, contact_ascend_dynamics
])
self.f_type_list = np.asarray(
['continuous', 'continuous', 'continuous', 'continuous'])
self.c_list = np.asarray([
flight_ascend_conditions, flight_descend_conditions,
contact_descend_coditions, contact_ascend_coditions
])
DTHybridSystem.__init__(self, self.f_list, self.f_type_list, self.x, self.u, self.c_list, \
self.initial_env, input_limits=np.vstack([[-500,1.4e3], [500,2.5e3]]))
a=1 # The half of the length of the vertical side b=1 # The half the length of the horizonatl side # Dynamics: mysystem.C=np.zeros((3,3)) g=9.8 mysystem.tau_g=np.array([0,-g,0]) mysystem.B=np.zeros((3,0)) # Mass Matrix M=1 I=M/3.0*a**2 mysystem.M=blk(*[M,M,I]) mysystem.M_inv=np.linalg.inv(mysystem.M) # Contact Point 1: The ground with left corner phi_1= y - a * sym.cos(theta) - b * sym.sin(theta) psi_1= - (x + a * sym.sin(theta) - b * sym.cos(theta) ) J_1n=np.array([0,1,-b*sym.cos(theta)-a*sym.sin(theta)]).reshape(3,1) J_1t=np.array([1,0,a*sym.cos(theta)+b*sym.sin(theta)]).reshape(3,1) J_1=np.hstack((J_1n,J_1t)) C1=contact_point_symbolic_2D(mysystem,phi=phi_1,psi=psi_1,J=J_1,friction=0.9,name="contact point") #C1.sliding=False #C1.sticking=False C1.no_contact=False # Contact Point 2: The ground with right corner phi_2= y - a * sym.cos(theta) + b * sym.sin(theta) psi_2= - ( x + a * sym.sin(theta) + b * sym.cos(theta) ) J_2n=np.array([0,1,b*sym.cos(theta)-a*sym.sin(theta)]).reshape(3,1) J_2t=np.array([1,0,a*sym.cos(theta)-b*sym.sin(theta)]).reshape(3,1)
def cos(x):
if isinstance(x, AutoDiffXd):
return x.cos()
elif isinstance(x, Variable):
return sym.cos(x)
return math.cos(x)
def test_method_jacobian(self):
# (x * cos(y)).Jacobian([x, y]) returns [cos(y), -x * sin(y)].
J = (x * sym.cos(y)).Jacobian([x, y])
npc.assert_equal(J[0], sym.cos(y))
npc.assert_equal(J[1], -x * sym.sin(y))
def Rotate(t):
return np.array([[sym.cos(t), -sym.sin(t)], [sym.sin(t), sym.cos(t)]])
def dist_pole_ground(x):
pole_y = pole_length * sym.cos(x[1]) + cart_height
return pole_y
def test_method_jacobian(self):
# (x * cos(y)).Jacobian([x, y]) returns [cos(y), -x * sin(y)].
J = (x * sym.cos(y)).Jacobian([x, y])
self._check_scalar(J[0], sym.cos(y))
self._check_scalar(J[1], -x * sym.sin(y))
def test_method_jacobian(self):
# (x * cos(y)).Jacobian([x, y]) returns [cos(y), -x * sin(y)].
J = (x * sym.cos(y)).Jacobian([x, y])
self.assertEqual(J[0], sym.cos(y))
self.assertEqual(J[1], -x * sym.sin(y))
def __init__(self, M1=1., I1=1., M2=10., I2=10., r1=0.5, r2=0.4, KL=1e3, KL2=1e5, BL2=125, KG=1e4, BG=75, k0=1., \
g=9.8, ground_height_function=lambda x: 0, initial_state=np.asarray([0.,0.,0.,1.5,1.0,0.,0.,0.,0.,0.])):
'''
2D hopper with actuated piston at the end of the leg.
The model of the hopper follows the one described in "Hopping in Legged Systems" (Raibert, 1984)
'''
self.M1 = M1
self.I1 = I1
self.M2 = M2
self.I2 = I2
self.r1 = r1
self.r2 = r2
self.KL = KL
self.KL2 = KL2
self.BL2 = BL2
self.k0 = k0
self.KG = KG
self.BG = BG
self.g = g
self.ground_height_function = ground_height_function
# state machine for touchdown detection
self.xTD = sym.Variable('xTD')
self.was_in_contact = False
# Symbolic variables
# State variables are s = [theta1, theta2, x0, y0, w]
# self.x = [s, sdot]
# Inputs are self.u = [tau, chi]
self.x = np.array([sym.Variable('x_' + str(i)) for i in range(10)])
self.u = np.array([sym.Variable('u_' + str(i)) for i in range(2)])
# Initial state
self.initial_env = {}
for i, state in enumerate(initial_state):
self.initial_env[self.x[i]] = state
self.initial_env[self.xTD] = 0
# print(self.initial_env)
# Dynamic modes
FK_extended = self.KL * (self.k0 - self.x[4] + self.u[1])
FK_retracted = self.KL2 * (self.k0 - self.x[4] +
self.u[1]) - self.BL2 * self.x[9]
FX_contact = -self.KG * (
self.x[2] - self.xTD) - self.BG * self.x[7] #FIXME: handling xTD
FX_flight = 0.
FY_contact = -self.KG * (self.x[3] - self.ground_height_function(
self.x[2])) - self.BG * (self.x[8])
FY_flight = 0.
W = self.x[4] - self.r1
# EOM is obtained from Raibert 1984 paper, Appendix I eqn 40~44
# The EOM is in the following form
# a1*theta1_ddot + a2*theta2_ddot + a3*x0_ddot + a4*w_ddot = A
# b1*theta1_ddot + b2*theta2_ddot + b3*y0_ddot + b4*w_ddot = B
# c1*theta1_ddot + c2*x0_ddot = C
# d1*theta1_ddot + d2*y0_ddot = D
# e1*theta1_ddot + e2*theta2_ddot = E
a1 = sym.cos(self.x[0]) * (self.M2 * W * self.x[4] + self.I1)
a2 = self.M2 * self.r2 * W * sym.cos(self.x[1])
a3 = self.M2 * W
a4 = self.M2 * W * sym.sin(self.x[0])
b1 = -sym.sin(self.x[0]) * (self.M2 * W * self.x[4] + self.I1)
b2 = -self.M2 * self.r2 * W * sym.sin(self.x[1])
b3 = self.M2 * W
b4 = self.M2 * W * sym.cos(self.x[1])
c1 = sym.cos(self.x[0]) * (self.M1 * self.r1 * W - self.I1)
c2 = self.M1 * W
d1 = -sym.sin(self.x[0]) * (self.M1 * self.r1 * W - self.I1)
d2 = self.M1 * W
e1 = -sym.cos(self.x[1] - self.x[0]) * self.I1 * self.r2
e2 = self.I2 * self.x[4]
# free flight with extended leg
flight_extended_conditions = np.asarray([
self.k0 - self.x[4] + self.u[1] > 0,
self.x[3] - self.ground_height_function(self.x[2]) > 0
])
A_flight_extended = W * self.M2 * (self.x[5] ** 2 * W * sym.sin(self.x[0]) - 2 * self.x[5] * self.x[9] * sym.cos(self.x[0]) + \
self.r2 * self.x[6] ** 2 * sym.sin(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.sin(self.x[0])) - \
self.r1 * FX_flight * (sym.cos(self.x[0]) ** 2) + sym.cos(self.x[0]) * (self.r1 * FY_flight * sym.sin(self.x[0]) - self.u[0]) + \
FK_extended * W * sym.sin(self.x[0])
B_flight_extended = W * self.M2 * (self.x[5] ** 2 * W * sym.cos(self.x[0]) + 2 * self.x[5] * self.x[9] * sym.sin(self.x[0]) + \
self.r2 * self.x[6] ** 2 * sym.cos(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - g) + \
self.r1 * FX_flight * sym.cos(self.x[0]) * sym.sin(self.x[0]) - sym.sin(self.x[0]) * (self.r1 * FY_flight * sym.sin(self.x[0]) - self.u[0]) + \
FK_extended * W * sym.cos(self.x[0])
C_flight_extended = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.sin(self.x[0]) - FK_extended * sym.sin(self.x[0]) + FX_flight) - \
sym.cos(self.x[0]) * (FY_flight * self.r1 * sym.sin(self.x[0]) - FX_flight * self.r1 * sym.cos(self.x[0]) - self.u[0])
D_flight_extended = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - FK_extended * sym.cos(self.x[0]) + FY_flight - self.M1 * self.g) - \
sym.sin(self.x[0]) * (FY_flight * self.r1 * sym.sin(self.x[0]) - FX_flight * self.r1 * sym.cos(self.x[0]) - self.u[0])
E_flight_extended = W * (FK_extended * self.r2 * sym.sin(self.x[1] - self.x[0]) + self.u[0]) - self.r2 * sym.cos(self.x[1] - self.x[0]) * \
(self.r1 * FY_flight * sym.sin(self.x[0]) - self.r1 * FX_flight * sym.cos(self.x[0]) - self.u[0])
theta1_ddot_flight_extended = ((a4*b2/b4-a2)*E_flight_extended/e2-a3*C_flight_extended/c2+a4*b3*D_flight_extended/(b4*d2)+\
A_flight_extended-a4*B_flight_extended/b4)/\
(a1-b1*a4/b4-(a2-a4*b2/b4)*e1/e2-a3*c1/c2+a4*b3*d1/(b4*d2))
theta2_ddot_flight_extended = (E_flight_extended -
e1 * theta1_ddot_flight_extended) / e2
x_ddot_flight_extended = (C_flight_extended -
c1 * theta1_ddot_flight_extended) / c2
y_ddot_flight_extended = (D_flight_extended -
d1 * theta1_ddot_flight_extended) / d2
w_ddot_flight_extended = (A_flight_extended+B_flight_extended-(a1+b1)*theta1_ddot_flight_extended-(a2+b2)*theta2_ddot_flight_extended-\
a3*x_ddot_flight_extended-b3*y_ddot_flight_extended)/(a4+b4)
flight_extended_ddots = np.asarray([
theta1_ddot_flight_extended, theta2_ddot_flight_extended,
x_ddot_flight_extended, y_ddot_flight_extended,
w_ddot_flight_extended
])
flight_extended_dynamics = np.hstack(
(self.x[5:], flight_extended_ddots))
# free flight with retracted leg
flight_retracted_conditions = np.asarray([
self.k0 - self.x[4] + self.u[1] <= 0,
self.x[3] - self.ground_height_function(self.x[2]) > 0
])
A_flight_retracted = W * self.M2 * (self.x[5] ** 2 * W * sym.sin(self.x[0]) - 2 * self.x[5] * self.x[9] * sym.cos(self.x[0]) + \
self.r2 * self.x[6] ** 2 * sym.sin(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.sin(self.x[0])) - \
self.r1 * FX_flight * (sym.cos(self.x[0]) ** 2) + sym.cos(self.x[0]) * (self.r1 * FY_flight * sym.sin(self.x[0]) - self.u[0]) + \
FK_retracted * W * sym.sin(self.x[0])
B_flight_retracted = W * self.M2 * (self.x[5] ** 2 * W * sym.cos(self.x[0]) + 2 * self.x[5] * self.x[9] * sym.sin(self.x[0]) + \
self.r2 * self.x[6] ** 2 * sym.cos(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - g) + \
self.r1 * FX_flight * sym.cos(self.x[0]) * sym.sin(self.x[0]) - sym.sin(self.x[0]) * (self.r1 * FY_flight * sym.sin(self.x[0]) - self.u[0]) + \
FK_retracted * W * sym.cos(self.x[0])
C_flight_retracted = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.sin(self.x[0]) - FK_retracted * sym.sin(self.x[0]) + FX_flight) - \
sym.cos(self.x[0]) * (FY_flight * self.r1 * sym.sin(self.x[0]) - FX_flight * self.r1 * sym.cos(self.x[0]) - self.u[0])
D_flight_retracted = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - FK_retracted * sym.cos(self.x[0]) + FY_flight - self.M1 * self.g) - \
sym.sin(self.x[0]) * (FY_flight * self.r1 * sym.sin(self.x[0]) - FX_flight * self.r1 * sym.cos(self.x[0]) - self.u[0])
E_flight_retracted = W * (FK_retracted * self.r2 * sym.sin(self.x[1] - self.x[0]) + self.u[0]) - self.r2 * sym.cos(self.x[1] - self.x[0]) * \
(self.r1 * FY_flight * sym.sin(self.x[0]) - self.r1 * FX_flight * sym.cos(self.x[0]) - self.u[0])
theta1_ddot_flight_retracted = ((a4*b2/b4-a2)*E_flight_retracted/e2-a3*C_flight_retracted/c2+a4*b3*D_flight_retracted/(b4*d2)+\
A_flight_retracted-a4*B_flight_retracted/b4)/\
(a1-b1*a4/b4-(a2-a4*b2/b4)*e1/e2-a3*c1/c2+a4*b3*d1/(b4*d2))
theta2_ddot_flight_retracted = (E_flight_retracted -
e1 * theta1_ddot_flight_retracted) / e2
x_ddot_flight_retracted = (C_flight_retracted -
c1 * theta1_ddot_flight_retracted) / c2
y_ddot_flight_retracted = (D_flight_retracted -
d1 * theta1_ddot_flight_retracted) / d2
w_ddot_flight_retracted = (A_flight_retracted+B_flight_retracted-(a1+b1)*theta1_ddot_flight_retracted-(a2+b2)*theta2_ddot_flight_retracted-\
a3*x_ddot_flight_retracted-b3*y_ddot_flight_retracted)/(a4+b4)
flight_retracted_ddots = np.asarray([
theta1_ddot_flight_retracted, theta2_ddot_flight_retracted,
x_ddot_flight_retracted, y_ddot_flight_retracted,
w_ddot_flight_retracted
])
flight_retracted_dynamics = np.hstack(
(self.x[5:], flight_retracted_ddots))
# contact with extended leg
contact_extended_conditions = np.asarray([
self.k0 - self.x[4] + self.u[1] > 0,
self.x[3] - self.ground_height_function(self.x[2]) <= 0
])
A_contact_extended = W * self.M2 * (self.x[5] ** 2 * W * sym.sin(self.x[0]) - 2 * self.x[5] * self.x[9] * sym.cos(self.x[0]) + \
self.r2 * self.x[6] ** 2 * sym.sin(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.sin(self.x[0])) - \
self.r1 * FX_contact * (sym.cos(self.x[0]) ** 2) + sym.cos(self.x[0]) * (self.r1 * FY_contact * sym.sin(self.x[0]) - self.u[0]) + \
FK_extended * W * sym.sin(self.x[0])
B_contact_extended = W * self.M2 * (self.x[5] ** 2 * W * sym.cos(self.x[0]) + 2 * self.x[5] * self.x[9] * sym.sin(self.x[0]) + \
self.r2 * self.x[6] ** 2 * sym.cos(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - g) + \
self.r1 * FX_contact * sym.cos(self.x[0]) * sym.sin(self.x[0]) - sym.sin(self.x[0]) * (self.r1 * FY_contact * sym.sin(self.x[0]) - self.u[0]) + \
FK_extended * W * sym.cos(self.x[0])
C_contact_extended = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.sin(self.x[0]) - FK_extended * sym.sin(self.x[0]) + FX_contact) - \
sym.cos(self.x[0]) * (FY_contact * self.r1 * sym.sin(self.x[0]) - FX_contact * self.r1 * sym.cos(self.x[0]) - self.u[0])
D_contact_extended = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - FK_extended * sym.cos(self.x[0]) + FY_contact - self.M1 * self.g) - \
sym.sin(self.x[0]) * (FY_contact * self.r1 * sym.sin(self.x[0]) - FX_contact * self.r1 * sym.cos(self.x[0]) - self.u[0])
E_contact_extended = W * (FK_extended * self.r2 * sym.sin(self.x[1] - self.x[0]) + self.u[0]) - self.r2 * sym.cos(self.x[1] - self.x[0]) * \
(self.r1 * FY_contact * sym.sin(self.x[0]) - self.r1 * FX_contact * sym.cos(self.x[0]) - self.u[0])
theta1_ddot_contact_extended = ((a4*b2/b4-a2)*E_contact_extended/e2-a3*C_contact_extended/c2+a4*b3*D_contact_extended/(b4*d2)+\
A_contact_extended-a4*B_contact_extended/b4)/\
(a1-b1*a4/b4-(a2-a4*b2/b4)*e1/e2-a3*c1/c2+a4*b3*d1/(b4*d2))
theta2_ddot_contact_extended = (E_contact_extended -
e1 * theta1_ddot_contact_extended) / e2
x_ddot_contact_extended = (C_contact_extended -
c1 * theta1_ddot_contact_extended) / c2
y_ddot_contact_extended = (D_contact_extended -
d1 * theta1_ddot_contact_extended) / d2
w_ddot_contact_extended = (A_contact_extended+B_contact_extended-(a1+b1)*theta1_ddot_contact_extended-(a2+b2)*theta2_ddot_contact_extended-\
a3*x_ddot_contact_extended-b3*y_ddot_contact_extended)/(a4+b4)
contact_extended_ddots = np.asarray([
theta1_ddot_contact_extended, theta2_ddot_contact_extended,
x_ddot_contact_extended, y_ddot_contact_extended,
w_ddot_contact_extended
])
contact_extended_dynamics = np.hstack(
(self.x[5:], contact_extended_ddots))
# contact with retracted leg
contact_retracted_conditions = np.asarray([
self.k0 - self.x[4] + self.u[1] <= 0,
self.x[3] - self.ground_height_function(self.x[2]) <= 0
])
A_contact_retracted = W * self.M2 * (self.x[5] ** 2 * W * sym.sin(self.x[0]) - 2 * self.x[5] * self.x[9] * sym.cos(self.x[0]) + \
self.r2 * self.x[6] ** 2 * sym.sin(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.sin(self.x[0])) - \
self.r1 * FX_contact * (sym.cos(self.x[0]) ** 2) + sym.cos(self.x[0]) * (self.r1 * FY_contact * sym.sin(self.x[0]) - self.u[0]) + \
FK_retracted * W * sym.sin(self.x[0])
B_contact_retracted = W * self.M2 * (self.x[5] ** 2 * W * sym.cos(self.x[0]) + 2 * self.x[5] * self.x[9] * sym.sin(self.x[0]) + \
self.r2 * self.x[6] ** 2 * sym.cos(self.x[1]) + self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - g) + \
self.r1 * FX_contact * sym.cos(self.x[0]) * sym.sin(self.x[0]) - sym.sin(self.x[0]) * (self.r1 * FY_contact * sym.sin(self.x[0]) - self.u[0]) + \
FK_retracted * W * sym.cos(self.x[0])
C_contact_retracted = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.sin(self.x[0]) - FK_retracted * sym.sin(self.x[0]) + FX_contact) - \
sym.cos(self.x[0]) * (FY_contact * self.r1 * sym.sin(self.x[0]) - FX_contact * self.r1 * sym.cos(self.x[0]) - self.u[0])
D_contact_retracted = W * (self.M1 * self.r1 * self.x[5] ** 2 * sym.cos(self.x[0]) - FK_retracted * sym.cos(self.x[0]) + FY_contact - self.M1 * self.g) - \
sym.sin(self.x[0]) * (FY_contact * self.r1 * sym.sin(self.x[0]) - FX_contact * self.r1 * sym.cos(self.x[0]) - self.u[0])
E_contact_retracted = W * (FK_retracted * self.r2 * sym.sin(self.x[1] - self.x[0]) + self.u[0]) - self.r2 * sym.cos(self.x[1] - self.x[0]) * \
(self.r1 * FY_contact * sym.sin(self.x[0]) - self.r1 * FX_contact * sym.cos(self.x[0]) - self.u[0])
theta1_ddot_contact_retracted = ((a4*b2/b4-a2)*E_contact_retracted/e2-a3*C_contact_retracted/c2+a4*b3*D_contact_retracted/(b4*d2)+\
A_contact_retracted-a4*B_contact_retracted/b4)/\
(a1-b1*a4/b4-(a2-a4*b2/b4)*e1/e2-a3*c1/c2+a4*b3*d1/(b4*d2))
theta2_ddot_contact_retracted = (
E_contact_retracted - e1 * theta1_ddot_contact_retracted) / e2
x_ddot_contact_retracted = (C_contact_retracted -
c1 * theta1_ddot_contact_retracted) / c2
y_ddot_contact_retracted = (D_contact_retracted -
d1 * theta1_ddot_contact_retracted) / d2
w_ddot_contact_retracted = (A_contact_retracted+B_contact_retracted-(a1+b1)*theta1_ddot_contact_retracted-(a2+b2)*theta2_ddot_contact_retracted-\
a3*x_ddot_contact_retracted-b3*y_ddot_contact_retracted)/(a4+b4)
contact_retracted_ddots = np.asarray([
theta1_ddot_contact_retracted, theta2_ddot_contact_retracted,
x_ddot_contact_retracted, y_ddot_contact_retracted,
w_ddot_contact_retracted
])
contact_retracted_dynamics = np.hstack(
(self.x[5:], contact_retracted_ddots))
self.f_list = np.asarray([
flight_extended_dynamics, flight_retracted_dynamics,
contact_extended_dynamics, contact_retracted_dynamics
])
self.f_type_list = np.asarray(
['continuous', 'continuous', 'continuous', 'continuous'])
self.c_list = np.asarray([
flight_extended_conditions, flight_retracted_conditions,
contact_extended_conditions, contact_retracted_conditions
])
DTHybridSystem.__init__(self, self.f_list, self.f_type_list, self.x, self.u, self.c_list, \
self.initial_env)
mysystem.q_o = np.array([x, y, theta])
mysystem.q_m = np.array([x_1, y_1, x_2, y_2])
# Dynamics:
mysystem.C = np.zeros((3, 3))
g = 9.8
mysystem.tau_g = np.array([0, -g, 0])
mysystem.B = np.zeros((3, 0))
M = 1
I = M / 3.0
mysystem.M = blk(*[M, M, I])
mysystem.M_inv = np.linalg.inv(mysystem.M)
h = 0.05
psi_1 = (x_1 - x) * sym.cos(theta) + (y_1 - y) * sym.sin(theta)
phi_1 = -((x_1 - x) * sym.sin(-theta) + (y_1 - y) * sym.cos(theta))
J_1n = np.array([-sym.sin(theta), sym.cos(theta), psi_1]).reshape(3, 1)
J_1t = np.array([sym.cos(theta), sym.sin(theta), 0]).reshape(3, 1)
J_1 = np.hstack((J_1n, J_1t))
C1 = contact_point_symbolic_2D(mysystem,
phi=phi_1,
psi=psi_1,
J=J_1,
friction=0.3,
name="contact point")
#C1.sliding=False
#C1.no_contact=False
psi_2 = (x_2 - x) * sym.cos(theta) + (y_2 - y) * sym.sin(theta)
phi_2 = -((x_2 - x) * sym.sin(-theta) + (y_2 - y) * sym.cos(theta))
def test_method_jacobian(self):
# (x * cos(y)).Jacobian([x, y]) returns [cos(y), -x * sin(y)].
J = (x * sym.cos(y)).Jacobian([x, y])
numpy_compare.assert_equal(J[0], sym.cos(y))
numpy_compare.assert_equal(J[1], -x * sym.sin(y))
p = 4 * R / (3 * np.pi)
# Dynamics:
mysystem.C = np.zeros((3, 3))
g = 9.8
mysystem.tau_g = np.array([0, -g, 0])
mysystem.B = np.zeros((3, 0))
# Mass Matrix
M = 1
I = M * R**2 * (1 / 2.0 - 16 / (9 * np.pi**2))
h = 0.05
mysystem.M = blk(*[M, M, I])
mysystem.M_inv = np.linalg.inv(mysystem.M)
x_c = x - p * sym.sin(theta)
y_c = y + p * sym.cos(theta)
x_R = x_c + R * sym.sin(theta_m)
y_R = y_c - R * sym.cos(theta_m)
"""
Ground
"""
phi_1 = y_c - R
psi_1 = -R * theta - x_c
J_1n = np.array([0, 1, -p * sym.sin(theta)]).reshape(3, 1)
J_1t = np.array([1, 0, R - p * sym.cos(theta)]).reshape(3, 1)
J_1 = np.hstack((J_1n, J_1t))
C1 = contact_point_symbolic_2D(mysystem,
phi=phi_1,
psi=psi_1,
J=J_1,
def test_method_jacobian(self):
# (x * cos(y)).Jacobian([x, y]) returns [cos(y), -x * sin(y)].
J = (x * sym.cos(y)).Jacobian([x, y])
self._check_scalar(J[0], sym.cos(y))
self._check_scalar(J[1], -x * sym.sin(y))
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
"""
Created on Wed Mar 6 19:09:04 2019
@author: sadra
"""
import numpy as np
import pydrake.symbolic as sym
from PWA_lib.Manipulation.contact_point_pydrake import contact_point_symbolic
from PWA_lib.Manipulation.system_symbolic_pydrake import system_symbolic
mysystem = system_symbolic("my system")
q = np.array([sym.Variable("q%i" % i) for i in range(3)])
J = np.array([q[0]**2 * sym.sin(q[2]), q[1]**3 * sym.cos(q[2])])
z = sym.Jacobian(J, q)
E = {q[i]: i + 1 for i in range(3)}
z_v = sym.Evaluate(z, E)