Adaptation of velocity and acceleration for small-step motions

The concatenation of many small-step trajectories can have many reasons. But especially with cartesian trajectories, there is a problem: although the speed is reduced using the “Move Group” (see figure 1), it is not actually adapted for the robot.

move_group.setMaxVelocityScalingFactor(0.5);
move_group.setMaxAccelerationScalingFactor(0.5);

Figure 1: Traditional setting of speed and acceleration.

The solution is to post-process a trajectory generated by MoveIt. For this, the first step is to extract the trajectory messages from the planned movement. They can simple be retrieved from the const moveit::planning_interface::MoveGroupInterface::Plan &plan via the trajectory field. The second step is the iterating over the messages and to modify three different parameters (see figure 2). Thats first the velocity ration (0.1 to 1.0) multiplied with the base velocity and second the time_from_start, which is 1 divided throught the target velocity ratio. The acceleration parameter is finally set by taking the square root of the corresponding newly computed velocity.

void applyMotionSpeedFactor(moveit_msgs::RobotTrajectory &trajectory,
                            double velocity) {

    for (auto &point : trajectory.joint_trajectory.points) {
        point.time_from_start *= 1 / velocity;

        for ( unsigned int i = 0; i < point.velocities.size(); i++){
            point.velocities[i] *= velocity;
            point.accelerations[i] = sqrt(std::abs(point.velocities[i]));
        }
    }
}

Figure 2: Postpocessing the trajectory

The shown problem does not effect joint space trajectories. They can be modified as shown in figure 1. A code example can be found here.