Reflexxes Motion Libraries
Manual and Documentation (Type II, Version 1.2.6)
|
Below, you can find the source code of a sample application that makes use of the position-based Reflexxes algorithm and that shows, how to set-up time- and phase-synchronized motion trajectories. The source file can be found in the folder
/src/RMLPositionSampleApplications/03_RMLPositionSampleApplication.cpp
The resulting trajectory of this sample program is displayed below the source code.
// ---------------------- Doxygen info ---------------------- // ---------------------------------------------------------- // For a convenient reading of this file's source code, // please use a tab width of four characters. // ---------------------------------------------------------- #include <stdio.h> #include <stdlib.h> #include <ReflexxesAPI.h> #include <RMLPositionFlags.h> #include <RMLPositionInputParameters.h> #include <RMLPositionOutputParameters.h> //************************************************************************* // defines #define CYCLE_TIME_IN_SECONDS 0.001 #define NUMBER_OF_DOFS 2 //************************************************************************* // Main function to run the process that contains the test application // // This function is based on the sample program of // 01_RMLPositionSampleApplication.cpp and contains a simple example to use // and distinguish between time- and phase-synchronized motion trajectories // that are generated by the Type II Reflexxes Motion Library. This code // snippet directly corresponds to the example trajectories shown in the // documentation. //************************************************************************* 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 ); // ******************************************************************** // Set-up a timer with a period of one millisecond // (not implemented in this example in order to keep it simple) // ******************************************************************** printf("-------------------------------------------------------\n" ); printf("Reflexxes Motion Libraries \n" ); printf("Example: 03_RMLPositionSampleApplication \n\n"); printf("This example demonstrates how to set-up the \n" ); printf("generation of time- and phase-synchronized motion \n" ); printf("trajectories using the position-based Online \n" ); printf("Trajectory Generation algorithm of the Type II \n" ); printf("Reflexxes Motion Library. \n\n"); printf("Copyright (C) 2014 Reflexxes GmbH \n" ); printf("-------------------------------------------------------\n" ); // ******************************************************************** // 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 ; 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(;;) { // **************************************************************** // Wait for the next timer tick // (not implemented in this example in order to keep it simple) // **************************************************************** // 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). // **************************************************************** // **************************************************************** // 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 ) ) { 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) ; }