External control of Quadcopter through ZMQ Remote API and C++

[caioguimaraes]

Hi, I'm new to CoppeliaSIM and I'm trying to simulate drone control techniques using the Quadcopter model. My methodology is adding a quadcopter to a new scene, deleting the built-in child script of the model and using cppzmq and the ZMQ RemoteAPI to control it. I'm using the following code in C++:

Code: Select all

#include <algorithm>
#include <array>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

#define SIM_REMOTEAPICLIENT_OBJECTS
#include "RemoteAPIClient.h"

static constexpr double RAD_TO_DEG = 180.0 / M_PI;
static constexpr double DT = 0.005;

struct Tee {
  std::ofstream file;
  std::string filename;
  void open() {
    auto now = std::chrono::system_clock::now();
    std::time_t t = std::chrono::system_clock::to_time_t(now);
    std::tm tm = *std::localtime(&t);
    std::ostringstream oss;
    oss << std::put_time(&tm, "%Y%m%d_%H%M%S") << ".log";
    filename = oss.str();
    file.open(filename);
    if (!file.is_open())
      std::cerr << "[WARNING] Could not open log file: " << filename << "\n";
    else
      std::cout << "[+] Logging to: " << filename << "\n";
  }
  void write(const std::string &s) {
    std::cout << s;
    if (file.is_open())
      file << s;
  }
  void flush() {
    std::cout.flush();
    if (file.is_open())
      file.flush();
  }
} tee;

#define LOG(...)                                                               \
  do {                                                                         \
    char _buf[1024];                                                           \
    snprintf(_buf, sizeof(_buf), __VA_ARGS__);                                 \
    tee.write(std::string(_buf));                                              \
  } while (0)

struct State {
  double x, y, z;
  double roll, pitch, yaw;
  double vx, vy, vz;
  double wx, wy, wz;
  std::array<double, 12> matrix = {};
  std::string toString() const {
    char buf[512];
    snprintf(buf, sizeof(buf),
             "Pos   : [%7.3f, %7.3f, %7.3f] m\n"
             "Orient: [%7.2f, %7.2f, %7.2f] deg\n"
             "LinVel: [%7.3f, %7.3f, %7.3f] m/s\n"
             "AngVel: [%7.3f, %7.3f, %7.3f] rad/s\n"
             "─────────────────────────────────────\n",
             x, y, z, roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG,
             vx, vy, vz, wx, wy, wz);
    return std::string(buf);
  }
};

static constexpr double K_T = 1.0e-7;
static constexpr std::array<double, 4> MOTOR_SIGNS = {+1.0, -1.0, +1.0, -1.0};

struct Quadcopter {
  int64_t body;
  std::array<int64_t, 4> propRespondables;
  std::array<int64_t, 4> propBodies;
  std::array<int64_t, 4> joints;

  static Quadcopter init(RemoteAPIObject::sim &sim) {
    Quadcopter q;
    q.body = sim.getObject("/Quadcopter");
    if (q.body < 0) {
      LOG("[ERROR] /Quadcopter not found.\n");
      std::exit(1);
    }
    const std::array<std::string, 4> respPaths = {
        "/Quadcopter/propeller[0]/respondable",
        "/Quadcopter/propeller[1]/respondable",
        "/Quadcopter/propeller[2]/respondable",
        "/Quadcopter/propeller[3]/respondable"};
    const std::array<std::string, 4> bodyPaths = {
        "/Quadcopter/propeller[0]", "/Quadcopter/propeller[1]",
        "/Quadcopter/propeller[2]", "/Quadcopter/propeller[3]"};
    const std::array<std::string, 4> jointPaths = {
        "/Quadcopter/joint", "/Quadcopter/propeller[1]/joint",
        "/Quadcopter/propeller[2]/joint", "/Quadcopter/propeller[3]/joint"};
    for (int i = 0; i < 4; ++i) {
      q.propRespondables[i] = sim.getObject(respPaths[i].c_str());
      q.propBodies[i] = sim.getObject(bodyPaths[i].c_str());
      q.joints[i] = sim.getObject(jointPaths[i].c_str());
      if (q.propRespondables[i] < 0 || q.propBodies[i] < 0) {
        LOG("[ERROR] Invalid handle for motor %d resp=%ld body=%ld\n", i,
            q.propRespondables[i], q.propBodies[i]);
        std::exit(1);
      }
    }
    LOG("[+] Quadcopter body: %ld\n", q.body);
    for (int i = 0; i < 4; ++i)
      LOG("[+] Motor [%d]  resp=%ld  body=%ld  joint=%ld\n", i,
          q.propRespondables[i], q.propBodies[i], q.joints[i]);
    return q;
  }

