Note

Generated by nbsphinx from a Jupyter notebook. All the examples as Jupyter notebooks are available in the tudatpy-examples repo.

# Asteroid orbit optimization with PyGMO - Custom Environment

Copyright (c) 2010-2022, Delft University of Technology. All rights reserved. This file is part of the Tudat. Redistribution and use in source and binary forms, with or without modification, are permitted exclusively under the terms of the Modified BSD license. You should have received a copy of the license with this file. If not, please or visit: http://tudat.tudelft.nl/LICENSE.

## Context

The aim of this tutorial is to illustrate the use of PyGMO to optimize an astrodynamics problem simulated with tudatpy. The problem describes the orbit design around a small body; the Itokawa asteroid. This example consists of three parts that build on each other to ultimately perform an orbit optimization. The three parts are built up as follows: Custom Environment (this page), Design Space Exploration, and Optimization.

**This part of the example is focussed on the creation of a custom environment, using manually defined rotation, ephemeris, gravity field, and shape settings**. A PyGMO compatible Problem class is also created for the next parts of the example. Using this problem class, a propagation is conducted to show a possible trajectory orbiting Itokawa.

### NOTE

It is assumed that the reader of this tutorial is already familiar with the content of this basic PyGMO tutorial. The full PyGMO documentation is available on this website. Be careful to read the correct the documentation webpage (there is also a similar one for previous yet now outdated versions here; as you can see, they can easily be confused). PyGMO is the Python counterpart of PAGMO.

## Import statements

The required import statements are made here, at the very beginning.

Some standard modules are first loaded. These are `os`

, `numpy`

and `matplotlib.pyplot`

.

Then, the different modules of `tudatpy`

that will be used are imported.

Finally, in this example, we also need to import the `pygmo`

library.

```
[1]:
```

```
# Load standard modules
import os
import numpy as np
# Uncomment the following to make plots interactive
# %matplotlib widget
from matplotlib import pyplot as plt
from itertools import combinations as comb
# Load tudatpy modules
from tudatpy.io import save2txt
from tudatpy.kernel import constants
from tudatpy.kernel.interface import spice
from tudatpy.kernel.astro import element_conversion
from tudatpy.kernel.astro import frame_conversion
from tudatpy.kernel import numerical_simulation
from tudatpy.kernel.numerical_simulation import environment_setup
from tudatpy.kernel.numerical_simulation import propagation_setup
from tudatpy.util import pareto_optimums
# Load pygmo library
import pygmo as pg
current_dir = os.path.abspath('')
```

## Creation of Custom Environment

Below a few helper functions are defined that create various custom environment models. These functions are used later in the example to create the simulation

### Itokawa rotation settings

The first helper function that is setup is `get_itokawa_rotation_settings()`

. This function can be called to get Itokawa rotation settings, of type environment_setup.rotation_model.RotationModelSettings, using a constant angular velocity.

This function only take the name of the body frame of Itokawa as an input. In addition, some fixed parameters are defined in this function:

The orientation of Itokawa pole, as:

The pole declination of -66.3 deg.

The pole right ascension 90.53 deg.

The meridian fixed at 0 deg.

The rotation rate of Itokawa of 712.143 deg/Earth day.

```
[2]:
```

```
def get_itokawa_rotation_settings(itokawa_body_frame_name):
# Definition of initial Itokawa orientation conditions through the pole orientation
pole_declination = np.deg2rad(-66.30) # Declination
pole_right_ascension = np.deg2rad(90.53) # Right ascension
meridian_at_epoch = 0.0 # Meridian
# Define initial Itokawa orientation in inertial frame (equatorial plane)
initial_orientation_j2000 = frame_conversion.inertial_to_body_fixed_rotation_matrix(
pole_declination, pole_right_ascension, meridian_at_epoch)
# Get initial Itokawa orientation in inertial frame but in the Ecliptic plane
initial_orientation_eclipj2000 = np.matmul(spice.compute_rotation_matrix_between_frames(
"ECLIPJ2000", "J2000", 0.0), initial_orientation_j2000)
# Manually check the results, if desired
check_results = False
if check_results:
np.set_printoptions(precision=100)
print(initial_orientation_j2000)
print(initial_orientation_eclipj2000)
# Compute rotation rate
rotation_rate = np.deg2rad(712.143) / constants.JULIAN_DAY
# Set up rotational model for Itokawa with constant angular velocity
return environment_setup.rotation_model.simple(
"ECLIPJ2000", itokawa_body_frame_name, initial_orientation_eclipj2000, 0.0, rotation_rate)
```

