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

时间：2017-10-26 23:20:12 阅读：526 评论：0 收藏：0 [点我收藏+]

标签：ase select mobile original sequence .com following 关节 enum

路径规划 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（两个信号的频率相等，相位差为0或一个常数，称这两个信号相位同步）

　　下图是某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

周期同步位置模式(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

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

标签：ase select mobile original sequence .com following 关节 enum

原文地址：http://www.cnblogs.com/21207-iHome/p/7724694.html

踩

(0)