Execute a plan/trajectory in the MuJoCo simulator.
Beginner Tutorial | Expected duration is 30 minutes
By: Alrick Dsouza
Introduction
This tutorial will introduce you to controlling the mushr car in the MuJoCo environment. Why MuJoCo??? MuJoCo is a physics simulator which provides a unique combination of speed, accuracy and modeling power for the purpose of model based optimization, and in particular optimization through contacts. MuJoCo is your goto simulator for RL and Deep RL projects. A well-known example would be DeepMind’s humanoid simulations. You can find some very interesting OpenAI+MuJoCo projects here.
Goal
To command the MuSHR car to execute a figure 8 plan in the MuJoCo simulator.
Requirements
- Complete the Quickstart Tutorial
- Complete the MuJoco Simulation Tutorial
Setup
Let’s first start by creating a new rospackage.
cd ~/catkin_ws/src
catkin_create_pkg mushr_mujoco_figure8 rospy geometry_msgs ackermann_msgs
Now let’s make the directories we will be needing.
cd mushr_mujoco_figure8
mkdir launch plans
We will now create a text file containing a sequence of control commands designed to produce a trajectory of figure-8 shape when executed by the vehicle
$ cd plans
$ nano figure8.txt
Paste the following sequence of control commands into the figure8.txt file.
0,0,0.785
2.0,0.09
2.0,0.09
2.0,0.09
2.0,0.09
2.0,0.09
2.0,0.09
2.0,0.09
2.0,0.09
2.0,0.09
2.0,0.09
2.0,0.09
2.0,-0.09
2.0,-0.09
2.0,-0.09
2.0,-0.09
2.0,-0.09
2.0,-0.09
2.0,-0.09
2.0,-0.09
2.0,-0.09
2.0,-0.09
2.0,-0.09
0.0,-0.09
Rostopic of interest
(Note: This section is only for better understanding, feel free to skip it.)
Before jumping into the code lets find out which rostopics we need to publish to. In a new terminal window type,
$ roslaunch mushr_mujoco_ros one_car.launch
Open a new terminal window, and type the following.
$ rostopic list -v
You will see a list of Published and Subscribed topics like below.
Published topics:
* /map_metadata [nav_msgs/MapMetaData] 1 publisher
* /mushr_mujoco_ros/buddy/velocimeter [geometry_msgs/Vector3Stamped] 1 publisher
* /mushr_mujoco_ros/buddy/car_imu [sensor_msgs/Imu] 1 publisher
* /rosout [rosgraph_msgs/Log] 3 publishers
* /rosout_agg [rosgraph_msgs/Log] 1 publisher
* /mux/ackermann_cmd_mux/input/teleop [ackermann_msgs/AckermannDriveStamped] 1 publisher
* /mushr_mujoco_ros/buddy/pose [geometry_msgs/PoseStamped] 1 publisher
* /mushr_mujoco_ros/body_state [mushr_mujoco_ros/BodyStateArray] 1 publisher
* /map [nav_msgs/OccupancyGrid] 1 publisher
Subscribed topics:
* /mushr_mujoco_ros/buddy/initialpose [geometry_msgs/PoseWithCovarianceStamped] 1 subscriber
* /rosout [rosgraph_msgs/Log] 1 subscriber
* /mushr_mujoco_ros/buddy/control [ackermann_msgs/AckermannDriveStamped] 1 subscriber
/mushr_mujoco_ros/buddy/control in Subscribed topics is our rostopic of interest.
Code
The entire code is written below. Feel free to try to code it by yourself, as this code is very similar to the MuSHR Introductory navigation (link: MuSHR Intro to ROS), and from the earlier section we now know our rostopic of interest. The code will be explained later on.
$ cd ~/catkin_ws/src/mushr_mujoco_figure8/src
$ nano path_publisher.py
Paste the below code in path_publisher.py.
|
|
Make the python script executable.
chmod +x path_publisher.py
The code explained
The detailed explanations of the python functions (run_plan, send_init_pose, send_command) are given in the introductory navigation tutorial (link: MuSHR Intro to ROS).
|
|
Line 2 initializes the rosnode. Lines 4-5 initializes the publisher node to control the MuSHR car. rospy.get_param("~control_topic", "/mushr_mujoco_ros/buddy/control")
calls the rostopic we want to publish messages to (will be more clear while writing the launch file, notice this is the highlighted rostopic from before). Similarly lines 7-9 initializes the publisher node to set the initial position of the MuSHR bot. Lines 11-13 reads the figure8.txt file we just created.
Writing the launch file
We need to create a launch file to launch our path_publisher.py code. Again the entire code is pasted below, feel free to write your own launch file (Note: Make sure to implement relevant changes in path_publisher.py in case you decide to make changes).
$ cd ~/catkin_ws/src/mushr_mujoco_figure8/launch
$ nano path_publisher.launch
Paste the below code in path_publisher.launch.
|
|
The launch file explained
Lines 2-4 create arguments with default values (note: these values can be set externally while running the launch file). Lines 6-9, initialize a rosnode with parameters “control_topic”, “init_pose_topic” and “plan_file”, which if you recall are used in rospy.get_param() in path_publisher.py. We can set values to arguments (arg) while executing the launch file. If no value was assigned to the argument, the default is used. In case you decide to make modifications and add some more arguments, make sure you always assign default values to the arguments.
Catkin_make
You are all set. Lets catkin_make our files to start executing the figure 8.
$ cd ~/catkin_ws/
$ catkin_make
Executing the figure 8
Now it’s time to execute our code. First let’s launch the MuSHR car. In a new terminal, enter the below commands.
$ source ~/catkin_ws/devel/setup.bash
$ roslaunch mushr_mujoco_ros one_car.launch
After this step, a MuSHR car is spawned in the MuJoCo simulator as shown below.
Next, in a new terminal enter the following roslaunch command.
$ roslaunch mushr_mujoco_figure8 path_publisher.launch
Since the default plan file is our plan file of interest, we don’t need to add the path to figure8.txt manually. But, if you would like to call a different plan file you can write:
$ roslaunch mushr_mujoco_figure8 path_publisher.launch plan_file:='~/catkin_ws/src/mushr_mujoco_figure8/plans/{name of text file}'
Awesome, if your MuSHR car makes an 8, you have successfully completed this tutorial! Feel free to play around and design your own plans and execute them.