-
Notifications
You must be signed in to change notification settings - Fork 3
Overview
The purpose of this software is to make us able to flexibly simulate autonomous drones and autonomous ground vehicles in the MuJoCo environment. The software's aim is to make complex simulation experiments easy to configure. The simulations can serve as a basis for controller design and trajectory planning algorithms.
MuJoCo stands for Multi-Joint dynamics with Contact. It is a general purpose physics engine that aims to facilitate research and development in robotics, biomechanics, graphics and animation, machine learning, and other areas that demand fast and accurate simulation of articulated structures interacting with their environment. It is a C/C++ library with a C API and python bindings. The Python API is consistent with the underlying C API which leads to some non-Pythonic code structure, but this way the documentation is supposed to be applicable to both languages.
The code in this repository propagates the required functionality of MuJoCo in a more object-oriented and high-level way to make it quicker and easier to use, and enables writing more reusable code.
The user can define a model in an XML file written in MJCF or URDF format. This project only uses the MJCF format. The XML file is then loaded and parsed by MuJoCo that creates an mjCModel (a C++ class) in memory, which is then compiled into an mjModel (a C struct):
(text editor) → MJCF/URDF file → (MuJoCo parser → mjCModel → MuJoCo compiler) → mjModel
Generating MuJoCo XML models provides more detailed descriptions of how to build a model.
- Types of objects in the simulation
- Creating MuJoCo models as XML
- ActiveSimulator class that manages the vehicles and the MuJoCo physics engine
- Trajectory classes for vehicle trajectories
- Controller classes to control vehicles
- Communication with motion capture systems
There are three types of objects that can be present in a simulation: background objects, moving objects and mocap objects. Of these, moving objects and mocap objects need to have corresponding Python classes because they need to be manipulated in some way, or their information needs to be collected or processed.
- Background objects: These can be static objects, like floor, walls, etc.; or they can be free objects like a something falling from the sky (on which only gravity and collisions can act). The main thing that separates these objects from the other two categories is that they are not of the main interest of a particuliar simulation. Therefore, they do not need have corresponding python classes because they don't have to be or cannot be manipulated by a script and no information about them needs to be collected during simulation.
- Moving objects with degrees of freedom: like a payload, drones, cars. They can -- but do not have to -- be actuated, and they need to have some sort of a joint that lets them move. The corresponding python classes are child classes of MovingObject base class. The reason python classes are needed is to be able to read and modify parameters, properties and other data of these objects during simulation (such as the mass or position of a payload or control input of a drone). These objects can be controlled or uncontrolled which is determined by whether they have actuators or not. Therefore, their update method needs to be written accordingly.
- Mocap objects: motion capture objects that take part in the collisions but do not take part in the simulation in any other way, and their position is updated based on a motion capture system. The corresponding python classes are subclasses of MocapObject.
This framework has only been used and tested with MJCF models, therefore, only this type of format will be discussed. MuJoCo does not yet provide the option to build models programmatically, so the user can define a model by writing an XML file by hand, or by using a script to generate XML files. In the framework, the SceneXmlGenerator class serves as an example on how to generate XML files with a Python program. The class uses the xml.etree.ElementTree package to build the XML tree and then to save it as text.
This is the main simulator class that initializes the MuJoCo environment and takes care of updating the vehicles, stepping the physics engine, updating the graphics, handling key press events, etc. The simulation itself takes place in MuJoCo, this class only facilitates it.
Since MuJoCo is written in C and C++, all the information about the simulation is stored in a huge structure called MjData, where the data of an individual object can be accessed with its index which can be confusing in case of a complex simulation. This class provides a solution to this problem. When an instance of the class is created with an MJCF model, it calls the parser functions it received in the constructor with the MJCF model as an input. The parser functions each return a list of MovingObjects or MocapObjects that they found in the model. These are Python objects that reference all the necessary arrays in memory that correspond to the given MJCF object in MuJoCo's MjData. Therefore, the sensor-, actuator- and other data of an object can be accessed in a more straightforward way.
This class does not know what kind of objects it is simulating, it only knows that each object is a subclass of MovingObject or MocapObject and therefore, has an update method that it needs to call at every control step. This way, it's simple to create a new type of simulated object because all it has to do is have an update method with the correct header.
In a simulation, there can be objects whose data is unimportant and do not need to be accessed in any way. They still take part in the physics simulation as they are in the MJCF model, but these objects do not require a corresponding Python class and do not need to be parsed.
A MovingObject instance can have a trajectory object attached to it. All the trajectory classes should be child classes of TrajectoryBase, therefore they should have an evaluate method with the correct header (see in TrajectoryBase), and return a dictionary containing the setpoint. This is the method that calculates the next setpoint for the vehicle according to its state and the time. The user can decide how to implement the algorithms in the evaluate method. The general concept is that the simulator calls every MovingObject instance's update method in which the evaluate method is called with the vehicle's state and the returned setpoint is used to calculate control for the vehicle.
In AIMotionLab, the actual vehicles receive their dynamic trajectory from a SkyBrush server, so we implemented a similar communication in the simulation framework. To receive trajectories in the simulation, TCP communication is used that runs on a background thread. The data has to be sent in a predefined format so that it can be decoded on the receiving end. The proposed architecture consists of two classes: one that takes care of the communication, and distributes the received trajectory data among the vehicles on a background thread, and one that is the subclass of TrajectoryBase and can store the current piece of trajectory that needs to be evaluated. The latter is responsible for maintaining thread-safety when its trajectory data is being updated or evaluated, because they could happen at the same time.
A MovingObject instance can have one or more controller objects attached to it. All the controller classes should be child classes of ControllerBase, therefore they should have a compute_control method with the correct header (see in ControllerBase). The input of the method is the setpoint coming from the trajectory's evaluate() and the output is the control input of the actuators. The vehicle can decide whether to switch controllers based on its state, or based on a signal from the trajectory object. The MovingObject class also provides the option to accept a function written by the user that can determine which controller to use.
To display the motion of real objects in a virtual environment as a digital twin, their position and orientation need to be updated in real time. Visually, the same virtual objects as the ones used for simulation can be used. Only, they do not need any actuators, sensors and joints, and they have to be marked "mocap" objects. The mocap objects' position and orientation can be directly updated by rewriting the corresponding array in the MjData instance. These objects only take part in collisions, no other forces are calculated on them.
Depending on what motion capture system is used, and how fast the data is streamed, the communication can run on the same thread as the simulation or on a different thread (if streaming is too slow) like in the case of the trajectories. Our setup uses the Optitrack system that can stream at 120 Hz. Control for the vehicles is generally calculated at 100 Hz, so the communication is handled on the main thread because whenever there are simulated vehicles simultaneously with mocap vehicles the communication will not cause jams. This package is used to communicate with Optitrack.
Rather than the concept of the simulator, these are the steps of a simulation in practice.
-
Scene construction: the scene is defined in one or more XML files. These files have to contain definitions of the objects that need to take part in the simulation because once the model is loaded, new objects cannot be added programmatically. Elements that may be needed to define objects:
- Meshes, textures, materials that can define what objects look like
- Bodies that contain geoms, joints, other bodies, sites and definitions of inertia and mass
- Actuators
- Sensors
- Contacts
A scene can be constructed by hand which is not recommended, or by the SceneXmlGenerator class which contains definitions for a set of predefined objects and can generate XML files based on them. For details on how to build models and objects, see Generating MuJoCo XML models
-
Loading the model: the model can be loaded by the ActiveSimulator class that calls the mujoco.MjModel.from_xml_path() to obtain the mjModel. On initialization, the class calls the list of object parser functions that were passed to their constructor and if these functions find any objects whose names conform to the naming convention, they create a list of corresponding Python objects and return this list. The ActiveSimulator class appends the list to its own list of objects which is initially empty. The simulator will later use this list to update the objects by calling their update method.
-
Creating and assigning controllers and trajectories to vehicles.
-
Running the simulation: After the ActiveSimulator loaded and parsed the mjModel, its update method needs to be called repeatedly in an infinite loop to carry out the simulation. This method fetches data from the Optitrack motioncapture system, updates the position of mocap objects, steps the physics engine, renders the graphics and saves the rendered frames in a list if video recording is on.
Here is a complete example of a drone hovering simulation:
import os
from aiml_virtual.xml_generator import SceneXmlGenerator
from aiml_virtual.simulator import ActiveSimulator
from aiml_virtual.controller import LqrLoadControl
from aiml_virtual.object.drone import DRONE_TYPES
from aiml_virtual.object import parseMovingObjects
from aiml_virtual.trajectory import TrajectoryBase
import numpy as np
# a dummy trajectory class to simulate a hovering drone
class DummyHoverTraj(TrajectoryBase):
def __init__(self, load_mass, target_pos):
super().__init__()
self.load_mass = load_mass
self.target_pos = target_pos
def evaluate(self, state, i, time, control_step):
self.output["load_mass"] = self.load_mass
self.output["target_pos"] = self.target_pos
self.output["target_rpy"] = np.array((0.0, 0.0, 0.0))
self.output["target_vel"] = np.array((0.0, 0.0, 0.0))
self.output["target_pos_load"] = np.array((0.0, 0.0, 0.0))
self.output["target_eul"] = np.zeros(3)
self.output["target_pole_eul"] = np.zeros(2)
self.output["target_ang_vel"] = np.zeros(3)
self.output["target_pole_ang_vel"] = np.zeros(2)
return self.output
RED = "0.85 0.2 0.2 1.0"
BLUE = "0.2 0.2 0.85 1.0"
# -------------------------------- 1. ---------------------------------
abs_path = os.path.dirname(os.path.abspath(__file__))
xml_path = os.path.join(abs_path, "..", "xml_models")
xmlBaseFileName = "scene_base.xml"
save_filename = "built_scene.xml"
# Set scenario parameters
drone0_init_pos = np.array([0.0, 0.0, 1.2, 0])
load0_mass = 0.2
load0_size = np.array([.07, .07, .04])
load0_initpos = np.array([drone0_init_pos[0], drone0_init_pos[1], drone0_init_pos[2] - (2 * load0_size[2]) - .57 ])
# create the model as xml
scene = SceneXmlGenerator(xmlBaseFileName)
drone0_name = scene.add_drone(np.array2string(drone0_init_pos[0:3])[1:-1], "1 0 0 0", BLUE, DRONE_TYPES.BUMBLEBEE_HOOKED, 2)
payload0_name = scene.add_payload(np.array2string(load0_initpos)[1:-1], np.array2string(load0_size)[1:-1], str(load0_mass), "1 0 0 0", RED)
scene.save_xml(os.path.join(xml_path, save_filename))
# -------------------------------- 2. ---------------------------------
virt_parsers = [parseMovingObjects]
mocap_parsers = None
control_step, graphics_step = 0.01, 0.02
xml_filename = os.path.join(xml_path, save_filename)
simulator = ActiveSimulator(xml_filename, None, control_step, graphics_step, virt_parsers, mocap_parsers,
connect_to_optitrack=False)
drone0 = simulator.get_MovingObject_by_name_in_xml(drone0_name)
payload0 = simulator.get_MovingObject_by_name_in_xml(payload0_name)
# -------------------------------- 3. ---------------------------------
drone0_trajectory = DummyHoverTraj(payload0.mass, drone0_init_pos[0:3])
drone0_controller = LqrLoadControl(drone0.mass, drone0.inertia, simulator.gravity)
drone0_controllers = [drone0_controller]
drone0.set_trajectory(drone0_trajectory)
drone0.set_controllers(drone0_controllers)
# -------------------------------- 4. ---------------------------------
while not simulator.should_close():
simulator.update()
simulator.close()