V-rep学习笔记：Reflexxes Motion Library 3

路径规划 VS 轨迹规划

　　轨迹规划的目的是将输入的简单任务描述变为详细的运动轨迹描述。注意轨迹和路径的区别：Trajectory refers to a time history of position, velocity, and acceleration for each degree of freedom. The path provides a pure geometric description of motion.

Path planning (global)

The (geometric) path is a sequence of waypoints defining the trajectory coarsely.
Issues solved at this level: obstacle avoidance, shortest path.

Trajectory generating (local)

The path provided by path planning constitutes the input to the trajectory generator.
Trajectory generator “approximates” the desired path waypoints by a class of polynomial functions and generates a time-based control sequence moving the manipulator/mobile platform from its initial configuration to the destination.

　　受到驱动机构性能等因素的制约，机器人进行运动轨迹规划时需要考虑加速度约束、速度约束和位置约束等多个约束条件。比如，一般低速运动情况下只要保证关节角度不超限即可，这对运动轨迹规划影响很小。但是当机器人运动速度较快时，关节角加速度和角速度容易超出约束范围，导致驱动电流过大或者超出限位的事故发生。轻则机器人运动出错，重则损坏硬件。此时，必须在机器人运动轨迹规划时综合考虑各种约束条件。

Trajectory = path + timing law

Having the path (way-points), the trajectory is completed by a choice of a timing law. If s(t) = t, the trajectory parametrization is the natural one given by the time.

• Operational (Cartesian) space: p(s) = ( x(s), y(s), z(s) ) ⇒ s = s(t).

• Joint space: q(λ) = (q₁(λ), q₂(λ), . . . , q_n(λ)), where n = DOFs ⇒ λ = λ(t).

The timing law：

• is chosen based on task specifications (stop in a point, move at a constant velocity, etc.);

• may consider optimality criteria (min transfer time, min energy, etc.);

• constraints are imposed by actuator capabilities (e.g. max torque, max velocity) and/or by the task (e.g., max acceleration on payload)

Reflexxes Motion在线轨迹生成库

　　Reflexxes Motion Library主要分为三层：接口层提供简单易用的应用程序接口，隐藏了算法的细节；算法层主要提供在线轨迹生成算法（On-Line Trajectory Generation algorithm）；数学运算层提供了最基本的数学运算功能以供算法使用：

Illustration of the three source code layers of the Reflexxes Motion Libraries.

　　提供合理的输入参数后，可以从输出端获取数据，用于底层控制

Input and output values of the on-line trajectory generation algorithm

同步行为（Synchronization Behavior）

　　Reflexxes的在线轨迹生成算法主要分为以下三步：

　　Step 1: Calculate the Synchronization Time

　　Step 2: Synchronization of All Selected DOFs

　　Step 3: Calculate Output Values

the basic OTG algorithm steps

　　当涉及多个轴同时运动时Reflexxes中可以设置它们的同步行为，分为无同步、时间同步、相位同步：

non-synchronized: 多个轴到达各自目标位置的时间不限定，即有的先到有的后到
time-synchronized: All DOFs, which are selected for trajectory-following control, have to reach their target state of motion M^trgt at the same time instant, namely at t^sync in order to achieve time synchronization
phase-synchronized: Phase synchronization is the synchronization in position, velocity, acceleration and jerk spaces. It means that, given any instant of time, all variables must complete
the same percentage of their trajectories.

　　下图是某3自由度系统（3个轴）分别在无同步、时间同步、相位同步设定下的轨迹曲线。可以看出没有同步的情况下三个轴到达各自目标点的时间不一致，而时间同步和相位同步下是一致的。

Non-synchronized, time-synchronized, and phase-synchronized trajectories for a system with three degrees of freedom.

　　To specify the behavior of the Reflexxes Motion Library, the enumeration RMLFlags::SyncBehaviorEnum consists of four elements, and the attribute RMLFlags::SynchronizationBehavior is used to specify the synchronization behavior of the desired trajectory.

RMLFlags::NO_SYNCHRONIZATION,
RMLFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE,
RMLFlags::ONLY_TIME_SYNCHRONIZATION, and
RMLFlags::ONLY_PHASE_SYNCHRONIZATION,

　　在Example 3 — Different Synchronization Behaviors of the Position-based algorithm这个例子中可以修改同步行为，看看具体的差别。轨迹生成器的输入参数如下图所示，不同颜色代表了不同的自由度：