  State getState(RemoteAPIObject::sim &sim) const {
    auto pos = sim.getObjectPosition(body, sim.handle_world);
    auto orient = sim.getObjectOrientation(body, sim.handle_world);
    auto [linVel, angVel] = sim.getObjectVelocity(body);
    auto mat = sim.getObjectMatrix(body, sim.handle_world);
    State s;
    s.x = pos[0];
    s.y = pos[1];
    s.z = pos[2];
    s.roll = orient[0];
    s.pitch = orient[1];
    s.yaw = orient[2];
    s.vx = linVel[0];
    s.vy = linVel[1];
    s.vz = linVel[2];
    s.wx = angVel[0];
    s.wy = angVel[1];
    s.wz = angVel[2];
    for (int i = 0; i < 12; ++i)
      s.matrix[i] = mat[i];
    return s;
  }

  void setMotorForces(RemoteAPIObject::sim &sim,
                      const std::array<double, 4> &thrusts) const {
    for (int i = 0; i < 4; ++i) {
      auto m = sim.getObjectMatrix(propBodies[i], sim.handle_world);
      m[3] = 0.0;
      m[7] = 0.0;
      m[11] = 0.0;
      auto force =
          sim.multiplyVector(m, std::vector<double>{0.0, 0.0, thrusts[i]});
      double tmag = MOTOR_SIGNS[i] * K_T * thrusts[i];
      auto torque = sim.multiplyVector(m, std::vector<double>{0.0, 0.0, tmag});
      sim.addForceAndTorque(propRespondables[i], force, torque);
    }
  }
};

struct Controller {
  double target_x = 0.0, target_y = 0.0, target_z = 1.0, target_yaw = 0.0;
  double kp_z = 2.0, ki_z = 0.0, kd_z = 0.0;
  double z_integral = 0.0, z_prevErr = 0.0;
  double kp_tilt = 0.25, kd_tilt = 8.0; // increased from 2.1
  double kp_pos = 0.005, kd_pos = 0.0;
  double kp_yaw = 0.1, kd_yaw = 2.0;
  static constexpr double BASE_THRUST = 1.275;
  static constexpr double MIN_THRUST = 0.4;
  static constexpr double MAX_THRUST = 10.0;
  double pAlphaE = 0.0, pBetaE = 0.0;
  double psp_x = 0.0, psp_y = 0.0;
  double prevYaw = 0.0;

