Coppelia Simulator

Coppelia is a simulator that can be easily integrated in ROS2.

Note

Installation and integration with ROS2 sections are based on notes provided by Davide Nardi and Luca Beber.

Installation

Go to the download page of the Coppelia website and download the CoppeliaSim Edu version for Ubuntu 22.04.

The software is provided as a tarball; in the following example, the software is installed in the ~/CoppeliaSim directory:

tar -xf ~/Downloads/CoppeliaSim_Edu*
mv CoppeliaSim_Edu* ~/CoppeliaSim
# Optionally delete the tarball
rm ~/Downloads/CoppeliaSim_Edu*.tar.xz

It is recommended to store the root directory of the CoppeliaSim installation as a environment variable, so that it can be easily accessed from the terminal and the rest of the presented code snippet can work with no problem. To do so add in the ~/.bashrc file the line

export COPPELIASIM_ROOT_DIR=$HOME/CoppeliaSim

or, more simply, call the command

echo "export COPPELIASIM_ROOT_DIR=$HOME/CoppeliaSim" >> ~/.bashrc

ROS2 Setup

Messages

CoppeliaSim has a ros2 workspace internally which is used to build required ROS2 messages. Messages that needs to be build must be specified inside the folder

$COPPELIASIM_ROOT_FOLDER/programming/ros2_packages/sim_ros2_interface/meta

In particular inside the file interfaces.txt must be listed all the required messages, for instance

geometry_msgs/msg/Wrench
geometry_msgs/msg/WrenchStamped
std_msgs/msg/MultiArrayDimension
std_msgs/msg/MultiArrayLayout
std_msgs/msg/Float64MultiArray
sensor_msgs/msg/JointState
rosgraph_msgs/msg/Clock

Note

To automatically append messages to the file interfaces.txt, you might want to use the following command:

cat << EOF >> $COPPELIASIM_ROOT_FOLDER/programming/ros2_packages/sim_ros2_interface/meta/interfaces.txt

In this way you can write the list of messages to be appended. Once all messages have been listed, write EOF on a new line and press enter to apply changes.

As example:

cat << EOF >> $COPPELIASIM_ROOT_FOLDER/programming/ros2_packages/sim_ros2_interface/meta/interfaces.txt
geometry_msgs/msg/Wrench
geometry_msgs/msg/WrenchStamped
std_msgs/msg/MultiArrayDimension
std_msgs/msg/MultiArrayLayout
std_msgs/msg/Float64MultiArray
sensor_msgs/msg/JointState
rosgraph_msgs/msg/Clock
EOF

Workspace build

After required messages have been configured, the ROS2 workspace must be built. This can be accomplished with the following command:

cd $COPPELIASIM_ROOT_FOLDER/programming/ros2_packages/sim_ros2_interface
colcon build --symlink-install

Note

In case of compilation errors, try the following command:

VERBOSE=1 MAKEFLAGS=-j1 colcon build --symlink-install --event-handlers console_direct+ --parallel-workers 1

ROS2 simulation interface

Coppelia does not provide a standard interface with the ROS simulator, but nodes must be programmed in the Coppelia environment to exchange data.

For this purpose, the preferred scripting language is LUA, which is faster than Python (that’s another alternative in Coppelia).

Scritps are bounded to objects in Coppelia: for this reason it is recommended to write code that belongs to the object itself, and not to the scene, so that if the entity is copied and pasted in other projects, the programmed interface is preserved.

Note

To see the list of all available API function, look here.

Robot simulation

This is an example scritp for a kinematic simulation of a robot in Coppelia that provide a good understanding of the interface principle. Here the code will be splitted up and explained in details; the whole script can be found here.

function table.fill(value, n)
    local t = {}
    for i = 1, n do
        table.insert(t, value)
    end
    return t
end

This is a simple used-defined function that allows to initialize a table with n equal elements value; this is useful, as example, for the initialization of the joint handles or maximum joint properties

q_home = {1.57, -1.57, 1.57 , -1.57, -1.57, 0.0}
joints = table.fill(nil, 6)
target = q_home
local max_vel = table.fill(180 * math.pi / 180, 6)
local max_acc = table.fill(40 * math.pi / 180, 6)
local max_jerk = table.fill(80 * math.pi / 180, 6)

Here it is reported the initialization of main variables; q_home is the home configuration of the robot (in the joint space), while joints is is a placeholder that will contain the Coppelia handles of the joints of the robot (i.e. the joint object inside the Coppelia object tree). For sake of simplicity, the initial target is set to the home configuration, while joint limits (for both velocity, acceleration and jerk) are initialized to values that are constant for all joints.

function apply_joint_position(ros_msg)
    target = ros_msg.data
    if(tostring(target[1]) == "nan") then
        return
    end
    msg_received = true
end

This function is the callback from the ROS2 subscriber (that will be initialized later); it takes the desired joint configuration for the system and sets the target accordingly. The msg_received boolean flag is used to assert when a new correct message is arrived. After processing of during the actuation step of the simulation, the flag will be set to false again.

function move_to_config(handles, max_vel, max_acc, max_jerk, target_pos)
    local current_pos = {}
    local current_vel = {}

    for i=1,#handles,1 do
        current_pos[i] = sim.getJointPosition(handles[i])
        current_vel[i] = sim.getJointVelocity(handles[i])
    end

    sim.moveToConfig(-1, current_pos, current_vel, nil, max_vel, max_acc, max_jerk,
                     target_pos, nil, mov_callback, handles)
