Trajectory Joint Interface example

At first, we create a UrDriver object as usual:

Listing 68 examples/trajectory_point_interface.cpp

  bool headless_mode = true;
  g_my_robot = std::make_unique<urcl::ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                           "external_control.urp");
  if (!g_my_robot->isHealthy())
  {
    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
    return 1;
  }

We use a small helper function to make sure that the reverse interface is active and connected before proceeding.

Initialization

As trajectory execution will be triggered asynchronously, we define a callback function to handle a finished trajectory. A trajectory is considered finished when the robot is no longer executing it, independent of whether it successfully reached its final point. The trajectory result will reflect whether it was executed successfully, was canceled upon request or failed for some reason.

Listing 69 examples/trajectory_point_interface.cpp

void trajDoneCallback(const urcl::control::TrajectoryResult& result)
{
  URCL_LOG_INFO("Trajectory done with result %s", urcl::control::trajectoryResultToString(result).c_str());
  g_trajectory_done = true;
}

That callback can be registered at the UrDriver object:

Listing 70 examples/trajectory_point_interface.cpp

  g_my_robot->getUrDriver()->registerTrajectoryDoneCallback(&trajDoneCallback);

MoveJ Trajectory

Then, in order to execute a trajectory, we need to define a trajectory as a sequence of points and parameters. The following example shows execution of a 2-point trajectory using URScript’s movej function:

Listing 71 examples/trajectory_point_interface.cpp

  {
    g_trajectory_done = false;
    // Trajectory definition
    std::vector<urcl::vector6d_t> points{ { -1.57, -1.57, 0, 0, 0, 0 }, { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } };
    std::vector<double> motion_durations{ 5.0, 2.0 };
    std::vector<double> velocities{ 2.0, 2.3 };
    std::vector<double> accelerations{ 2.5, 2.5 };
    std::vector<double> blend_radii{ 0.1, 0.1 };

    // Trajectory execution
    g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
                                                             static_cast<int>(points.size() * 2));
    for (size_t i = 0; i < points.size(); i++)
    {
      g_my_robot->getUrDriver()->writeTrajectoryPoint(points[i], false, static_cast<float>(motion_durations[i]),
                                                      static_cast<float>(blend_radii[i]));
    }

    // Same motion, but parametrized with acceleration and velocity
    motion_durations = { 0.0, 0.0 };
    for (size_t i = 0; i < points.size(); i++)
    {
      g_my_robot->getUrDriver()->writeTrajectoryPoint(
          points[i], static_cast<float>(accelerations[i]), static_cast<float>(velocities[i]), false,
          static_cast<float>(motion_durations[i]), static_cast<float>(blend_radii[i]));
    }

    while (!g_trajectory_done)
    {
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
          urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
    }
  }

In fact, the path is followed twice, once parametrized by a segment duration and once using maximum velocity / acceleration settings. If a duration > 0 is given for a segment, the velocity and acceleration settings will be ignored as in the underlying URScript functions. In the example above, each of the g_my_robot->getUrDriver()->writeTrajectoryPoint() calls will be translated into a movej command in URScript.

While the trajectory is running, we need to tell the robot program that connection is still active and we expect the trajectory to be running. This is being done by the g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); call.

MoveL Trajectory

Similar to the movej-based trajectory, execution can be done interpolating in joint space:

Listing 72 examples/trajectory_point_interface.cpp

  {
    g_trajectory_done = false;
    // Trajectory definition
    std::vector<urcl::vector6d_t> points{ { -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 },
                                          { -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 } };
    std::vector<double> motion_durations{ 5.0, 5.0 };
    std::vector<double> velocities{ 2.0, 2.3 };
    std::vector<double> accelerations{ 2.5, 2.5 };
    std::vector<double> blend_radii{ 0.0, 0.0 };

    // Trajectory execution of the path that goes through the points twice.
    g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
                                                             static_cast<int>(points.size() * 2));
    for (size_t i = 0; i < points.size(); i++)
    {
      // setting the cartesian parameter makes it interpret the 6d vector as a pose and use movel
      g_my_robot->getUrDriver()->writeTrajectoryPoint(points[i], true, static_cast<float>(motion_durations[i]),
                                                      static_cast<float>(blend_radii[i]));
    }

    // Same motion, but parametrized with acceleration and velocity
    motion_durations = { 0.0, 0.0 };
    for (size_t i = 0; i < points.size(); i++)
    {
      g_my_robot->getUrDriver()->writeTrajectoryPoint(
          points[i], static_cast<float>(accelerations[i]), static_cast<float>(velocities[i]), true,
          static_cast<float>(motion_durations[i]), static_cast<float>(blend_radii[i]));
    }

    while (!g_trajectory_done)
    {
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
          urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
    }
  }

Spline based interpolation

Similar to the Spline Interpolation example, the trajectory point interface can be used to execute motions using the spline interpolation:

Listing 73 examples/trajectory_point_interface.cpp

  {
    g_trajectory_done = false;
    // Trajectory definition
    std::vector<urcl::vector6d_t> positions{ { -1.57, -1.57, 0, 0, 0, 0 },
                                             { -1.57, -1.57, -1.57, 0, 0, 0 },
                                             { -1.57, -1.57, 0, 0, 0, 0 },
                                             { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } };
    std::vector<urcl::vector6d_t> velocities{
      { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 1.5, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
    };
    std::vector<double> motion_durations{ 3.0, 3.0, 3.0, 3.0 };

    // Trajectory execution
    g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
                                                             static_cast<int>(positions.size()));
    for (size_t i = 0; i < positions.size(); i++)
    {
      g_my_robot->getUrDriver()->writeTrajectorySplinePoint(positions[i], velocities[i],
                                                            static_cast<float>(motion_durations[i]));
    }

    while (!g_trajectory_done)
    {
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
          urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
    }
  }