Understanding/clarify trajectory generation with abb_libegm library

[kappa95]

Dear community,

I start this discussion to leave my understanding about the use of abb_libegm library and particularly on how the trajectories are generated.

(Easy answer): How to load a trajectory (a list of poses):

Trajectories are loaded into object: abb::egm::wrapper::trajectory::TrajectoryGoal
The points are contained into pointer: abb::egm::wrapper::trajectory::PointGoal*

Hence the poses are loaded as:

abb::egm::wrapper::trajectory::TrajectoryGoal trajectory;
abb::egm::wrapper::trajectory::PointGoal *p_point;

p_point = trajectory.add_points(); // add the pointer of the points to the traj
p_point->set_reach(reaching);     // Specify If it is important to reach the position
p_point->set_duration(duration);  // seconds of the time that i want to reach the position: depends on the speed

// Set position!
p_point->mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_position()->set_x(poses[i][0]);
p_point->mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_position()->set_y(poses[i][1]);
p_point->mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_position()->set_z(poses[i][2]);
// Set the orientation euler XYZ!
p_point->mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_euler()->set_x(poses[i][3]);
p_point->mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_euler()->set_y(poses[i][4]);
p_point->mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_euler()->set_z(poses[i][5]);

Which is the shape of the trajectory in time? <--> How to choose the time duration?

The different poses are interpolated in EGM with a method that you have to choose. The available ones are:

• Linear (S(t) = A + B*t)
• Square (S(t) = A + Bt + Ct^2)
• Cubic (S(t) = A + Bt + Ct^2 + D*t^3)
• Quintic (S(t) = A + Bt + Ct^2 + Dt^3 + Et^4 + F*t^5)

where 0<=t<=T i.e. duration

The boundary conditions are between the actual pose and the next point of the trajectory.

Hence, how can I choose the Spline method? I have used this:

// Get the trajectory configurations:
auto traj_config = egm_interface.getConfiguration();
traj_config.base.use_logging = true;  // Still unknown what it is doing
traj_config.base.use_velocity_outputs = true;  // Still unknown what it is doing
traj_config.spline_method = traj_config.Linear;  // Choosing Linear spline method :D

Then how can I decide a speed?
Assuming a linear spline and given the positions between the points, it's straightforward:

// Return the cartesian distance
float length_comp(float x, float y, float z)
{
  float length = 0.0;
  float square = pow(x, 2) + pow(y, 2) + pow(z, 2);
  length = sqrt(square);
  return length;
}

int main()
{
/// stuff here...
// Speed:
float speed = 10; // chosing a 10mm/s
float duration = length_comp(dx, dy, dz) / speed;
p_point->set_duration(duration);  // seconds of the time that i want to reach the position: depends on the speed
}

I tested on the real robot measuring time in circular trajectories with a 200 points discretization and the motion appears smooth and the time for the whole trajectory is comparable as a circumference/speed.

If I am wrong with my understanding please correct.