def axis_angle_from_matrix(rotation_matrix): """ :param rotation_matrix: 4x4 or 3x3 Matrix :type rotation_matrix: Matrix :return: 3x1 Matrix, angle :rtype: (Matrix, Union[float, Symbol]) """ rm = rotation_matrix angle = (trace(rm[:3, :3]) - 1) / 2 angle = Min(angle, 1) angle = Max(angle, -1) angle = acos(angle) x = (rm[2, 1] - rm[1, 2]) y = (rm[0, 2] - rm[2, 0]) z = (rm[1, 0] - rm[0, 1]) n = sqrt(x * x + y * y + z * z) m = if_eq_zero(n, 1, n) axis = Matrix([ if_eq_zero(n, 0, x / m), if_eq_zero(n, 0, y / m), if_eq_zero(n, 1, z / m) ]) sign = if_eq_zero(angle, 1, diffable_sign(angle)) axis *= sign angle = sign * angle return axis, angle
def asTwist(self): acosarg = (casadi.trace(self.R) - 1) / 2 theta = casadi.acos(casadi.if_else(casadi.fabs(acosarg) >= 1., 1., acosarg)) w = casadi.horzcat((self.R[2, 1] - self.R[1, 2]), (self.R[0, 2] - self.R[2, 0]), (self.R[1, 0] - self.R[0, 1])) return casadi.horzcat(casadi.reshape(self.t, 1, 3), theta * w / casadi.norm_2(w))
def quaternion_slerp(q1, q2, t): """ spherical linear interpolation that takes into account that q == -q :param q1: 4x1 Matrix :type q1: Matrix :param q2: 4x1 Matrix :type q2: Matrix :param t: float, 0-1 :type t: Union[float, Symbol] :return: 4x1 Matrix; Return spherical linear interpolation between two quaternions. :rtype: Matrix """ cos_half_theta = dot(q1.T, q2) if0 = -cos_half_theta q2 = if_greater_zero(if0, -q2, q2) cos_half_theta = if_greater_zero(if0, -cos_half_theta, cos_half_theta) if1 = Abs(cos_half_theta) - 1.0 # enforce acos(x) with -1 < x < 1 cos_half_theta = Min(1, cos_half_theta) cos_half_theta = Max(-1, cos_half_theta) half_theta = acos(cos_half_theta) sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta) if2 = 0.001 - Abs(sin_half_theta) ratio_a = save_division(sin((1.0 - t) * half_theta), sin_half_theta) ratio_b = save_division(sin(t * half_theta), sin_half_theta) return if_greater_eq_zero( if1, Matrix(q1), if_greater_zero(if2, 0.5 * q1 + 0.5 * q2, ratio_a * q1 + ratio_b * q2))
def axis_angle_from_matrix(rotation_matrix): """ MAKE SURE MATRIX IS NORMALIZED :param rotation_matrix: 4x4 or 3x3 Matrix :type rotation_matrix: Matrix :return: 3x1 Matrix, angle :rtype: (Matrix, Union[float, Symbol]) """ q = quaternion_from_matrix(rotation_matrix) return axis_angle_from_quaternion(q[0], q[1], q[2], q[3]) # TODO use 'if' to make angle always positive? rm = rotation_matrix cos_angle = (trace(rm[:3, :3]) - 1) / 2 cos_angle = Min(cos_angle, 1) cos_angle = Max(cos_angle, -1) angle = acos(cos_angle) x = (rm[2, 1] - rm[1, 2]) y = (rm[0, 2] - rm[2, 0]) z = (rm[1, 0] - rm[0, 1]) n = sqrt(x * x + y * y + z * z) axis = Matrix([ if_eq(Abs(cos_angle), 1, 0, x / n), if_eq(Abs(cos_angle), 1, 0, y / n), if_eq(Abs(cos_angle), 1, 1, z / n) ]) return axis, angle
def diffable_axis_angle_from_matrix(rotation_matrix): """ MAKE SURE MATRIX IS NORMALIZED :param rotation_matrix: 4x4 or 3x3 Matrix :type rotation_matrix: Matrix :return: 3x1 Matrix, angle :rtype: (Matrix, Union[float, Symbol]) """ # TODO nan if angle 0 # TODO buggy when angle == pi # TODO use 'if' to make angle always positive? rm = rotation_matrix cos_angle = (trace(rm[:3, :3]) - 1) / 2 cos_angle = diffable_min_fast(cos_angle, 1) cos_angle = diffable_max_fast(cos_angle, -1) angle = acos(cos_angle) x = (rm[2, 1] - rm[1, 2]) y = (rm[0, 2] - rm[2, 0]) z = (rm[1, 0] - rm[0, 1]) n = sqrt(x * x + y * y + z * z) axis = Matrix([ if_eq(Abs(cos_angle), 1, 0, x / n), if_eq(Abs(cos_angle), 1, 0, y / n), if_eq(Abs(cos_angle), 1, 1, z / n) ]) return axis, angle
def get_angle_casadi(point1, point2, point3): v12 = vector3(*point1) - vector3(*point2) v13 = vector3(*point1) - vector3(*point3) v23 = vector3(*point2) - vector3(*point3) d12 = norm(v12) d13 = norm(v13) d23 = norm(v23) #return w.acos(w.dot(v12, v13.T)[0] / (d12 * d13)) return acos((d12**2 + d13**2 - d23**2) / (2 * d12 * d13))
def terminal_constraints3(x, t, x0, t0): # https://space.stackexchange.com/questions/1904/how-to-programmatically-calculate-orbital-elements-using-position-velocity-vecto # http://control.asu.edu/Classes/MAE462/462Lecture06.pdf h = ca.vertcat(x[1] * x[5] - x[4] * x[2], x[3] * x[2] - x[0] * x[5], x[0] * x[4] - x[1] * x[3]) n = ca.vertcat(-h[1], h[0], 0) r = ca.sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]) e = ca.vertcat( 1 / muE * (x[4] * h[2] - x[5] * h[1]) - x[0] / r, 1 / muE * (x[5] * h[0] - x[3] * h[2]) - x[1] / r, 1 / muE * (x[3] * h[1] - x[4] * h[0]) - x[2] / r, ) e_mag = ca.sqrt(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]) h_sq = h[0] * h[0] + h[1] * h[1] + h[2] * h[2] v_mag = ca.sqrt(x[3] * x[3] + x[4] * x[4] + x[5] * x[5]) a = -muE / (v_mag * v_mag - 2.0 * muE / r) i = ca.acos(h[2] / ca.sqrt(h_sq)) n_mag = ca.sqrt(n[0] * n[0] + n[1] * n[1]) node_asc = ca.acos(n[0] / n_mag) # if n[1] < -1e-12: node_asc = 2 * np.pi - node_asc argP = ca.acos((n[0] * e[0] + n[1] * e[1]) / (n_mag * e_mag)) # if e[2] < 0: # argP = 2*np.pi - argP a_req = 24361140.0 e_req = 0.7308 i_req = 28.5 * np.pi / 180.0 node_asc_req = 269.8 * np.pi / 180.0 argP_req = 130.5 * np.pi / 180.0 return [ (a - a_req) / (Re), e_mag - e_req, i - i_req, node_asc - node_asc_req, argP - argP_req, ]
def slerp(v1, v2, t): """ spherical linear interpolation :param v1: any vector :param v2: vector of same length as v1 :param t: value between 0 and 1. 0 is v1 and 1 is v2 :return: """ angle = acos(dot(v1.T, v2)[0]) return (sin((1-t)*angle)/sin(angle))*v1 + (sin(t*angle)/sin(angle))*v2
def log(cls, q): assert q.shape == (4, 1) or q.shape == (4, ) v = ca.SX(3, 1) norm_q = ca.norm_2(q) theta = 2 * ca.acos(q[0]) c = ca.sin(theta / 2) v[0] = theta * q[1] / c v[1] = theta * q[2] / c v[2] = theta * q[3] / c return ca.if_else(ca.fabs(c) > eps, v, ca.SX([0, 0, 0]))
def rotation_distance(a_R_b, a_R_c): """ :param a_R_b: 4x4 or 3x3 Matrix :type a_R_b: Matrix :param a_R_c: 4x4 or 3x3 Matrix :type a_R_c: Matrix :return: angle of axis angle representation of b_R_c :rtype: Union[float, Symbol] """ difference = dot(a_R_b.T, a_R_c) angle = (trace(difference[:3, :3]) - 1) / 2 angle = Min(angle, 1) angle = Max(angle, -1) return acos(angle)
def axis_angle_from_quaternion(x, y, z, w): """ :type x: Union[float, Symbol] :type y: Union[float, Symbol] :type z: Union[float, Symbol] :type w: Union[float, Symbol] :return: 4x1 Matrix :rtype: Matrix """ l = norm(Matrix([x, y, z, w])) x, y, z, w = x / l, y / l, z / l, w / l w2 = sqrt(1 - w**2) angle = (2 * acos(Min(Max(-1, w), 1))) m = if_eq_zero(w2, 1, w2) # avoid /0 x = if_eq_zero(w2, 0, x / m) y = if_eq_zero(w2, 0, y / m) z = if_eq_zero(w2, 1, z / m) return Matrix([x, y, z]), angle
def diffable_axis_angle_from_matrix_stable(rotation_matrix): """ :param rotation_matrix: 4x4 or 3x3 Matrix :type rotation_matrix: Matrix :return: 3x1 Matrix, angle :rtype: (Matrix, Union[float, Symbol]) """ # TODO buggy when angle == pi rm = rotation_matrix angle = (trace(rm[:3, :3]) - 1) / 2 angle = diffable_min_fast(angle, 1) angle = diffable_max_fast(angle, -1) angle = acos(angle) x = (rm[2, 1] - rm[1, 2]) y = (rm[0, 2] - rm[2, 0]) z = (rm[1, 0] - rm[0, 1]) n = sqrt(x * x + y * y + z * z) m = diffable_if_eq_zero(n, 1, n) axis = Matrix([ diffable_if_eq_zero(n, 0, x / m), diffable_if_eq_zero(n, 0, y / m), diffable_if_eq_zero(n, 1, z / m) ]) return axis, angle
def diffable_slerp(q1, q2, t): """ !takes a long time to compile! :param q1: 4x1 Matrix :type q1: Matrix :param q2: 4x1 Matrix :type q2: Matrix :param t: float, 0-1 :type t: Union[float, Symbol] :return: 4x1 Matrix; Return spherical linear interpolation between two quaternions. :rtype: Matrix """ cos_half_theta = dot(q1.T, q2) if0 = -cos_half_theta q2 = diffable_if_greater_zero(if0, -q2, q2) cos_half_theta = diffable_if_greater_zero(if0, -cos_half_theta, cos_half_theta) if1 = diffable_abs(cos_half_theta) - 1.0 # enforce acos(x) with -1 < x < 1 cos_half_theta = diffable_min_fast(1, cos_half_theta) cos_half_theta = diffable_max_fast(-1, cos_half_theta) half_theta = acos(cos_half_theta) sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta) if2 = 0.001 - diffable_abs(sin_half_theta) ratio_a = save_division(sin((1.0 - t) * half_theta), sin_half_theta) ratio_b = save_division(sin(t * half_theta), sin_half_theta) return diffable_if_greater_eq_zero( if1, Matrix(q1), diffable_if_greater_zero(if2, 0.5 * q1 + 0.5 * q2, ratio_a * q1 + ratio_b * q2))
def create_rocket_stage_phase(mocp, phase_name, p, **kwargs): is_powered = kwargs['is_powered'] has_heating = kwargs['has_heating'] drag_area = kwargs['drag_area'] maxQ = kwargs['maxQ'] init_mass = kwargs['init_mass'] init_position = kwargs['init_position'] init_velocity = kwargs['init_velocity'] if is_powered: init_steering = kwargs['init_steering'] engine_type = kwargs['engine_type'] engine_count = kwargs['engine_count'] mdot_max = kwargs['mdot_max'] mdot_min = kwargs['mdot_min'] if engine_type == 'vacuum': effective_exhaust_velocity = kwargs['effective_exhaust_velocity'] else: exhaust_velocity = kwargs['exhaust_velocity'] exit_area = kwargs['exit_area'] exit_pressure = kwargs['exit_pressure'] duration = mocp.create_phase(phase_name, init=0.001, n_intervals=2) # Total vessel mass mass = mocp.add_trajectory(phase_name, 'mass', init=init_mass) # ECEF position vector rx = mocp.add_trajectory(phase_name, 'rx', init=init_position[0]) ry = mocp.add_trajectory(phase_name, 'ry', init=init_position[1]) rz = mocp.add_trajectory(phase_name, 'rz', init=init_position[2]) r_vec = casadi.vertcat(rx, ry, rz) # ECEF velocity vector vx = mocp.add_trajectory(phase_name, 'vx', init=init_velocity[0]) vy = mocp.add_trajectory(phase_name, 'vy', init=init_velocity[1]) vz = mocp.add_trajectory(phase_name, 'vz', init=init_velocity[2]) v_vec = casadi.vertcat(vx, vy, vz) if is_powered: # ECEF steering unit vector ux = mocp.add_trajectory(phase_name, 'ux', init=init_steering[0]) uy = mocp.add_trajectory(phase_name, 'uy', init=init_steering[1]) uz = mocp.add_trajectory(phase_name, 'uz', init=init_steering[2]) u_vec = casadi.vertcat(ux, uy, uz) # Engine mass flow for a SINGLE engine mdot = mocp.add_trajectory(phase_name, 'mdot', init=mdot_max) mdot_rate = mocp.add_trajectory(phase_name, 'mdot_rate', init=0.0) if has_heating: heat = mocp.add_trajectory(phase_name, 'heat', init=0.0) ## Dynamics r_squared = rx**2 + ry**2 + rz**2 + 1e-8 v_squared = vx**2 + vy**2 + vz**2 + 1e-8 airspeed = v_squared**0.5 r = r_squared**0.5 altitude = r - 1 r3_inv = r_squared**(-1.5) mocp.add_path_output( 'downrange_distance', casadi.asin( casadi.norm_2( casadi.cross( r_vec / r, casadi.SX( [p.launch_site_x, p.launch_site_y, p.launch_site_z]))))) mocp.add_path_output('altitude', altitude) mocp.add_path_output('airspeed', airspeed) mocp.add_path_output('vertical_speed', (r_vec.T / r) @ v_vec) mocp.add_path_output( 'horizontal_speed_ECEF', casadi.norm_2(v_vec - ((r_vec.T / r) @ v_vec) * (r_vec / r))) # Gravitational acceleration (mu=1 is omitted) gx = -r3_inv * rx gy = -r3_inv * ry gz = -r3_inv * rz # Atmosphere atmosphere_fraction = casadi.exp(-altitude / p.scale_height) air_pressure = p.air_pressure_MSL * atmosphere_fraction air_density = p.air_density_MSL * atmosphere_fraction dynamic_pressure = 0.5 * air_density * v_squared mocp.add_path_output('dynamic_pressure', dynamic_pressure) if is_powered: lateral_airspeed = casadi.sqrt( casadi.sumsqr(v_vec - ((v_vec.T @ u_vec) * u_vec)) + 1e-8) lateral_dynamic_pressure = 0.5 * air_density * lateral_airspeed * airspeed # Same as Q * sin(alpha), but without trigonometry mocp.add_path_output('lateral_dynamic_pressure', lateral_dynamic_pressure) mocp.add_path_output( 'alpha', casadi.acos((v_vec.T @ u_vec) / airspeed) * 180.0 / pi) mocp.add_path_output( 'pitch', 90.0 - casadi.acos((r_vec.T / r) @ u_vec) * 180.0 / pi) # Thrust force if is_powered: if engine_type == 'vacuum': engine_thrust = effective_exhaust_velocity * mdot else: engine_thrust = exhaust_velocity * mdot + exit_area * ( exit_pressure - air_pressure) total_thrust = engine_thrust * engine_count mocp.add_path_output('total_thrust', total_thrust) Tx = total_thrust * ux Ty = total_thrust * uy Tz = total_thrust * uz else: Tx = 0.0 Ty = 0.0 Tz = 0.0 # Drag force drag_factor = (-0.5 * drag_area) * air_density * airspeed Dx = drag_factor * vx Dy = drag_factor * vy Dz = drag_factor * vz # Coriolis Acceleration ax_Coriolis = 2 * vy * p.body_rotation_speed ay_Coriolis = -2 * vx * p.body_rotation_speed az_Coriolis = 0.0 # Centrifugal Acceleration ax_Centrifugal = rx * p.body_rotation_speed**2 ay_Centrifugal = ry * p.body_rotation_speed**2 az_Centrifugal = 0.0 # Acceleration ax = gx + ax_Coriolis + ax_Centrifugal + (Dx + Tx) / mass ay = gy + ay_Coriolis + ay_Centrifugal + (Dy + Ty) / mass az = gz + az_Coriolis + az_Centrifugal + (Dz + Tz) / mass if is_powered: mocp.set_derivative(mass, -engine_count * mdot) mocp.set_derivative(mdot, mdot_rate) else: mocp.set_derivative(mass, 0.0) mocp.set_derivative(rx, vx) mocp.set_derivative(ry, vy) mocp.set_derivative(rz, vz) mocp.set_derivative(vx, ax) mocp.set_derivative(vy, ay) mocp.set_derivative(vz, az) # Heat flow heat_flow = 1e-4 * (air_density**0.5) * (airspeed**3.15) mocp.add_path_output('heat_flow', heat_flow) if has_heating: mocp.set_derivative(heat, heat_flow) ## Constraints # Duration is positive and bounded mocp.add_constraint(duration > 0.006) mocp.add_constraint(duration < 10.0) # Mass is positive mocp.add_path_constraint(mass > 1e-6) # maxQ soft constraint weight_dynamic_pressure_penalty = mocp.get_parameter( 'weight_dynamic_pressure_penalty') slack_maxQ = mocp.add_trajectory(phase_name, 'slack_maxQ', init=10.0) mocp.add_path_constraint(slack_maxQ > 0.0) mocp.add_mean_objective(weight_dynamic_pressure_penalty * slack_maxQ) mocp.add_path_constraint(dynamic_pressure / maxQ < 1.0 + slack_maxQ) if is_powered: # u is a unit vector mocp.add_path_constraint(ux**2 + uy**2 + uz**2 == 1.0) # Do not point down mocp.add_path_constraint((r_vec / r).T @ u_vec > -0.2) # Engine flow limits mocp.add_path_constraint(mdot_min < mdot) mocp.add_path_constraint(mdot < mdot_max) # Throttle rate reduction mocp.add_mean_objective(1e-6 * mdot_rate**2) # Lateral dynamic pressure penalty weight_lateral_dynamic_pressure_penalty = mocp.get_parameter( 'weight_lateral_dynamic_pressure_penalty') mocp.add_mean_objective( weight_lateral_dynamic_pressure_penalty * (lateral_dynamic_pressure / p.maxQ_sin_alpha)**8) variables = locals().copy() RocketStagePhase = collections.namedtuple('RocketStagePhase', sorted(list(variables.keys()))) return RocketStagePhase(**variables)
def rotationAngle(mx_R): acosarg = (casadi.trace(mx_R) - 1.) / 2. return casadi.if_else(casadi.fabs(acosarg) >= 1., 0., casadi.acos(acosarg))
def create_rocket_stage_phase(mocp, phase_name, p, **kwargs): engine_parameters = kwargs['engine_parameters'] atmosphere_parameters = kwargs['atmosphere_parameters'] init_mass = kwargs['init_mass'] has_steering_unit_vector_constraint = kwargs.get( 'has_steering_unit_vector_constraint', True) if init_mass is None: assert atmosphere_parameters is None # Must have mass for drag-acceleration assert engine_parameters is None # Must have mass for thrust-acceleration if phase_name == 'target': init_position = [ p.target_position_x, p.target_position_y, p.target_position_z ] init_velocity = [ p.target_velocity_x, p.target_velocity_y, p.target_velocity_z ] else: init_position = [p.launch_site_x, p.launch_site_y, p.launch_site_z] init_velocity = [0.0, 0.0, 0.0] duration = mocp.create_phase(phase_name, init=0.001, n_intervals=3) if init_mass is not None: # Total vessel mass mass = mocp.add_trajectory(phase_name, 'mass', init=init_mass) # ECEF position vector rx = mocp.add_trajectory(phase_name, 'rx', init=init_position[0]) ry = mocp.add_trajectory(phase_name, 'ry', init=init_position[1]) rz = mocp.add_trajectory(phase_name, 'rz', init=init_position[2]) r_vec = casadi.vertcat(rx, ry, rz) mocp.add_path_constraint(sumsqr(r_vec) > 0.99) # ECEF velocity vector vx = mocp.add_trajectory(phase_name, 'vx', init=init_velocity[0]) vy = mocp.add_trajectory(phase_name, 'vy', init=init_velocity[1]) vz = mocp.add_trajectory(phase_name, 'vz', init=init_velocity[2]) v_vec = casadi.vertcat(vx, vy, vz) if engine_parameters is not None: launch_site_param = DM( [p.launch_site_x, p.launch_site_y, p.launch_site_z]) init_up_direction = normalize(launch_site_param) # ECEF steering unit vector ux = mocp.add_trajectory(phase_name, 'ux', init=init_up_direction[0]) uy = mocp.add_trajectory(phase_name, 'uy', init=init_up_direction[1]) uz = mocp.add_trajectory(phase_name, 'uz', init=init_up_direction[2]) u_vec = casadi.vertcat(ux, uy, uz) if has_steering_unit_vector_constraint: # u is a unit vector mocp.add_constraint(sumsqr(mocp.start(u_vec)) == 1.0) omega_x = mocp.add_trajectory(phase_name, 'omega_x', init=0.0) omega_y = mocp.add_trajectory(phase_name, 'omega_y', init=0.0) omega_z = mocp.add_trajectory(phase_name, 'omega_z', init=0.0) omega_vec = casadi.vertcat(omega_x, omega_y, omega_z) alpha_x = mocp.add_trajectory(phase_name, 'alpha_x', init=0.0) alpha_y = mocp.add_trajectory(phase_name, 'alpha_y', init=0.0) alpha_z = mocp.add_trajectory(phase_name, 'alpha_z', init=0.0) alpha_vec = casadi.vertcat(alpha_x, alpha_y, alpha_z) du_dt = casadi.cross(omega_vec, u_vec) + u_vec * 10 * (1.0 - sumsqr(u_vec)) mocp.set_derivative(ux, du_dt[0]) mocp.set_derivative(uy, du_dt[1]) mocp.set_derivative(uz, du_dt[2]) mocp.set_derivative(omega_x, alpha_x) mocp.set_derivative(omega_y, alpha_y) mocp.set_derivative(omega_z, alpha_z) mocp.add_path_constraint(dot(u_vec, omega_vec) == 1e-20) mocp.add_mean_objective(1e-6 * sumsqr(alpha_vec)) mocp.add_mean_objective(1e-6 * sumsqr(omega_vec)) mocp.add_path_output('u_mag_err', sumsqr(u_vec) - 1) throttle = mocp.add_trajectory(phase_name, 'throttle', init=1.0) throttle_rate = mocp.add_trajectory(phase_name, 'throttle_rate', init=0.0) r_squared = sumsqr(r_vec) + 1e-8 v_squared = sumsqr(v_vec) + 1e-8 airspeed = v_squared**0.5 orbital_radius = r_squared**0.5 up_direction = r_vec / orbital_radius altitude = orbital_radius - 1.0 planet_angular_velocity = casadi.DM( [0.0, -p.body_angular_rate, 0.0] ) # KSP coordinates: planet angular velocity points in negative y direction a_gravity = -(r_squared**( -1.5)) * r_vec # Gravitational acceleration (mu=1 is omitted) a_coriolis = -2 * cross(planet_angular_velocity, v_vec) # Coriolis acceleration a_centrifugal = -cross(planet_angular_velocity, cross(planet_angular_velocity, r_vec)) # Centrifugal acceleration a_vec = a_gravity + a_coriolis + a_centrifugal # Total acceleration on vehicle # Atmosphere if atmosphere_parameters is not None: atmosphere_fraction = casadi.exp(-altitude / atmosphere_parameters.scale_height) air_density = atmosphere_parameters.air_density_MSL * atmosphere_fraction dynamic_pressure = 0.5 * air_density * v_squared mocp.add_path_output('dynamic_pressure', dynamic_pressure) # Drag force drag_force_vector = (-0.5 * atmosphere_parameters.drag_area * atmosphere_parameters.air_density_MSL ) * atmosphere_fraction * airspeed * v_vec a_vec += (drag_force_vector / mass) # Engine thrust if engine_parameters is not None: if atmosphere_parameters is None: exhaust_velocity = engine_parameters.exhaust_velocity_vacuum else: exhaust_velocity = engine_parameters.exhaust_velocity_vacuum + \ atmosphere_fraction * (engine_parameters.exhaust_velocity_sea_level - engine_parameters.exhaust_velocity_vacuum) engine_mass_flow = engine_parameters.max_mass_flow * throttle thrust = exhaust_velocity * engine_mass_flow mocp.add_path_output('thrust', thrust) thrust_acceleration = thrust / mass mocp.add_path_output('thrust_acceleration', thrust_acceleration) thrust_force_vector = thrust * u_vec a_vec += (thrust_force_vector / mass) ## Differential equations if engine_parameters is not None: mocp.set_derivative(mass, -engine_mass_flow) mocp.set_derivative(throttle, throttle_rate) elif init_mass is not None: mocp.set_derivative(mass, 0.0) # Atmospheric coast: mass is constant for i in range(3): mocp.set_derivative(r_vec[i], v_vec[i]) mocp.set_derivative(v_vec[i], a_vec[i]) ## Constraints # Duration is positive and bounded mocp.add_constraint(duration > 0.004) if phase_name == 'target': mocp.add_constraint(duration < 20.0) else: mocp.add_constraint(duration < 10.0) # Mass is positive if init_mass is not None: mocp.add_path_constraint(mass > 1e-6) if engine_parameters is not None: # Do not point down mocp.add_path_constraint(dot(up_direction, u_vec) > -0.2) # Throttle limits mocp.add_path_constraint(throttle < 0.90) mocp.add_path_constraint(throttle > 0.05) # Throttle rate reduction mocp.add_mean_objective(1e-5 * throttle_rate**2) # Q alpha constraint if (atmosphere_parameters is not None) and (engine_parameters is not None): lateral_airspeed = casadi.sqrt( casadi.sumsqr(v_vec - (dot(v_vec, u_vec) * u_vec)) + 1e-8) lateral_dynamic_pressure = 0.5 * air_density * lateral_airspeed * airspeed # Same as Q * sin(alpha), but without trigonometry mocp.add_path_output('lateral_dynamic_pressure', lateral_dynamic_pressure) mocp.add_mean_objective( (lateral_dynamic_pressure / atmosphere_parameters.lateral_dynamic_pressure_limit)**8) ## Outputs mocp.add_path_output('altitude', altitude) mocp.add_path_output('airspeed', airspeed) mocp.add_path_output('vertical_speed', dot(up_direction, v_vec)) mocp.add_path_output( 'horizontal_speed_ECEF', casadi.norm_2(v_vec - (dot(up_direction, v_vec) * (up_direction)))) if engine_parameters is not None: mocp.add_path_output( 'AoA', casadi.acos(dot(v_vec, u_vec) / airspeed) * 180.0 / pi) mocp.add_path_output( 'pitch', 90.0 - casadi.acos(dot(up_direction, u_vec)) * 180.0 / pi) variables = locals().copy() RocketStagePhase = collections.namedtuple('RocketStagePhase', sorted(list(variables.keys()))) return RocketStagePhase(**variables)
def angle_between_vector(v1, v2): v1 = v1[:3] v2 = v2[:3] return acos(dot(v1.T, v2) / (norm(v1) * norm(v2)))
def __setPhysics__(self): # Generalized Coordinates, q = [x_B, z_B, theta_H, theta_K] q = ca.SX.sym('q', self.n_generalized, 1) dq = ca.SX.sym('dq', self.n_generalized, 1) # ddq = ca.SX.sym('ddq', self.n_generalized, 1) # Inputs and external force, u = [u_H, u_K] u = ca.SX.sym('u', self.dof, 1) lam = ca.SX.sym('lambda', 3, self.n_contact) """Hopper Dynamics""" "Joint positions from Base to end effector" p = ca.SX.zeros(2, self.n_joints) p[0, 0], p[1, 0] = q[0] + (self.length[1, 0]) * ca.sin( q[2]), q[1] - (self.length[1, 0]) * ca.cos(q[2]) p[0, 1], p[1, 1] = p[0, 0] + self.length[2, 0] * ca.sin(q[2] + q[3]), p[ 1, 0] - self.length[2, 0] * ca.cos(q[2] + q[3]) # COG g = ca.SX.zeros(2, self.n_joints + 1) g[0, 0], g[1, 0] = q[0], q[1] g[0, 1], g[1, 1] = p[0, 0] / 2, p[1, 0] / 2 g[0, 2], g[ 1, 2] = p[0, 0] + (self.length[2, 0] / 2) * ca.sin(q[2] + q[3]), p[ 1, 0] - (self.length[2, 0] / 2) * ca.cos(q[2] + q[3]) J_ang = ca.DM([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1., 0], [0, 0, 1., 1.]]) a = (J_ang @ q).T J_ee = ca.jacobian(p[:, -1], q) "For inertia matrix" H = ca.SX.zeros(self.n_generalized, self.n_generalized) for i in range(self.dof): J_l = ca.jacobian(p[:, i], q) J_a = ca.jacobian(a[:, i], q) I = (1 / 12) * self.mass[i] * (self.length[i, 0]**2) # Rod M = self.mass[i] H += M * J_l.T @ J_l + I * J_a.T @ J_a "For coriolis + centrifugal matrix" C = ca.SX.zeros(self.n_generalized, self.n_generalized) for i in range(self.n_generalized): for j in range(self.n_generalized): sum_ = 0 for k in range(self.n_generalized): c_ijk = ca.jacobian(H[i, j], q[k]) + ca.jacobian( H[i, j], q[j]) - ca.jacobian(H[k, j], q[i]) sum_ += c_ijk @ dq[k] C[i, j] = (1 / 2) * sum_ # sum_ = 0 # for k in range(self.dof): # c_ijk = ca.jacobian(H[i, j], q[k]) - (1/2)*ca.jacobian(H[j, k], q[i]) # sum_ += c_ijk @ dq[j] @ dq[k] # C[i, j] = sum_ "For G matrix" V = self.gravity * ca.sum1(self.mass * g[1, :].T) G = ca.jacobian(V, q).T "For B matrix" B = ca.DM([[0., 0.], [0., 0.], [1., 0.], [0., 1.]]) "For external force" pe_terrain = ca.SX.zeros(2, 1) pe_terrain[0, 0] = p[0, -1] pe_terrain[1, 0] = self.terrain.heightMap(p[0, -1]) phi = p[:, -1] - pe_terrain B_J_C = ca.jacobian(phi, q) theta_contact = ca.acos( ca.dot(self.terrain.heightMapNormalVector(p[0, -1]), ca.DM([0, 1]))) # rotate lambda force represented in contact frame to world frame B_Rot_C = ca.SX.zeros(2, 2) B_Rot_C[0, 0], B_Rot_C[0, 1] = ca.cos(theta_contact), ca.sin(theta_contact) B_Rot_C[1, 0], B_Rot_C[1, 1] = -ca.sin(theta_contact), ca.cos(theta_contact) C_lam = ca.SX.zeros(2, 1) C_lam[0, 0], C_lam[1, 0] = lam[0, 0] - lam[1, 0], lam[2, 0] B_lam = B_Rot_C @ C_lam dp = J_ee @ dq # print(dp.shape) psi = ca.dot(self.terrain.heightMapTangentVector(pe_terrain[0, 0]), dp) # psi = dp[0, 0] self.kinematics = ca.Function('Kinematics', [q], [p, g], ['q'], ['p', 'g']) self.dynamics = ca.Function('Dynamics', [q, dq, u, lam], [ H, C, B, G, phi, J_ee, B_J_C, C_lam, B_lam, psi ], ['q', 'dq', 'u', 'lam'], [ 'H', 'C', 'B', 'G', 'phi', 'J_ee', 'B_J_C', 'C_lam', 'B_lam', 'psi' ])