### Itokawa ephemeris settings

The next helper function defined, `get_itokawa_ephemeris_settings()`

, that can be used to set the ephemeris of Itokawa.

The ephemeris for Itokawa are set using a Keplerian orbit around the Sun. To do this, the initial position at a certain epoch is needed. This position is defined inside the function by the following kepler elements:

Semi-major axis of \(\approx\) 1.324 Astronomical Units (\(\approx\) 1.98E+8 km).

Eccentricity of \(\approx\) 0.28.

Inclination of \(\approx\) 1.62 deg.

Inclination of \(\approx\) 1.62 deg.

Argument of periapsis of \(\approx\) 162.8 deg.

Longitude of ascending node of \(\approx\) 69.1 deg.

Mean anomaly of \(\approx\) 187.6 deg.

The only input that this function takes is the gravitational parameter of the Sun, which can be obtained using `spice.get_body_gravitational_parameter("Sun")`

.

```
[3]:
```

```
def get_itokawa_ephemeris_settings(sun_gravitational_parameter):
# Define Itokawa initial Kepler elements
itokawa_kepler_elements = np.array([
1.324118017407799 * constants.ASTRONOMICAL_UNIT,
0.2801166461882852,
np.deg2rad(1.621303507642802),
np.deg2rad(162.8147699851312),
np.deg2rad(69.0803904880264),
np.deg2rad(187.6327516838828)])
# Convert mean anomaly to true anomaly
itokawa_kepler_elements[5] = element_conversion.mean_to_true_anomaly(
eccentricity=itokawa_kepler_elements[1],
mean_anomaly=itokawa_kepler_elements[5])
# Get epoch of initial Kepler elements (in Julian Days)
kepler_elements_reference_julian_day = 2459000.5
# Sets new reference epoch for Itokawa ephemerides (different from J2000)
kepler_elements_reference_epoch = (kepler_elements_reference_julian_day - constants.JULIAN_DAY_ON_J2000) \
* constants.JULIAN_DAY
# Sets the ephemeris model
return environment_setup.ephemeris.keplerian(
itokawa_kepler_elements,
kepler_elements_reference_epoch,
sun_gravitational_parameter,
"Sun",
"ECLIPJ2000")
```

### Itokawa gravity field settings

The `get_itokawa_gravity_field_settings()`

helper function can be used to get the gravity field settings of Itokawa.

It creates a Spherical Harmonics gravity field model expanded up to order 4 and degree 4. Normalized coefficients are hardcoded (see `normalized_cosine_coefficients`

and `normalized_sine_coefficients`

), as well as the gravitational parameter (2.36). The reference radius and the Itokawa body fixed frame are to be given as inputs to this function.

```
[4]:
```

```
def get_itokawa_gravity_field_settings(itokawa_body_fixed_frame, itokawa_radius):
itokawa_gravitational_parameter = 2.36
normalized_cosine_coefficients = np.array([
[1.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[-0.145216, 0.0, 0.219420, 0.0, 0.0],
[0.036115, -0.028139, -0.046894, 0.069022, 0.0],
[0.087852, 0.034069, -0.123263, -0.030673, 0.150282]])
normalized_sine_coefficients = np.array([
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, -0.006137, -0.046894, 0.033976, 0.0],
[0.0, 0.004870, 0.000098, -0.015026, 0.011627]])
return environment_setup.gravity_field.spherical_harmonic(
gravitational_parameter=itokawa_gravitational_parameter,
reference_radius=itokawa_radius,
normalized_cosine_coefficients=normalized_cosine_coefficients,
normalized_sine_coefficients=normalized_sine_coefficients,
associated_reference_frame=itokawa_body_fixed_frame)
```

### Itokawa shape settings

The next helper function defined, `get_itokawa_shape_settings()`

return the shape settings object for Itokawa. It uses a simple spherical model, and take the radius of Itokawa as input.

```
[5]:
```

```
def get_itokawa_shape_settings(itokawa_radius):
# Creates spherical shape settings
return environment_setup.shape.spherical(itokawa_radius)
```

### Simulation bodies