　　程序模拟了偶发事件的产生，传感器捕获到事件后Reflexxes可以快速、动态地计算轨迹（react instantaneously to unforeseen sensor events ）。程序运行到1000ms时接收到一个传感器事件，这时将设定一个中间位置（intermediate point / waypoint），轴运动到中间位置后再将目标位置设为最开始的值。比如机器人运行过程中突然遇到障碍物，传感器检测到障碍后就可以先改变位置避开障碍，然后再驶向目标。

#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <iostream>

#include <ReflexxesAPI.h>
#include <RMLPositionFlags.h>
#include <RMLPositionInputParameters.h>
#include <RMLPositionOutputParameters.h>


//*************************************************************************
// defines

#define CYCLE_TIME_IN_SECONDS                   0.001   // time step: 1ms
#define NUMBER_OF_DOFS                          2


int main()
{
    // ********************************************************************
    // Variable declarations and definitions

    bool                        IntermediateTargetStateSet  =   false
                            ,   IntermediateStateReached    =   false   ;

    int                         ResultValue                 =   0       ;

    double                      Time                        =   0.0     ;

    ReflexxesAPI                *RML                        =   NULL    ;

    RMLPositionInputParameters  *IP                         =   NULL    ;

    RMLPositionOutputParameters *OP                         =   NULL    ;

    RMLPositionFlags            Flags                                   ;

    // ********************************************************************
    // Creating all relevant objects of the Type II Reflexxes Motion Library

    RML =   new ReflexxesAPI(                   NUMBER_OF_DOFS
                                            ,   CYCLE_TIME_IN_SECONDS   );

    IP  =   new RMLPositionInputParameters(     NUMBER_OF_DOFS          );

    OP  =   new RMLPositionOutputParameters(    NUMBER_OF_DOFS          );

    std::ofstream out("data.txt", std::ios::app);

    // ********************************************************************
    // Set-up the input parameters

    // In this test program, arbitrary values are chosen. If executed on a
    // real robot or mechanical system, the position is read and stored in
    // an RMLPositionInputParameters::CurrentPositionVector vector object.
    // For the very first motion after starting the controller, velocities
    // and acceleration are commonly set to zero. The desired target state
    // of motion and the motion constraints depend on the robot and the
    // current task/application.
    // The internal data structures make use of native C data types
    // (e.g., IP->CurrentPositionVector->VecData is a pointer to
    // an array of NUMBER_OF_DOFS double values), such that the Reflexxes
    // Library can be used in a universal way.

    IP->CurrentPositionVector->VecData      [0] =    100.0      ;
    IP->CurrentPositionVector->VecData      [1] =    100.0      ;

    IP->CurrentVelocityVector->VecData      [0] =      0.0      ;
    IP->CurrentVelocityVector->VecData      [1] =      0.0      ;

    IP->CurrentAccelerationVector->VecData  [0] =      0.0      ;
    IP->CurrentAccelerationVector->VecData  [1] =      0.0      ;

    IP->MaxVelocityVector->VecData          [0] =    300.0      ;
    IP->MaxVelocityVector->VecData          [1] =    300.0      ;

    IP->MaxAccelerationVector->VecData      [0] =    400.0      ;
    IP->MaxAccelerationVector->VecData      [1] =    400.0      ;

    IP->MaxJerkVector->VecData              [0] =    500.0      ;
    IP->MaxJerkVector->VecData              [1] =    500.0      ;

    IP->TargetPositionVector->VecData       [0] =    700.0      ;
    IP->TargetPositionVector->VecData       [1] =    300.0      ;

    IP->TargetVelocityVector->VecData       [0] =     0.0       ;
    IP->TargetVelocityVector->VecData       [1] =     0.0       ;

    // The selection vector contains boolean values to mask single DOFs, for which no output values are calculated. 
    IP->SelectionVector->VecData            [0] =   true        ;
    IP->SelectionVector->VecData            [1] =   true        ;

    // ********************************************************************
    // Setting the flag for time- and phase-synchronization:
    //
    //  - RMLPositionFlags::ONLY_TIME_SYNCHRONIZATION for
    //    time-synchronization
    //  - RMLPositionFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE for
    //    phase-synchronization
    //
    // Please feel free to change this flag to see the difference in the
    // behavior of the algorithm.

    Flags.SynchronizationBehavior = RMLPositionFlags::ONLY_TIME_SYNCHRONIZATION;

    // ********************************************************************
    // Starting the control loop

    for(;;)
    {

        // Calling the Reflexxes OTG algorithm
        ResultValue =   RML->RMLPosition(       *IP
                                            ,   OP
                                            ,   Flags       );

        if (ResultValue < 0)
        {
            printf("An error occurred (%d).\n", ResultValue );
            break;
        }

        // ****************************************************************
        // Here, the new state of motion, that is
        //
        // - OP->NewPositionVector
        // - OP->NewVelocityVector
        // - OP->NewAccelerationVector
        //
        // can be used as input values for lower level controllers. In the
        // most simple case, a position controller in actuator space is
        // used, but the computed state can be applied to many other
        // controllers (e.g., Cartesian impedance controllers,
        // operational space controllers).
        // ****************************************************************
        for (int i = 0; i < NUMBER_OF_DOFS; i++)
            out << OP->NewPositionVector->VecData[i] << ",";
        out << std::endl;

        // ****************************************************************
        // Feed the output values of the current control cycle back to
        // input values of the next control cycle

        *IP->CurrentPositionVector      =   *OP->NewPositionVector      ;
        *IP->CurrentVelocityVector      =   *OP->NewVelocityVector      ;
        *IP->CurrentAccelerationVector  =   *OP->NewAccelerationVector  ;

        Time    +=  CYCLE_TIME_IN_SECONDS;

        // ****************************************************************
        // In this introductory example, we simple trigger a sensor event
        // after one second. On a real-world system, trigger signal are
        // commonly generated based on (unforeseen) sensor signals. This
        // event changes the input parameters and specifies a
        // intermediate state of motion, that is, a new desired target
        // state of motion for the Reflexxes algorithm.


        if (    (   Time >= 1.0                 )
            &&  (   !IntermediateTargetStateSet )   )
        {
            IntermediateTargetStateSet   =   true;

            IP->TargetPositionVector->VecData       [0] =    550.0      ;
            IP->TargetPositionVector->VecData       [1] =    250.0      ;

            IP->TargetVelocityVector->VecData       [0] =   -150.0      ;
            IP->TargetVelocityVector->VecData       [1] =    -50.0      ;
        }

        // ****************************************************************
        // After reaching the intermediate state of motion define above
        // we switch the values of the desired target state of motion
        // back to the original one. In the documentation and the
        // description of time- and phase-synchronized motion trajectories,
        // this switching happens at 3873 milliseconds.

        if (    (   ResultValue == ReflexxesAPI::RML_FINAL_STATE_REACHED    )
            &&  (   !IntermediateStateReached                               )   )
        {
            IntermediateStateReached    =   true;

            IP->TargetPositionVector->VecData       [0] =    700.0      ;
            IP->TargetPositionVector->VecData       [1] =    300.0      ;

            IP->TargetVelocityVector->VecData       [0] =     0.0       ;
            IP->TargetVelocityVector->VecData       [1] =     0.0       ;

            continue;
        }

        // ****************************************************************
        // After the final state of motion is reached, we leave the loop
        // and terminate the program.

        if (ResultValue == ReflexxesAPI::RML_FINAL_STATE_REACHED)
        {
            break;
        }
    }

    // ********************************************************************
    // Deleting the objects of the Reflexxes Motion Library end terminating
    // the process

    delete  RML         ;
    delete  IP          ;
    delete  OP          ;

    exit(EXIT_SUCCESS)  ;
}

　　从上面的轨迹曲线图中很难看出时间同步和相位同步有什么差别，但是将两个自由度的位置画成散点图就可以发现差异：

　　可以看出时间同步只是两个轴会同时到达各自目标位置，而相位同步在此基础上还限定了其相位关系（在信号处理中如果两个信号的频率相等，相位差为0或一个常数，称这两个信号相位同步）。设置成相位同步后两个自由度的位置轨迹呈线性关系，在图中表现为一条从起点到目标点的直线：p₂(t) = ( p₁(t) - 100 ) / 3 + 100

参考：

V-rep学习笔记：Reflexxes Motion Library 1

V-rep学习笔记：Reflexxes Motion Library 4

周期同步位置模式(CSP)，轮廓位置模式(PPM)，位置模式(PM)

On-Line Trajectory Generation in Robotic Systems

Introduction to Robotics-Mechanics and Control. Chapter 7 Trajectory generation

Path and trajectory generation

Robot trajectory generation

Modern Robotics Mechanics, Planning, and Control