A Manipulator Pose Planning Algorithm Based on Matrix Information Geometry

Algorithm 1

Algorithm for the position and the pose matrix at each detected point.

Input: detection path curve, initial and target positions, and poses of the manipulator .

Output: position coordinates and pose matrices at all detected points.

(1) According to the length Equation (19) of the motion trajectory segment between and as well as the curve equation of the detection trajectory, the position coordinates of each detected point are calculated.

(2) Let the poses of the manipulator at the initial and target positions be and , respectively, and the Lie algebra at each detected point is expressed as

    .

Then the pose matrix at each detected point can be calculated by

      .