Next, the `create_simulation_bodies()`

function is setup, that returns an environment.SystemOfBodies object. This object contains all the body settings and body objects required by the simulation. Only one input is required to this function: the radius of Itokawa.

Moreover, in the system of bodies that is returned, a `Spacecraft`

body is included, with a mass of 400kg, and a radiation pressure interface. This body is the one for which an orbit is to be optimised around Itokawa.

```
[6]:
```

```
def create_simulation_bodies(itokawa_radius):
### CELESTIAL BODIES ###
# Define Itokawa body frame name
itokawa_body_frame_name = "Itokawa_Frame"
# Create default body settings for selected celestial bodies
bodies_to_create = ["Sun", "Earth", "Jupiter", "Saturn", "Mars"]
# Create default body settings for bodies_to_create, with "Earth"/"J2000" as
# global frame origin and orientation. This environment will only be valid
# in the indicated time range [simulation_start_epoch --- simulation_end_epoch]
body_settings = environment_setup.get_default_body_settings(
bodies_to_create,
"SSB",
"ECLIPJ2000")
# Add Itokawa body
body_settings.add_empty_settings("Itokawa")
# Adds Itokawa settings
# Gravity field
body_settings.get("Itokawa").gravity_field_settings = get_itokawa_gravity_field_settings(itokawa_body_frame_name,
itokawa_radius)
# Rotational model
body_settings.get("Itokawa").rotation_model_settings = get_itokawa_rotation_settings(itokawa_body_frame_name)
# Ephemeris
body_settings.get("Itokawa").ephemeris_settings = get_itokawa_ephemeris_settings(
spice.get_body_gravitational_parameter( 'Sun') )
# Shape (spherical)
body_settings.get("Itokawa").shape_settings = get_itokawa_shape_settings(itokawa_radius)
# Create system of selected bodies
bodies = environment_setup.create_system_of_bodies(body_settings)
### VEHICLE BODY ###
# Create vehicle object
bodies.create_empty_body("Spacecraft")
bodies.get("Spacecraft").set_constant_mass(400.0)
# Create radiation pressure settings, and add to vehicle
reference_area_radiation = 4.0
radiation_pressure_coefficient = 1.2
radiation_pressure_settings = environment_setup.radiation_pressure.cannonball(
"Sun",
reference_area_radiation,
radiation_pressure_coefficient)
environment_setup.add_radiation_pressure_interface(
bodies,
"Spacecraft",
radiation_pressure_settings)
return bodies
```

### Acceleration models

The `get_acceleration_models()`

helper function returns the acceleration models to be used during the astrodynamic simulation. The following accelerations are included:

Gravitational acceleration modelled as a Point Mass from the Sun, Jupiter, Saturn, Mars, and the Earth.

Gravitational acceleration modelled as Spherical Harmonics up to degree and order 4 from Itokawa.

Radiatio pressure from the Sun using a simplified canonnball model.

This function takes as input the list of bodies that will be propagated, the list of central bodies related to the propagated bodies, and the system of bodies used.

```
[7]:
```

```
def get_acceleration_models(bodies_to_propagate, central_bodies, bodies):
# Define accelerations acting on Spacecraft
accelerations_settings_spacecraft = dict(
Sun = [ propagation_setup.acceleration.cannonball_radiation_pressure(),
propagation_setup.acceleration.point_mass_gravity() ],
Itokawa = [ propagation_setup.acceleration.spherical_harmonic_gravity(3, 3) ],
Jupiter = [ propagation_setup.acceleration.point_mass_gravity() ],
Saturn = [ propagation_setup.acceleration.point_mass_gravity() ],
Mars = [ propagation_setup.acceleration.point_mass_gravity() ],
Earth = [ propagation_setup.acceleration.point_mass_gravity() ]
)
# Create global accelerations settings dictionary
acceleration_settings = {"Spacecraft": accelerations_settings_spacecraft}
# Create acceleration models
return propagation_setup.create_acceleration_models(
bodies,
acceleration_settings,
bodies_to_propagate,
central_bodies)
```

### Termination settings

The termination settings for the simulation are defined by the `get_termination_settings()`

helper.

Nominally, the simulation terminates when a final epoch is reached. However, this can happen in advance if the spacecraft breaks out of the predefined altitude range. This is defined by the four inputs that this helper function takes, related to the mission timing and the mission altitude range.

