How to Implement MoveL: Cartesian Linear Motion with IK

In the previous article, we built the mental model for MoveJ versus MoveL: MoveJ interpolates in joint space, while MoveL tries to keep the tool center point, or TCP, on a straight Cartesian line. This article implements the piece that a classical controller actually needs: take the current pose T0 and a target pose T1, discretize the Cartesian line into waypoints, interpolate orientation with quaternion SLERP, solve IK at every waypoint, then run FK again to verify that the TCP path stayed straight.

If you are joining the series here, review robot modeling and FK, inverse kinematics, and MoveJ/MoveL in industrial programming. A useful non-series companion is Simulation for Robotics: MuJoCo vs Isaac Sim vs Gazebo, because MoveL should be tested in simulation before it touches real hardware.

What does MoveL actually promise?

In Universal Robots-style documentation, MoveL is described as a command where the tool/TCP moves in a straight line; to keep the tool linear, each joint may need to perform a more complicated motion than simple joint interpolation. MoveIt uses the same practical idea: when a Cartesian path is requested, the end effector attempts to move linearly in Cartesian space. Industrial tasks such as welding, dispensing glue, surface treatment, probing, inserting a pin, or lowering a vacuum gripper along Z rely on this promise.

The common beginner mistake is to assume that MoveL means every joint moves linearly. Usually it is the opposite. If the TCP is constrained to a straight line, the joints may trace curved profiles, change speed at different rates, or even reverse a wrist joint to avoid an orientation error. Therefore, a controller cannot simply do this:

q(s) = q0 + s * (q1 - q0)

That formula is a geometric MoveJ. For MoveL, the interpolation is done on pose:

p(s) = p0 + s * (p1 - p0)
R(s) = SLERP(R0, R1, s)
T(s) = [R(s), p(s)]
q(s) = IK(T(s), seed = q(s - ds))

Here s runs from 0 to 1. The position part p(s) is a linear interpolation in the base or world frame. The orientation part should not be interpolated by separately lerping roll, pitch, and yaw. Euler angles wrap, hit singular cases, and can command a long rotation when the short rotation is only a few degrees away. Unit quaternions and SLERP are the standard robust option. Drake provides PiecewiseQuaternionSlerp, and Eigen exposes QuaternionBase::slerp(t, other). One important detail: q and -q represent the same orientation, so before SLERP you should choose the sign that makes the quaternion dot product non-negative; otherwise the robot may rotate along the long way around.

Path geometry is not the same as trajectory timing

This article focuses on the geometric path first. A path answers: which TCP poses should the robot pass through? A trajectory adds: at what time, with what velocity and acceleration? The API moveL(pose_target, v, a) includes speed and acceleration, but for a beginner-friendly implementation we split the problem into layers:

In production controllers, time parameterization can become the hardest layer because a Cartesian speed must respect joint velocity and acceleration limits through the Jacobian. In this article we keep v and a in the API so the architecture is correct, but the minimal implementation uses them only as command metadata and focuses on path correctness. Later motion-profile and blending articles can make timing fully dynamic.

Choosing the discretization step ds

If waypoints are too sparse, the low-level controller may interpolate joints between two IK solutions and the real TCP segment can bow away from the intended line. If waypoints are too dense, IK becomes slow and numerical noise can accumulate. A practical rule is:

N_pos = ceil(||p1 - p0|| / max_step_m)
N_rot = ceil(angle(R0^-1 R1) / max_step_rad)
N = max(N_pos, N_rot, 2)

For a learning-grade 6-DOF arm, max_step_m = 0.005 to 0.02 m is reasonable in simulation. For orientation, max_step_rad = 2 to 5 degrees avoids large wrist jumps. On real hardware, begin much slower, restrict the workspace, enable emergency stop access, and do not trust a path until FK and a visualizer confirm it.

At every waypoint, IK should use the previous waypoint's solution as the seed. This is the main trick that keeps the IK branch continuous. If every IK call starts from the same fixed seed, the solver may jump from elbow-up to elbow-down in the middle of the path. The TCP could still be mathematically correct, but the joint path would jerk badly. The IK solver from part 5 already includes damped least squares and joint-limit handling; MoveL wraps it in a waypoint loop and adds continuity checks.

Python prototype: pose lerp, SLERP, and straightness check

