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_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,
#!/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)