```
[8]:
```

```
def get_termination_settings(mission_initial_time,
mission_duration,
minimum_distance_from_com,
maximum_distance_from_com):
# Mission duration
time_termination_settings = propagation_setup.propagator.time_termination(
mission_initial_time + mission_duration,
terminate_exactly_on_final_condition=False
)
# Upper altitude
upper_altitude_termination_settings = propagation_setup.propagator.dependent_variable_termination(
dependent_variable_settings=propagation_setup.dependent_variable.relative_distance('Spacecraft', 'Itokawa'),
limit_value=maximum_distance_from_com,
use_as_lower_limit=False,
terminate_exactly_on_final_condition=False
)
# Lower altitude
lower_altitude_termination_settings = propagation_setup.propagator.dependent_variable_termination(
dependent_variable_settings=propagation_setup.dependent_variable.altitude('Spacecraft', 'Itokawa'),
limit_value=minimum_distance_from_com,
use_as_lower_limit=True,
terminate_exactly_on_final_condition=False
)
# Define list of termination settings
termination_settings_list = [time_termination_settings,
upper_altitude_termination_settings,
lower_altitude_termination_settings]
return propagation_setup.propagator.hybrid_termination(termination_settings_list,
fulfill_single_condition=True)
```

### Dependent variables to save

Finally, the `get_dependent_variables_to_save()`

helper function returns a pre-defined list of dependent variables to save during the propagation, alongside the propagated state. This function can be expanded, but contains by default only the position of the spacecraft with respect to the Itokawa asteroid expressed in spherical coordinates.

```
[9]:
```

```
def get_dependent_variables_to_save():
dependent_variables_to_save = [
propagation_setup.dependent_variable.central_body_fixed_spherical_position(
"Spacecraft", "Itokawa"
)
]
return dependent_variables_to_save
```

## Optimisation problem formulation

The optimisation problem can now be defined. This has to be done in a class that is compatible to what the PyGMO library can expect from this User Defined Problem (UDP). See this page from the PyGMO documentation as a reference. In this example, this class is called `AsteroidOrbitProblem`

.

The `AsteroidOrbitProblem.__init__()`

method is used to setup the problem. Most importantly, many problem-related objects are saved trough it: the system of bodie, the integrator settings, the propagator settings, the parameters that will later be used for the termination settings, and the design variables boundaries.

Then, the `AsteroidOrbitProblem.get_bounds()`

function is used by PyGMO to define the search space. This function returns the boundaries of each design variable, as defined in the pygmo.problem.get_bounds documentation.

The `AsteroidOrbitProblem.get_nobj()`

function is also used by PyGMO. It returns the number of objectives in the problem, in this case 2.

The last function used by PyGMO is `AsteroidOrbitProblem.fitness()`

. As mentioned in the pygmo.problem.fitness documentation, PyGMO will input a given set of design variable to this function, that is expected to return a score associated with them. This is thus the cost function of the problem. In this case, this `AsteroidOrbitProblem.fitness()`

runs a simulation using TudatPy based on the orbital elements that PyGMO
inputs as design variables. Then, the score relative to the two optimisation objectives is computed and returned. Note that, in PyGMO, this fitness function will always be **minimised**. To **maximise** objectives instead, the fitness that is returned will have to be for instance inversed. This is why, because we want to maximise the coverage, the fitness for this objective is computed as the inverse of the mean latitudes. If the mean of the latitudes is high, the coverage is high, which is
closer to the optimum. Because this better value is higher than worse values, we return the fitness as the inverse of the mean latitudes.

One more function is included, `AsteroidOrbitProblem.get_last_run_dynamics_simulator()`

. This allows to get the dynamic simulator of the last simulation that was run in the problem.

```
[10]:
```