The prototype below does not require ROS. It assumes you already have fk(q) and ik(T, seed) from earlier articles. If you do not have a full 6-DOF arm yet, you can mock ik or use a 2-link model to understand the pipeline. The important part is the data flow and the straight-line error metric.

import numpy as np
from dataclasses import dataclass
from scipy.spatial.transform import Rotation as R, Slerp

@dataclass
class Pose:
    p: np.ndarray      # shape (3,)
    q: np.ndarray      # quaternion xyzw, shape (4,)

def make_transform(pose: Pose) -> np.ndarray:
    T = np.eye(4)
    T[:3, :3] = R.from_quat(pose.q).as_matrix()
    T[:3, 3] = pose.p
    return T

def interpolate_pose(start: Pose, goal: Pose, max_step_m=0.01, max_step_deg=3.0):
    distance = np.linalg.norm(goal.p - start.p)
    r0 = R.from_quat(start.q)
    r1 = R.from_quat(goal.q)
    rot_angle = (r0.inv() * r1).magnitude()

    n_pos = int(np.ceil(distance / max_step_m)) if distance > 1e-12 else 1
    n_rot = int(np.ceil(rot_angle / np.deg2rad(max_step_deg))) if rot_angle > 1e-12 else 1
    n = max(n_pos, n_rot, 2)

    s_values = np.linspace(0.0, 1.0, n + 1)
    positions = start.p[None, :] + s_values[:, None] * (goal.p - start.p)[None, :]

    slerp = Slerp([0.0, 1.0], R.concatenate([r0, r1]))
    rotations = slerp(s_values)

    return [Pose(p=positions[i], q=rotations[i].as_quat()) for i in range(len(s_values))]

def line_error(points, p0, p1):
    points = np.asarray(points)
    d = p1 - p0
    length = np.linalg.norm(d)
    if length < 1e-12:
        return 0.0
    u = d / length
    errors = []
    for p in points:
        projection = p0 + np.dot(p - p0, u) * u
        errors.append(np.linalg.norm(p - projection))
    return float(np.max(errors))

def plan_movel(q_start, pose_goal, fk, ik, max_step_m=0.01):
    T0 = fk(q_start)
    pose_start = Pose(
        p=T0[:3, 3],
        q=R.from_matrix(T0[:3, :3]).as_quat(),
    )

    waypoints = interpolate_pose(pose_start, pose_goal, max_step_m=max_step_m)
    q_path = [np.asarray(q_start, dtype=float)]
    tcp_points = [pose_start.p]

    seed = q_path[0]
    for i, pose in enumerate(waypoints[1:], start=1):
        T_target = make_transform(pose)
        sol = ik(T_target, seed=seed)
        if not sol.success:
            raise RuntimeError(f"IK failed at waypoint {i}/{len(waypoints)-1}: {sol.reason}")
        q_path.append(sol.q)
        seed = sol.q

        T_check = fk(sol.q)
        tcp_points.append(T_check[:3, 3])

    err = line_error(tcp_points, pose_start.p, pose_goal.p)
    return np.asarray(q_path), np.asarray(tcp_points), err

Once you have q_path and tcp_points, plot the TCP path. Beginners often plot only joint curves, but MoveL must be judged in Cartesian space.

import matplotlib.pyplot as plt

q_path, tcp, max_err = plan_movel(q0, pose_goal, fk=robot.fk, ik=robot.ik)

fig = plt.figure(figsize=(8, 6))
ax = fig.add_subplot(111, projection="3d")
ax.plot(tcp[:, 0], tcp[:, 1], tcp[:, 2], "o-", label="FK of IK waypoints")
ax.plot([tcp[0, 0], tcp[-1, 0]], [tcp[0, 1], tcp[-1, 1]], [tcp[0, 2], tcp[-1, 2]], "--", label="ideal line")
ax.set_title(f"MoveL TCP straightness, max error = {max_err * 1000:.2f} mm")
ax.set_xlabel("x [m]")
ax.set_ylabel("y [m]")
ax.set_zlabel("z [m]")
ax.legend()
plt.show()

A minimal pass checklist:

If the line error is large, the cause is usually not the formula for p(s). It is more often loose IK convergence, a mismatch between FK and IK tool frames, a wrong TCP offset, or a controller executing joint interpolation between waypoints that are too far apart.

