This function sends trajectory point(With limitation) that will be added in the robotical arm's FIFO.
#include <iostream>
#include <dlfcn.h>
#include <vector>
#include "KinovaTypes.h"
using namespace std;
int main()
{
int result;
cout << "SendAdvanceTrajectory function example" << endl;
void * commandLayer_handle;
int (*MyInitAPI)();
int (*MyCloseAPI)();
int (*MySendAdvanceTrajectory)(TrajectoryPoint command);
int (*MyStartControlAPI)();
commandLayer_handle = dlopen("Kinova.API.USBCommandLayerUbuntu.so",RTLD_NOW|RTLD_GLOBAL);
MyInitAPI = (int (*)()) dlsym(commandLayer_handle,"InitAPI");
MyCloseAPI = (int (*)()) dlsym(commandLayer_handle,"CloseAPI");
MySendAdvanceTrajectory = (int (*)(TrajectoryPoint)) dlsym(commandLayer_handle,"SendAdvanceTrajectory");
MyStartControlAPI = (int (*)()) dlsym(commandLayer_handle,"StartControlAPI");
if((MyInitAPI == NULL) || (MyCloseAPI == NULL) || (MySendAdvanceTrajectory == NULL) || (MyStartControlAPI == NULL))
{
cout << "Unable to initialize the command layer." << endl;
}
else
{
cout << "The command has been initialized correctly." << endl << endl;
cout << "Calling the method InitAPI()" << endl;
result = (*MyInitAPI)();
cout << "result of InitAPI() = " << result << endl << endl;
cout << "We take control of the robotic arm." << endl;
result = (*MyStartControlAPI)();
TrajectoryPoint trajectoryPoint;
trajectoryPoint.Limitations.accelerationParameter1 = 0.0f;
trajectoryPoint.Limitations.accelerationParameter2 = 0.0f;
trajectoryPoint.Limitations.accelerationParameter3 = 0.0f;
trajectoryPoint.Limitations.forceParameter1 = 0.0f;
trajectoryPoint.Limitations.forceParameter2 = 0.0f;
trajectoryPoint.Limitations.forceParameter3 = 0.0f;
trajectoryPoint.Limitations.speedParameter1 = 0.08f;
trajectoryPoint.Limitations.speedParameter2 = 0.6f;
trajectoryPoint.Limitations.speedParameter3 = 0.08f;
trajectoryPoint.LimitationsActive = 1;
trajectoryPoint.Position.Type = CARTESIAN_POSITION;
trajectoryPoint.Position.Actuators.Actuator1 = 0.0f;
trajectoryPoint.Position.Actuators.Actuator2 = 0.0f;
trajectoryPoint.Position.Actuators.Actuator3 = 0.0f;
trajectoryPoint.Position.Actuators.Actuator4 = 0.0f;
trajectoryPoint.Position.Actuators.Actuator5 = 0.0f;
trajectoryPoint.Position.Actuators.Actuator6 = 0.0f;
trajectoryPoint.Position.Delay = 0.0f;
trajectoryPoint.Position.HandMode = POSITION_MODE;
trajectoryPoint.Position.Fingers.Finger1 = 30.0f;
trajectoryPoint.Position.Fingers.Finger2 = 30.0f;
trajectoryPoint.Position.Fingers.Finger3 = 30.0f;
trajectoryPoint.Position.CartesianPosition.X = 0.214f;
trajectoryPoint.Position.CartesianPosition.Y = -0.465f;
trajectoryPoint.Position.CartesianPosition.Z = 0.497f;
trajectoryPoint.Position.CartesianPosition.ThetaX = 1.56f;
trajectoryPoint.Position.CartesianPosition.ThetaY = 0.81f;
trajectoryPoint.Position.CartesianPosition.ThetaZ = 0.04f;
(*MySendAdvanceTrajectory)(trajectoryPoint);
trajectoryPoint.Position.CartesianPosition.Z = 0.3f;
(*MySendAdvanceTrajectory)(trajectoryPoint);
trajectoryPoint.Position.CartesianPosition.Z = 0.5f;
(*MySendAdvanceTrajectory)(trajectoryPoint);
cout << endl << "Calling the method CloseAPI()" << endl;
result = (*MyCloseAPI)();
cout << "result of CloseAPI() = " << result << endl;
}
return 0;
}