```
class AsteroidOrbitProblem:
def __init__(self,
bodies,
propagator_settings,
mission_initial_time,
mission_duration,
design_variable_lower_boundaries,
design_variable_upper_boundaries):
# Sets input arguments as lambda function attributes
# NOTE: this is done so that the class is "pickable", i.e., can be serialized by pygmo
self.bodies_function = lambda: bodies
self.propagator_settings_function = lambda: propagator_settings
# Initialize empty dynamics simulator
self.dynamics_simulator_function = lambda: None
# Set other input arguments as regular attributes
self.mission_initial_time = mission_initial_time
self.mission_duration = mission_duration
self.mission_final_time = mission_initial_time + mission_duration
self.design_variable_lower_boundaries = design_variable_lower_boundaries
self.design_variable_upper_boundaries = design_variable_upper_boundaries
def get_bounds(self):
return (list(self.design_variable_lower_boundaries), list(self.design_variable_upper_boundaries))
def get_nobj(self):
return 2
def fitness(self,
orbit_parameters):
# Retrieves system of bodies
current_bodies = self.bodies_function()
# Retrieves Itokawa gravitational parameter
itokawa_gravitational_parameter = current_bodies.get("Itokawa").gravitational_parameter
# Reset the initial state from the design variable vector
new_initial_state = element_conversion.keplerian_to_cartesian_elementwise(
gravitational_parameter=itokawa_gravitational_parameter,
semi_major_axis=orbit_parameters[0],
eccentricity=orbit_parameters[1],
inclination=np.deg2rad(orbit_parameters[2]),
argument_of_periapsis=np.deg2rad(235.7),
longitude_of_ascending_node=np.deg2rad(orbit_parameters[3]),
true_anomaly=np.deg2rad(139.87))
# Retrieves propagator settings object
propagator_settings = self.propagator_settings_function()
# Reset the initial state
propagator_settings.initial_states = new_initial_state
# Propagate orbit
dynamics_simulator = numerical_simulation.create_dynamics_simulator(current_bodies,
propagator_settings)
# Update dynamics simulator function
self.dynamics_simulator_function = lambda: dynamics_simulator
# Retrieve dependent variable history
dependent_variables = dynamics_simulator.dependent_variable_history
dependent_variables_list = np.vstack(list(dependent_variables.values()))
# Retrieve distance
distance = dependent_variables_list[:, 0]
# Retrieve latitude
latitudes = dependent_variables_list[:, 1]
# Compute mean latitude
mean_latitude = np.mean(np.absolute(latitudes))
# Computes fitness as mean latitude
current_fitness = 1.0 / mean_latitude
# Exaggerate fitness value if the spacecraft has broken out of the selected distance range
current_penalty = 0.0
if (max(dynamics_simulator.dependent_variable_history.keys()) < self.mission_final_time):
current_penalty = 1.0E2
return [current_fitness + current_penalty, np.mean(distance) + current_penalty * 1.0E3]
def get_last_run_dynamics_simulator(self):
return self.dynamics_simulator_function()
```

### Setup orbital simulation

Before running the optimisation, some aspect of the orbital simulation around Itokawa still need to be setup. Most importantly, the simulation bodies, acceleration models, integrator settings, and propagator settings, all have to be defined. To do so, the helpers that were defined above are used.

#### Simulation settings

The simulation settings are first defined.

The SPICE kernels are loaded, so that we can acess the gravitational parameter of the Sun in the `create_simulation_bodies()`

function.

The definition of the termination parameters follows, with a maximum mission duration of 5 Earth days. The altitude range above Itokawa is also defined between 150 meters and 5 km.

Follows the definition of the design variable range, that PyGMO will use during the optimisation. This range is as follows:

Initial semi-major axis between 300 and 2000 meters.

Initial eccentricity between 0 and 0.3.

Initial inclination between 0 and 180 deg.

Initial longitude of the ascending node between 0 and 360 deg.

The system of bodies is then setup using the `create_simulation_bodies()`

helper, and a radius for Itokawa of 161.915 meters.

Finally, the acceleration models are setup using the `get_acceleration_models()`

helper.

```
[11]:
```

```
# Load spice kernels
spice.load_standard_kernels()
# Set simulation start and end epochs
mission_initial_time = 0.0
mission_duration = 5.0 * constants.JULIAN_DAY
# Define Itokawa radius
itokawa_radius = 161.915
# Set altitude termination conditions
minimum_distance_from_com = 150.0 + itokawa_radius
maximum_distance_from_com = 5.0E3 + itokawa_radius
# Set boundaries on the design variables
design_variable_lb = (300, 0.0, 0.0, 0.0)
design_variable_ub = (2000, 0.3, 180, 360)
# Create simulation bodies
bodies = create_simulation_bodies(itokawa_radius)
# Define bodies to propagate and central bodies
bodies_to_propagate = ["Spacecraft"]
central_bodies = ["Itokawa"]
# Create acceleration models
acceleration_models = get_acceleration_models(bodies_to_propagate, central_bodies, bodies)
```