  std::array<double, 4> compute(const State &s) {
    double ez = target_z - s.z;
    z_integral += ez * DT;
    z_integral = std::clamp(z_integral, -0.3, 0.3);
    double thrust = BASE_THRUST + kp_z * ez + ki_z * z_integral +
                    kd_z * (ez - z_prevErr) / DT - 2.0 * s.vz;
    z_prevErr = ez;

    double alphaE = s.matrix[6]; // world-Z of body-Y: zero when level
    double betaE = s.matrix[2];  // world-Z of body-X: zero when level

    double ex_world = target_x - s.x;
    double ey_world = target_y - s.y;
    double cy = std::cos(s.yaw), sy = std::sin(s.yaw);
    double sp_x = cy * ex_world + sy * ey_world;
    double sp_y = -sy * ex_world + cy * ey_world;

    double alphaCorr = kp_tilt * alphaE + kd_tilt * (alphaE - pAlphaE);
    double betaCorr = -kp_tilt * betaE - kd_tilt * (betaE - pBetaE);
    pAlphaE = alphaE;
    pBetaE = betaE;

    alphaCorr += kp_pos * sp_y + kd_pos * (sp_y - psp_y);
    betaCorr -= kp_pos * sp_x + kd_pos * (sp_x - psp_x);
    psp_x = sp_x;
    psp_y = sp_y;

    double yawErr = target_yaw - s.yaw;
    while (yawErr > M_PI)
      yawErr -= 2.0 * M_PI;
    while (yawErr < -M_PI)
      yawErr += 2.0 * M_PI;
    double rotCorr = kp_yaw * yawErr + kd_yaw * (yawErr - prevYaw);
    prevYaw = yawErr;

    std::array<double, 4> t = {thrust * (1.0 - alphaCorr + betaCorr + rotCorr),
                               thrust * (1.0 - alphaCorr - betaCorr - rotCorr),
                               thrust * (1.0 + alphaCorr - betaCorr + rotCorr),
                               thrust * (1.0 + alphaCorr + betaCorr - rotCorr)};
    for (auto &v : t)
      v = std::clamp(v, MIN_THRUST, MAX_THRUST);
    return t;
  }
};

struct DeadlockGuard {
  double lastTime = -1.0;
  int stallCount = 0;
  static constexpr int MAX_STALL = 10;
  bool check(double t) {
    if (t <= lastTime + 1e-9) {
      if (++stallCount >= MAX_STALL) {
        LOG("[DEADLOCK] sim_time frozen at %.4fs. Aborting.\n", t);
        return true;
      }
    } else {
      stallCount = 0;
    }
    lastTime = t;
    return false;
  }
};

bool isCrashed(const State &s) {
  if (s.z < 0.05) {
    LOG("[CRASH] z=%.3f below floor threshold.\n", s.z);
    return true;
  }
  if (std::abs(s.roll) > 60.0 * M_PI / 180.0 ||
      std::abs(s.pitch) > 60.0 * M_PI / 180.0) {
    LOG("[CRASH] roll=%.2f pitch=%.2f exceeded 60 deg.\n", s.roll * RAD_TO_DEG,
        s.pitch * RAD_TO_DEG);
    return true;
  }
  return false;
}

int main() {
  tee.open();
  RemoteAPIClient client("localhost", 23000, -1, -1);
  RemoteAPIObject::sim sim = client.getObject().sim();
  LOG("[+] Connected to CoppeliaSim\n");
  sim.setStepping(true);
  sim.startSimulation();
  client.step();

  Quadcopter quad = Quadcopter::init(sim);
  for (int i = 0; i < 4; i++) {
    auto p = sim.getObjectPosition(quad.propBodies[i], sim.handle_world);
    LOG("[PROPPOS] motor %d  x=%.3f  y=%.3f  z=%.3f\n", i, p[0], p[1], p[2]);
  }

  State s0 = quad.getState(sim);
  Controller ctrl;
  ctrl.pAlphaE = s0.matrix[6];
  ctrl.pBetaE = s0.matrix[2];
  DeadlockGuard dg;
  LOG("[+] Simulation started (stepping mode, DT=%.0fms)\n\n", DT * 1000.0);

  for (int i = 0; i < 6000; ++i) {
    double t = sim.getSimulationTime();
    if (dg.check(t))
      break;
    State s = quad.getState(sim);
    if (isCrashed(s))
      break;
    if (i % 20 == 0) {
      LOG("=== Step %d | sim_time=%.3f s ===\n", i, t);
      tee.write(s.toString());
    }
    auto thrusts = ctrl.compute(s);
    if (i % 20 == 0)
      LOG("[THRUST] %.3f  %.3f  %.3f  %.3f\n", thrusts[0], thrusts[1],
          thrusts[2], thrusts[3]);
    quad.setMotorForces(sim, thrusts);
    client.step();
    tee.flush();
  }

  sim.stopSimulation();
  client.step();
  LOG("[+] Done. Log: %s\n", tee.filename.c_str());
  return 0;
}