C++ skeleton: moveL(pose_target, v, a)

The C++ version below assumes you already have a RobotModel class with fk(q) and jacobian(q), plus the IKSolver from part 5. We use Eigen::Isometry3d for poses and Eigen::Quaterniond for orientation. The API returns joint waypoints; an executor or time parameterizer can process them later.

#pragma once
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <stdexcept>
#include <vector>

struct MoveLOptions {
    double tcp_speed = 0.05;       // m/s, used for timing metadata later
    double tcp_accel = 0.25;       // m/s^2
    double max_step = 0.01;        // m
    double max_rot_step = 3.0 * M_PI / 180.0;
    double line_tolerance = 0.002; // m
};

struct IKResult {
    bool success = false;
    Eigen::VectorXd q;
    double residual = 0.0;
    int iterations = 0;
    const char* reason = "";
};

struct MoveLResult {
    std::vector<Eigen::VectorXd> q_path;
    std::vector<Eigen::Isometry3d> cartesian_waypoints;
    double max_line_error = 0.0;
};

class RobotModel {
public:
    Eigen::Isometry3d fk(const Eigen::VectorXd& q) const;
    Eigen::MatrixXd jacobian(const Eigen::VectorXd& q) const;
};

class IKSolver {
public:
    IKResult solve(const Eigen::Isometry3d& target, const Eigen::VectorXd& seed) const;
};

Pose interpolation:

static std::vector<Eigen::Isometry3d> interpolateCartesian(
    const Eigen::Isometry3d& start,
    const Eigen::Isometry3d& goal,
    const MoveLOptions& opt) {

    const Eigen::Vector3d p0 = start.translation();
    const Eigen::Vector3d p1 = goal.translation();
    const double dist = (p1 - p0).norm();

    Eigen::Quaterniond q0(start.rotation());
    Eigen::Quaterniond q1(goal.rotation());
    q0.normalize();
    q1.normalize();

    if (q0.dot(q1) < 0.0) {
        q1.coeffs() *= -1.0;
    }

    const double angle = q0.angularDistance(q1);
    const int n_pos = std::max(1, static_cast<int>(std::ceil(dist / opt.max_step)));
    const int n_rot = std::max(1, static_cast<int>(std::ceil(angle / opt.max_rot_step)));
    const int n = std::max({n_pos, n_rot, 2});

    std::vector<Eigen::Isometry3d> out;
    out.reserve(n + 1);

    for (int i = 0; i <= n; ++i) {
        const double s = static_cast<double>(i) / static_cast<double>(n);
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        T.translation() = p0 + s * (p1 - p0);
        T.linear() = q0.slerp(s, q1).toRotationMatrix();
        out.push_back(T);
    }
    return out;
}

Straight-line error measured after FK:

static double maxLineError(
    const std::vector<Eigen::Vector3d>& points,
    const Eigen::Vector3d& p0,
    const Eigen::Vector3d& p1) {

    const Eigen::Vector3d d = p1 - p0;
    const double length = d.norm();
    if (length < 1e-12) return 0.0;

    const Eigen::Vector3d u = d / length;
    double max_err = 0.0;
    for (const auto& p : points) {
        const Eigen::Vector3d proj = p0 + (p - p0).dot(u) * u;
        max_err = std::max(max_err, (p - proj).norm());
    }
    return max_err;
}

The main moveL wrapper:

MoveLResult moveL(
    const RobotModel& robot,
    const IKSolver& ik,
    const Eigen::VectorXd& q_start,
    const Eigen::Isometry3d& pose_target,
    const MoveLOptions& opt) {

    const Eigen::Isometry3d pose_start = robot.fk(q_start);
    auto waypoints = interpolateCartesian(pose_start, pose_target, opt);

    MoveLResult result;
    result.cartesian_waypoints = waypoints;
    result.q_path.reserve(waypoints.size());
    result.q_path.push_back(q_start);

    Eigen::VectorXd seed = q_start;
    for (std::size_t i = 1; i < waypoints.size(); ++i) {
        IKResult sol = ik.solve(waypoints[i], seed);
        if (!sol.success) {
            throw std::runtime_error(std::string("MoveL IK failed at waypoint ") +
                                     std::to_string(i) + ": " + sol.reason);
        }

        const double jump = (sol.q - seed).lpNorm<Eigen::Infinity>();
        if (jump > 0.35) {
            throw std::runtime_error("MoveL rejected discontinuous IK branch");
        }

        result.q_path.push_back(sol.q);
        seed = sol.q;
    }

    std::vector<Eigen::Vector3d> tcp;
    tcp.reserve(result.q_path.size());
    for (const auto& q : result.q_path) {
        tcp.push_back(robot.fk(q).translation());
    }

    result.max_line_error = maxLineError(tcp, pose_start.translation(), pose_target.translation());
    if (result.max_line_error > opt.line_tolerance) {
        throw std::runtime_error("MoveL line tolerance exceeded");
    }

    return result;
}