#### Dependent variables, termination settings, and orbit parameters

To define the propagator settings in the subsequent sections, we first call the `get_dependent_variables_to_save()`

and `get_termination_settings()`

helpers to define the dependent variables and termination settings.

The `orbit_parameters`

list is defined from a `final_population.dat`

file from the optimization. These values are not crucial, but do help for a ‘relatively’ stable orbit.

```
[12]:
```

```
# Define list of dependent variables to save
dependent_variables_to_save = get_dependent_variables_to_save()
# Create propagation settings
termination_settings = get_termination_settings(
mission_initial_time, mission_duration, minimum_distance_from_com, maximum_distance_from_com)
orbit_parameters = [1.20940330e+03, 2.61526215e-01, 7.53126558e+01, 2.60280587e+02]
```

#### Integrator and Propagator settings

Let’s now define the integrator settings. In this case, a variable step integration scheme is used, with the followings:

RKF7(8) coefficient set.

Initial time step of 1 sec.

Minimum and maximum time steps of 1E-6 sec and 1 Earth day.

Relative and absolute error tolerances of 1E-8.

Then, we define pure translational propagation settings with a Cowell propagator. The initial state is set to 0 for both the position and the velocity. This is because the initial state will later be changed in the `AsteroidOrbitProblem.fitness()`

function during the optimisation.

```
[13]:
```

```
# Create numerical integrator settings
integrator_settings = propagation_setup.integrator.runge_kutta_variable_step_size(
initial_time_step=1.0,
coefficient_set=propagation_setup.integrator.CoefficientSets.rkf_78,
minimum_step_size=1.0E-6,
maximum_step_size=constants.JULIAN_DAY,
relative_error_tolerance=1.0E-8,
absolute_error_tolerance=1.0E-8)
# Get current propagator, and define translational state propagation settings
propagator = propagation_setup.propagator.cowell
# Define propagation settings
initial_state = np.zeros(6)
propagator_settings = propagation_setup.propagator.translational(central_bodies,
acceleration_models,
bodies_to_propagate,
initial_state,
mission_initial_time,
integrator_settings,
termination_settings,
propagator,
dependent_variables_to_save)
```

## Propagating Orbit

To finalise this part of the example, a simple orbit is propagated to show the plausibility of the simulation. The PyGMO compatible problem class is created, after which the fitness function is called which creates the dynamics simulator and subsequently returns the data that can be used for post-processing.

The 4 design variables are:

initial values of the semi-major axis.

initial eccentricity.

initial inclination.

initial longitude of the ascending node.

```
[14]:
```

```
orbitProblem = AsteroidOrbitProblem(bodies,
propagator_settings,
mission_initial_time,
mission_duration,
design_variable_lb,
design_variable_ub)
design_variable_vector = np.array([1.20940330e+03, 2.61526215e-01, 7.53126558e+01, 2.60280587e+02])
orbitProblem.fitness(design_variable_vector)
```

```
[14]:
```

```
[1.1222730428316021, 1290.1081546723017]
```

### Visualizing orbit

With a little bit of post-processing, the orbit can be plotted. You can see that with only 2 full orbits that the trajectory is already very perturbed. This has to do with all the perturbations that are in the model, which poses a challenge for the optimization in the next two parts of the example.

```
[15]:
```

```
state_history = orbitProblem.get_last_run_dynamics_simulator().state_history
state_history = np.vstack(list(state_history.values()))
state_history = np.vstack(state_history)
fig = plt.figure(figsize=(15,8))
ax = fig.add_subplot(1, 1, 1, projection='3d')
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
x = itokawa_radius * np.cos(u)*np.sin(v)
y = itokawa_radius * np.sin(u)*np.sin(v)
z = itokawa_radius * np.cos(v)
ax.plot_wireframe(x, y, z, color="red")
ax.plot(state_history[:, 0], state_history[:, 1], state_history[:, 2])
ax.set_xlabel('X [m]')
ax.set_ylabel('Y [m]')
ax.set_zlabel('Z [m]')
ax.set_xlim([-1000,1000])
ax.set_ylim([-1000,1000])
ax.set_zlim([-1000,1000])
ax.grid()
```