Simulation and Dynamics steps are set to 5ms. In essence, I'd like help in understanding what I'm doing wrong, since the Quadcopter hovers nicely for a few ms and then tilts in roll over the X axis and can't recover no matter what I do regarding the PID parameters. Obs.: I'm not asking for help in making everything work. I meant regarding the framework I'm using or the way I'm applying forces and torques in the propellers joints. Thank you very much!

Here is a log

Code: Select all

./drone_controller
[+] Logging to: 20260317_111142.log
[+] Connected to CoppeliaSim
[+] Quadcopter body: 15
[+] Motor [0]  resp=18  body=17  joint=20
[+] Motor [1]  resp=23  body=22  joint=25
[+] Motor [2]  resp=28  body=27  joint=30
[+] Motor [3]  resp=33  body=32  joint=35
[PROPPOS] motor 0  x=0.092  y=0.092  z=0.990
[PROPPOS] motor 1  x=-0.092  y=0.092  z=0.990
[PROPPOS] motor 2  x=-0.092  y=-0.092  z=0.990
[PROPPOS] motor 3  x=0.092  y=-0.092  z=0.990
[+] Simulation started (stepping mode, DT=5ms)

=== Step 0 | sim_time=0.005 s ===
Pos   : [  0.000,  -0.000,   1.000] m
Orient: [  -0.00,   -0.00,    0.00] deg
LinVel: [  0.000,  -0.000,  -0.049] m/s
AngVel: [ -0.000,  -0.000,   0.000] rad/s
─────────────────────────────────────
[THRUST] 1.374  1.374  1.374  1.374
=== Step 20 | sim_time=0.105 s ===
Pos   : [  0.000,  -0.000,   0.998] m
Orient: [   0.07,    0.00,   -0.01] deg
LinVel: [  0.000,  -0.001,  -0.009] m/s
AngVel: [  0.009,   0.000,  -0.000] rad/s
─────────────────────────────────────
[THRUST] 1.299  1.299  1.298  1.298
=== Step 40 | sim_time=0.205 s ===
Pos   : [  0.000,  -0.000,   0.997] m
Orient: [   0.16,    0.00,   -0.01] deg
LinVel: [  0.000,  -0.003,   0.000] m/s
AngVel: [  0.020,   0.000,   0.000] rad/s
─────────────────────────────────────
[THRUST] 1.282  1.282  1.278  1.278
=== Step 60 | sim_time=0.305 s ===
Pos   : [  0.000,  -0.001,   0.997] m
Orient: [   0.34,    0.01,   -0.01] deg
LinVel: [  0.000,  -0.007,   0.002] m/s
AngVel: [  0.047,   0.001,   0.000] rad/s
─────────────────────────────────────
[THRUST] 1.280  1.280  1.272  1.272
=== Step 80 | sim_time=0.405 s ===
Pos   : [  0.000,  -0.002,   0.998] m
Orient: [   0.76,    0.02,   -0.01] deg
LinVel: [  0.000,  -0.015,   0.002] m/s
AngVel: [  0.105,   0.002,   0.000] rad/s
─────────────────────────────────────
[THRUST] 1.285  1.285  1.266  1.265
=== Step 100 | sim_time=0.505 s ===
Pos   : [  0.000,  -0.004,   0.998] m
Orient: [   1.71,    0.03,   -0.01] deg
LinVel: [  0.001,  -0.035,   0.002] m/s
AngVel: [  0.237,   0.004,  -0.000] rad/s
─────────────────────────────────────
[THRUST] 1.297  1.297  1.254  1.253
=== Step 120 | sim_time=0.605 s ===
Pos   : [  0.000,  -0.010,   0.998] m
Orient: [   3.85,    0.06,   -0.01] deg
LinVel: [  0.001,  -0.079,   0.001] m/s
AngVel: [  0.534,   0.008,  -0.000] rad/s
─────────────────────────────────────
[THRUST] 1.324  1.326  1.229  1.227
=== Step 140 | sim_time=0.705 s ===
Pos   : [  0.000,  -0.022,   0.998] m
Orient: [   8.67,    0.13,   -0.02] deg
LinVel: [  0.003,  -0.177,  -0.001] m/s
AngVel: [  1.201,   0.016,  -0.000] rad/s
─────────────────────────────────────
[THRUST] 1.390  1.392  1.174  1.171
=== Step 160 | sim_time=0.805 s ===
Pos   : [  0.001,  -0.050,   0.997] m
Orient: [  19.47,    0.26,   -0.06] deg
LinVel: [  0.006,  -0.400,  -0.015] m/s
AngVel: [  2.692,   0.032,  -0.000] rad/s
─────────────────────────────────────
[THRUST] 1.550  1.555  1.071  1.065
=== Step 180 | sim_time=0.905 s ===
Pos   : [  0.002,  -0.113,   0.993] m
Orient: [  43.48,    0.47,   -0.21] deg
LinVel: [  0.013,  -0.911,  -0.086] m/s
AngVel: [  5.917,   0.064,  -0.001] rad/s
─────────────────────────────────────
[THRUST] 1.963  1.971  0.963  0.950
[CRASH] roll=61.93 pitch=0.57 exceeded 60 deg.
[+] Done. Log: 20260317_111142.log