Here v and a do not yet generate a full motion profile, but the API is pointed in the right direction: tcp_speed and tcp_accel belong to the Cartesian command, not a joint command. When time parameterization is added, an initial segment time can be estimated with T = distance / v, then checked through the Jacobian:

xdot = J(q) qdot
qdot = J#(q) xdot

If any joint exceeds qdot_max, lower v or increase segment time. This is why MoveL needs the Jacobian again: not only for numerical IK, but also for deciding whether a requested TCP speed is feasible at the current robot configuration.

Minimal CMakeLists

If you split the code into src/movel.cpp, include/movel.hpp, and the existing robot/IK files, a small CMake project can look like this:

cmake_minimum_required(VERSION 3.16)
project(classical_arm_controller LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(Eigen3 REQUIRED)

add_library(arm_controller
    src/robot_model.cpp
    src/ik_solver.cpp
    src/movel.cpp
)

target_include_directories(arm_controller PUBLIC include)
target_link_libraries(arm_controller PUBLIC Eigen3::Eigen)

add_executable(test_movel examples/test_movel.cpp)
target_link_libraries(test_movel PRIVATE arm_controller)

Build it with:

cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j
./build/test_movel

test_movel.cpp should print at least the number of waypoints, the largest IK residual, max_line_error, and the final FK pose error. Do not print only success=true; for MoveL, success means the geometric path was verified, not merely that the final IK target was reachable.

Classic MoveL implementation bugs

The first bug is interpolating orientation with roll-pitch-yaw. If yaw goes from 179 degrees to -179 degrees, linear Euler interpolation can command almost 358 degrees of rotation instead of 2 degrees. Quaternion SLERP handles this much better as long as quaternions are normalized and sign-adjusted.

The second bug is forgetting the tool frame. MoveL says the TCP moves straight, not necessarily the flange. If a gripper extends 120 mm from the flange, FK must return the TCP pose after applying the tool offset. A wrong tool frame makes the flange path look correct while the actual tool tip sweeps away from the intended line.

The third bug is solving IK independently for every waypoint. Always pass seed = q_previous, check joint jumps, and prefer solutions near the previous configuration. Industrial controllers care deeply about branch continuity, because a wrist flip halfway through a path can be more dangerous than a small pose residual.

The fourth bug is assuming every Cartesian line is feasible. A reachable start and a reachable goal do not guarantee that the segment between them is reachable. The line can pass through a singularity, a joint limit, a self-collision region, or a forbidden workcell volume. MoveL should fail early with a useful diagnostic: which waypoint failed, what the residual was, and which joint was near its limit.

Conclusion

MoveL is geometrically simple but controller-wise nontrivial: linearly interpolate TCP position, interpolate orientation with SLERP, solve IK continuously at every waypoint, and run FK again to verify straightness. When this is done correctly, you have a practical primitive for precise pick-and-place, probing, glue dispensing, straight welding passes, and any operation where the tool tip must follow a controlled line.

The next step is time parameterization so that v and a actually define timing, velocity, and acceleration for every joint. But optimize timing only after the geometric path is correct. Straightness first, speed second.

References

• Universal Robots Cartesian Interface: MoveL tool moves in a straight line
• MoveIt Quickstart: Cartesian Path attempts linear end-effector motion
• Eigen QuaternionBase: slerp, normalized, angularDistance
• Drake PiecewiseQuaternionSlerp: quaternion trajectories via spherical linear interpolation
• Fast and Accurate Relative Motion Tracking for Two Industrial Robots

Related Posts

• Inverse Kinematics: analytical vs numerical IK
• MoveJ and MoveL: from joint space to Cartesian space
• Jogging and frames: base, tool, world