V-rep學習筆記:Reflexxes Motion Library 1

weixin_33896726發表於2017-01-24

  V-REP中整合了線上運動軌跡生成庫Reflexxes Motion Library Type IV,目前Reflexxes公司已經被谷歌收購。(The Reflexxes Motion Libraries provide instantaneous trajectory generation capabilities for motion control systems.  Reflexxes Motion Library contains a set of On-Line Trajectory Generation (OTG) algorithms that are designed to control robots and mechanical systems.) Reflexxes的眾多優點使其可以應用於機器人、數控機床和伺服驅動系統等領域。

  隨著計算機技術的發展,數控系統已經從最初的由數字邏輯電路構成的硬線數控系統發展到了以計算機為核心的計算機數控(Computer Numerical Control,CNC)系統。相對於硬線數控系統而言,CNC系統的控制功能主要由軟體實現,並可處理邏輯電路難以處理的複雜資訊,因而具有較高的柔性和更高的效能。高速加工對CNC的最基本要求:以足夠快的速度處理資料、為各進給軸加減速產生無衝擊的理論值。高速加工CNC的核心技術是樣條實時插補和無衝擊的加速器。如果jerk(加加速讀/突變)過大,可在短時間內實現加速,但同時會造成機床的振動,從而使所加工表面出現條紋,降低了表面質量。如果加加速過小,可以實現高的表面質量,但很難實現快加速功能。因此,為了保證在高速情況下加工出高質量表面,合理的機床加加速非常重要。

   在基於位置的線上軌跡生成演算法中輸入、輸出如下圖所示。輸入端:當前狀態包括當前位置/姿態向量以及當前速度向量;運動約束包括最大速度和最大加速度(在Type IV庫中還可以支援加加速度);目標狀態包括目標位置/姿態以及目標速度。為了保證演算法的可行性和數值穩定性,輸入引數需要滿足以下幾點要求:

  (1)最大速度和最大加速度必須大於0;

  (2)在位置模式下(for the position-based algorithm),最大速度必須大於或等於目標速度;

  (3)所有輸入引數的數值必須在一定的範圍內。 For the position-based algorithm this is range is specified by

    and for the velocity-based algorithm

Input and output values of the position-based Type II On-Line Trajectory Generation algorithm of the Reflexxes Motion Libraries

   提供合理的輸入引數後,可以從輸出端獲取資料,用於底層控制。下面是一個生成三自由度關節軌跡的簡單例子。The position-based On-Line Trajectory Generation algorithm of the Type II Reflexxes Motion Library is called by the method ReflexxesAPI::RMLPosition(). In this example (cf. Example 1 — Introduction to the Position-based algorithm), we assume to control a simple Cartesian robot with three translational degrees of freedom, and we apply the following input values at time instant $ T_i $.

  三個軸的輸入引數如下:

  下載Reflexxes Motion Libraries Type II後將其解壓,用Visual Studio開啟工程檔案生成TypeIIRML.lib(參考Download and Build Instructions for the Type II Reflexxes Motion Library)。將01_RMLPositionSampleApplication中的原始檔程式碼進行修改,新增寫檔案操作,將生成的輸出資料記錄在文字檔案中:

#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    //one millisecond
#define NUMBER_OF_DOFS                          3


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

    int                         ResultValue = 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);


    // ********************************************************************
    // 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] = 0.0;
    IP->CurrentPositionVector->VecData[2] = 50.0;

    IP->CurrentVelocityVector->VecData[0] = 100.0;
    IP->CurrentVelocityVector->VecData[1] = -220.0;
    IP->CurrentVelocityVector->VecData[2] = -50.0;

    IP->CurrentAccelerationVector->VecData[0] = -150.0;
    IP->CurrentAccelerationVector->VecData[1] = 250.0;
    IP->CurrentAccelerationVector->VecData[2] = -50.0;

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

    IP->MaxAccelerationVector->VecData[0] = 300.0;
    IP->MaxAccelerationVector->VecData[1] = 200.0;
    IP->MaxAccelerationVector->VecData[2] = 100.0;

    IP->TargetPositionVector->VecData[0] = -600.0;
    IP->TargetPositionVector->VecData[1] = -200.0;
    IP->TargetPositionVector->VecData[2] = -350.0;

    IP->TargetVelocityVector->VecData[0] = 50.0;
    IP->TargetVelocityVector->VecData[1] = -50.0;
    IP->TargetVelocityVector->VecData[2] = -200.0;

    IP->SelectionVector->VecData[0] = true;
    IP->SelectionVector->VecData[1] = true;
    IP->SelectionVector->VecData[2] = true;

    std::ofstream out("out.txt", std::ios::app);
    // ********************************************************************
    // Starting the control loop

    while (ResultValue != ReflexxesAPI::RML_FINAL_STATE_REACHED)
    {
        // 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 < 3; 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;
    }

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

    delete  RML;
    delete  IP;
    delete  OP;

    out.close();

    exit(EXIT_SUCCESS);
}
View Code

  while迴圈中呼叫RMLPosition函式(This is the central method of each Reflexxes Type Motion Library)單步執行軌跡生成演算法。如果還沒達到目標狀態,該函式返回ReflexxesAPI::RML_WORKING;如果已達到設定的運動狀態則返回ReflexxesAPI::RML_FINAL_STATE_REACHED,其它返回值可以參見API手冊。

  將記錄的三個軸的位置資料在Excel中用折線圖顯示出來(速度和加速度資訊也可以按照同樣方式操作):

 

  可以看出達到設定的目標位置。Reflexxes Motion Library有更復雜和強大的用法,這裡只是先作一個簡單的介紹。


   在V-REP中整合了Reflexxes Motion Library type IV C++庫,目前在lua API中有如下幾個函式可以使用:

   將UR5機器人從模型庫拖到新建的場景裡中,與其關聯的程式碼裡設定了當前關節速度、加速度以及運動過程中的最大速度、加速度、目標速度以及目標位置。然後呼叫simRMLMoveToJointPositions同時移動UR機器人的6個關節使其按照運動規律達到目標位置。

-- This is a threaded script, and is just an example!

jointHandles={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
    jointHandles[i]=simGetObjectHandle('UR5_joint'..i)
end

-- Set-up some of the RML vectors:
vel=180
accel=40
jerk=80
currentVel={0,0,0,0,0,0,0}
currentAccel={0,0,0,0,0,0,0}
maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
targetVel={0,0,0,0,0,0}

targetPos1={90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180}

-- simRMLMoveToJointPositions: Moves (actuates) several joints at the same time using the Reflexxes Motion Library. 
-- This function can only be called from child scripts running in a thread
--[[
number result,table newPos,table newVel,table newAccel,number timeLeft=simRMLMoveToJointPositions(table jointHandles,
                                                       number flags,table currentVel,table currentAccel,table maxVel,
                                           table maxAccel,table maxJerk,table targetPos,table targetVel,table direction)
--]]
simRMLMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos1,targetVel)

targetPos2={-90*math.pi/180,45*math.pi/180,90*math.pi/180,135*math.pi/180,90*math.pi/180,90*math.pi/180}
simRMLMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos2,targetVel)

targetPos3={0,0,0,0,0,0}
simRMLMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos3,targetVel)
View Code

  新增一個Graph記錄基座轉動關節的角位移,可以看出確實符合程式中的設定:0°→90°→-90°→0°

 

 

參考:

Reflexxes Motion Libraries  Manual and Documentation

V-REP: Reflexxes Motion Library Demonstration

相關文章