[caioguimaraes]

Here is the CMakeLists.txt I'm using to build the application:

Code: Select all

cmake_minimum_required(VERSION 3.15)
project(drone_formation CXX)

set(CMAKE_CXX_STANDARD 17)

# ── CoppeliaSim paths ─────────────────────────────────────────
set(COPPELIASIM_ROOT "/opt/CoppeliaSim")
set(ZMQREMOTE_CPP   "${COPPELIASIM_ROOT}/programming/zmqRemoteApi/clients/cpp")

if(NOT EXISTS "${ZMQREMOTE_CPP}/RemoteAPIClient.h")
    message(FATAL_ERROR
        "RemoteAPIClient.h not found at ${ZMQREMOTE_CPP}\n"
        "Have you built the client library?\n"
        "  cd ${ZMQREMOTE_CPP} && mkdir build && cd build\n"
        "  cmake -DCOPPELIASIM_ROOT_DIR=${COPPELIASIM_ROOT} .. && cmake --build .")
endif()

if(NOT EXISTS "${ZMQREMOTE_CPP}/build/libRemoteAPIClient.a")
    message(FATAL_ERROR
        "libRemoteAPIClient.a not found at ${ZMQREMOTE_CPP}/build\n"
        "Build it first:\n"
        "  cd ${ZMQREMOTE_CPP}/build\n"
        "  cmake -DCOPPELIASIM_ROOT_DIR=${COPPELIASIM_ROOT} .. && cmake --build .")
endif()

# ── ZeroMQ ────────────────────────────────────────────────────
# sudo dnf install zeromq-devel cppzmq-devel
find_package(PkgConfig REQUIRED)
pkg_check_modules(ZMQ REQUIRED libzmq)
find_package(cppzmq REQUIRED)

# ── Controller binary ─────────────────────────────────────────
add_executable(drone_controller controller/main.cpp)

target_include_directories(drone_controller PRIVATE
    ${ZMQREMOTE_CPP}
    ${ZMQREMOTE_CPP}/build/jsoncons/include
    ${ZMQ_INCLUDE_DIRS}
)

target_link_libraries(drone_controller PRIVATE
    ${ZMQREMOTE_CPP}/build/libRemoteAPIClient.a
    ${ZMQ_LIBRARIES}
    cppzmq
)

I'm using Fedora 43, gcc (GCC) 15.2.1 20260123 (Red Hat 15.2.1-7), and cmake version 3.31.10