end

function mov_callback(target_pos,vel,acc,handles)
    for i = 1, #handles, 1 do
            sim.setJointTargetPosition(handles[i],target_pos[i])
    end
end

This function implements the interface with coppelia that sets the target joint configuration for the kinematic simulation of the robot. The handles are the joints that are commanded with the specified joint limits. Internally the function collects the current state of the robot (i.e. joint position and velocity) and calls the built-in sim.moveToConfig documented here.

The mov_callback function is the callback that processes the result of the sim.moveToConfig.

function sysCall_init()
    if simROS2 then
        publisher = simROS2.createPublisher(
            '/coppelia_joint_states',
            'sensor_msgs/msg/JointState'
            )
        subscriber = simROS2.createSubscription(
            '/coppelia_set_joints',
            'std_msgs/msg/Float64MultiArray',
            'apply_joint_position'
            )
    end

    joints[1] = sim.getObject('/shoulder_pan_joint')
    joints[2] = sim.getObject('/shoulder_lift_joint')
    joints[3] = sim.getObject('/elbow_joint')
    joints[4] = sim.getObject('/wrist_1_joint')
    joints[5] = sim.getObject('/wrist_2_joint')
    joints[6] = sim.getObject('/wrist_3_joint')

    move_to_config(joints, max_vel, max_acc, max_jerk, target)
    msg_received = true
end

The sysCall_init function is the initialization function that is called automatically by Coppelia at the beginning of the simulation, thus is used to initialize different elements.

In this case a ROS2 topic subscriber and publisher are initialized; the former reads the desired joint configuration (that’s actually computer by a ROS controller) while the latter publishes the current joint configuration of the robot.

Then, the joints handler are initialized by binding the associated Coppelia objects. Furthermore, the robot is initially commanded to reach the home configuration.

function sysCall_actuation()
    if msg_received == true then
        move_to_config(joints, max_vel, max_acc, max_jerk, target)
        msg_received = false
    end
end

The sysCall_actuation is the function that is called at each actuation step of the simulation. In this case it checks if a new target position has been received from the publisher and, if so, it calls the move_to_config function to update the simulation target state.

function sysCall_sensing()
    local curr_pos = {}
    local curr_vel = {}
    local curr_torque = {}
    for i=1, #joints, 1 do
        curr_pos[i] = sim.getJointPosition(joints[i])
        curr_vel[i] = sim.getJointVelocity(joints[i])
        curr_torque[i] = sim.getJointForce(joints[i])
    end

    local msg ={
        header={
            stamp = simROS2.getSystemTime(),
            frame_id = 'robot_base'
        },
        name = {
            'shoulder_pan_joint', 'shoulder_lif_joint', 'elbow_joint',
            'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
        },
        position = curr_pos,
        velocity = curr_vel,
        effort = curr_torque
    }
    simROS2.publish(publisher, msg)
end

Finally, the sysCall_sensing function is called at each sensing step of the simulation; in this case it reads the current joint configuration and publishes it to the ROS2 topic as defined in the initialization.

ZeroMQ remote API

As described here Coppelia provides a remote API based on ZeroMQ. This can be used to control the flow of the simulation, and in particular it can call all regular API functions that are available within the Coppelia environment.

Installation

Coppelia provides a python package for accessing the ZeroMQ remote API that can be installed as follows:

python3 -m pip install coppeliasim-zmqremoteapi-client

Usage

The following is a simple python examples that ensures that a scene is loaded, it automatically starts the simulation and it waits until the simulation time reaches 10 seconds before stopping it.

from coppeliasim_zmqremoteapi_client import RemoteAPIClient
client = RemoteAPIClient()
sim = client.require("sim")

sim.loadScene("/path/to/scene.ttt")
sim.startSimulation()

while sim.getSimulationTime() < 10:
    pass

sim.stopSimulation()

Tips and Tricks

Automatic simulation start

One can automatically boot up the Coppeliasim simulator on a given scene calling

$COPPELIASIM_ROOT_DIR/coppeliaSim.sh /path/to/scene.ttt

This operation can be performed also inside python, for instance when we want to start the simulator a launch file. We can check if the simulator is already running by checking the active processes on the system using the psutil library (that can be installed with pip3 install psutil):

import psutil

is_running = "coppeliaSim" in (p.name() for p in psutil.process_iter())

Using the subprocess python package one can also execute the command that boots the simulator with

import os
import subprocess

coppeliasim_dir = os.environ["COPPELIASIM_ROOT_DIR"]
scene = "/path/to/scene.ttt"
subprocess.Popen([f"{coppeliasim_dir}/coppeliaSim.sh", scene])

Combining the 2 scripts one could do

import os
import psutil
import subprocess

if "coppeliaSim" not in (p.name() for p in psutil.process_iter()):
    coppeliasim_dir = os.environ["COPPELIASIM_ROOT_DIR"]
    scene = "/path/to/scene.ttt"
    subprocess.Popen([f"{coppeliasim_dir}/coppeliaSim.sh", scene],
                     stdout=subprocess.DEVNULL,
                     stderr=subprocess.DEVNULL)