TrajectoryPoint kinova_controller::ConvertControl(State & value, int type)
{
TrajectoryPoint pointToSend;
pointToSend.InitStruct();
if(type == -1)
{
pointToSend.Position.Type = this->controltype;
}
else
{
pointToSend.Position.Type = this->InitPositionType(type);
}
pointToSend.Position.HandMode = HAND_NOMOVEMENT;
if(controltype==ANGULAR_POSITION || controltype==ANGULAR_VELOCITY)
{
value=value/DEG;
pointToSend.Position.Actuators.Actuator1 = (float)value[0];
pointToSend.Position.Actuators.Actuator2 = (float)value[1];
pointToSend.Position.Actuators.Actuator3 = (float)value[2];
pointToSend.Position.Actuators.Actuator4 = (float)value[3];
pointToSend.Position.Actuators.Actuator5 = (float)value[4];
pointToSend.Position.Actuators.Actuator6 = (float)value[5];
}
else
{
pointToSend.Position.CartesianPosition.X = (float)value[0];
pointToSend.Position.CartesianPosition.Y = (float)value[1];
pointToSend.Position.CartesianPosition.Z = (float)value[2];
//We set the orientation part of the position (unit is RAD)
pointToSend.Position.CartesianPosition.ThetaX = (float)value[3];
pointToSend.Position.CartesianPosition.ThetaY = (float)value[4];
pointToSend.Position.CartesianPosition.ThetaZ = (float)value[5];
}
return pointToSend;
}
void JacoTrajectoryActionServer::actionCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
try {
if (arm_comm_.isStopped()) {
ROS_ERROR_STREAM("Could not complete joint angle action because the arm is 'stopped'.");
action_server_.setAborted();
return;
}
// Clear all previous trajectories and points
arm_comm_.initTrajectory();
trajectory_msgs::JointTrajectoryPoint goal_waypoint =
goal->trajectory.points[goal->trajectory.points.size() - 1];
trajectory_msgs::JointTrajectoryPoint start_waypoint =
goal->trajectory.points[0];
JacoAngles current_joint_angles, goal_joint_angles, start_joint_angles;
goal_joint_angles.Actuator1 = goal_waypoint.positions[0] * (180 / PI);
goal_joint_angles.Actuator2 = goal_waypoint.positions[1] * (180 / PI);
goal_joint_angles.Actuator3 = goal_waypoint.positions[2] * (180 / PI);
goal_joint_angles.Actuator4 = goal_waypoint.positions[3] * (180 / PI);
goal_joint_angles.Actuator5 = goal_waypoint.positions[4] * (180 / PI);
goal_joint_angles.Actuator6 = goal_waypoint.positions[5] * (180 / PI);
convertDHAnglesToPhysical(goal_joint_angles);
normalizeAngles(goal_joint_angles);
start_joint_angles.Actuator1 = start_waypoint.positions[0] * (180 / PI);
start_joint_angles.Actuator2 = start_waypoint.positions[1] * (180 / PI);
start_joint_angles.Actuator3 = start_waypoint.positions[2] * (180 / PI);
start_joint_angles.Actuator4 = start_waypoint.positions[3] * (180 / PI);
start_joint_angles.Actuator5 = start_waypoint.positions[4] * (180 / PI);
start_joint_angles.Actuator6 = start_waypoint.positions[5] * (180 / PI);
convertDHAnglesToPhysical(start_joint_angles);
normalizeAngles(start_joint_angles);
///////////////////////////////////
//Sanity Check that arm is actually starting from the first waypoint in the trajectory
///////////////////////////////////
arm_comm_.getJointAngles(current_joint_angles);
normalizeAngles(current_joint_angles);
AngularInfo error = computeError(start_joint_angles, current_joint_angles);
if (std::abs(error.Actuator1) > MAX_ERROR_TOLERANCE) {
ROS_ERROR_STREAM("Way to large of error in JacoTrajectoryActionServer actuator 1 before even starting, setting to aborted");
action_server_.setAborted();
ROS_INFO_STREAM("Leaving: JacoTrajectoryActionServer::actionCallback\n");
return;
}
if (std::abs(error.Actuator2) > MAX_ERROR_TOLERANCE) {
ROS_ERROR_STREAM("Way to large of error in JacoTrajectoryActionServer actuator 2 before even starting, setting to aborted");
action_server_.setAborted();
ROS_INFO_STREAM("Leaving: JacoTrajectoryActionServer::actionCallback\n");
return;
}
if (std::abs(error.Actuator3) > MAX_ERROR_TOLERANCE) {
ROS_ERROR_STREAM("Way to large of error in JacoTrajectoryActionServer actuator 3 before even starting, setting to aborted");
action_server_.setAborted();
ROS_INFO_STREAM("Leaving: JacoTrajectoryActionServer::actionCallback\n");
return;
}
if (std::abs(error.Actuator4) > MAX_ERROR_TOLERANCE) {
ROS_ERROR_STREAM("Way to large of error in JacoTrajectoryActionServer actuator 4 before even starting, setting to aborted");
action_server_.setAborted();
ROS_INFO_STREAM("Leaving: JacoTrajectoryActionServer::actionCallback\n");
return;
}
if (std::abs(error.Actuator5) > MAX_ERROR_TOLERANCE) {
ROS_ERROR_STREAM("Way to large of error in JacoTrajectoryActionServer actuator 5 before even starting, setting to aborted");
action_server_.setAborted();
return;
}
if (std::abs(error.Actuator6) > MAX_ERROR_TOLERANCE) {
ROS_ERROR_STREAM("Way to large of error in JacoTrajectoryActionServer actuator 6 before even starting, setting to aborted");
action_server_.setAborted();
ROS_INFO_STREAM("Leaving: JacoTrajectoryActionServer::actionCallback\n");
return;
}
ros::Time start_time = ros::Time::now();
bool stop = false;
bool read_joint_angles = false;
for (unsigned int i = 0; i < goal->trajectory.points.size(); i++) {
// Get the waypoint to be reached
trajectory_msgs::JointTrajectoryPoint waypoint =
goal->trajectory.points[i];
// Initialize a trajectory point which we will be constantly
// feeding to the robot until the waypoint time's up or until
// we are in the final configuration.
TrajectoryPoint point;
point.InitStruct();
memset(&point, 0, sizeof (point));
// The trajectory consists of angular velocity waypoints
point.Position.Type = ANGULAR_VELOCITY;
// Set up the trajectory point with waypoint values converted
// from [rad/s] to [deg/s]
point.Position.Actuators.Actuator1 = -waypoint.velocities[0] * (180 / PI);
point.Position.Actuators.Actuator2 = +waypoint.velocities[1] * (180 / PI);
point.Position.Actuators.Actuator3 = -waypoint.velocities[2] * (180 / PI);
point.Position.Actuators.Actuator4 = -waypoint.velocities[3] * (180 / PI);
point.Position.Actuators.Actuator5 = -waypoint.velocities[4] * (180 / PI);
point.Position.Actuators.Actuator6 = -waypoint.velocities[5] * (180 / PI);
JacoAngles current_waypoint_angles;
current_waypoint_angles.Actuator1 = waypoint.positions[0] * (180 / PI);
current_waypoint_angles.Actuator2 = waypoint.positions[1] * (180 / PI);
current_waypoint_angles.Actuator3 = waypoint.positions[2] * (180 / PI);
current_waypoint_angles.Actuator4 = waypoint.positions[3] * (180 / PI);
current_waypoint_angles.Actuator5 = waypoint.positions[4] * (180 / PI);
current_waypoint_angles.Actuator6 = waypoint.positions[5] * (180 / PI);
convertDHAnglesToPhysical(current_waypoint_angles);
normalizeAngles(current_waypoint_angles);
read_joint_angles = false;
while(!read_joint_angles){
try{
arm_comm_.getJointAngles(current_joint_angles);
read_joint_angles = true;
} catch (const std::exception& e) {
ROS_ERROR_STREAM(e.what());
ROS_ERROR_STREAM("Carrying on anyway!");
}
}
normalizeAngles(current_joint_angles);
error = computeError(current_waypoint_angles, current_joint_angles);
stop = false;
checkCurrentWayPointError(error.Actuator1, stop);
checkCurrentWayPointError(error.Actuator2, stop);
checkCurrentWayPointError(error.Actuator3, stop);
checkCurrentWayPointError(error.Actuator4, stop);
checkCurrentWayPointError(error.Actuator5, stop);
checkCurrentWayPointError(error.Actuator6, stop);
if(stop)
{
ROS_ERROR_STREAM("Inside Open Loop Velocity Controller: Way to large of error in JacoTrajectoryActionServer actuator setting to aborted");
ROS_ERROR_STREAM("error.Actuator1: " << error.Actuator1 << std::endl);
ROS_ERROR_STREAM("error.Actuator2: " << error.Actuator2 << std::endl);
ROS_ERROR_STREAM("error.Actuator3: " << error.Actuator3 << std::endl);
ROS_ERROR_STREAM("error.Actuator4: " << error.Actuator4 << std::endl);
ROS_ERROR_STREAM("error.Actuator5: " << error.Actuator5 << std::endl);
ROS_ERROR_STREAM("error.Actuator6: " << error.Actuator6 << std::endl);
action_server_.setAborted();
return;
}
ros::Rate r(100); // The loop below will run at 100Hz
while ((waypoint.time_from_start >= ros::Time::now() - start_time)) {
ros::spinOnce();
// If preempted, stop the motion, enable the arm again and
// send a notification of preemption
if (action_server_.isPreemptRequested() || !ros::ok()) {
arm_comm_.stopAPI();
arm_comm_.startAPI();
action_server_.setPreempted();
return;
} else if (arm_comm_.isStopped()) {
action_server_.setAborted();
return;
}
// Get the current real angles and normalize them for comparing
// with the target and adjusting the execution
// arm_comm_.getJointAngles(current_joint_angles);
// normalizeAngles(current_joint_angles);
// Adjust actuator velocities accordingly
arm_comm_.addTrajectoryPoint(point);
// Ensures executing at 100Hz
r.sleep();
}
}
stop = false;
ros::Rate r(100); // The loop below will run at 100Hz (every 10ms)
// This loop finalizes the movement by checking angular distance
// of joints from the desired configuration. Instead of moving
// to the last waypoint, we move and check each joint separately
// to ensure we are at the right position.
bool reached_goal = false;
while (!reached_goal) {
// Get the current real angles and normalize them for comparing
// with the target and adjusting the execution
arm_comm_.getJointAngles(current_joint_angles);
normalizeAngles(current_joint_angles);
AngularInfo error = computeError(goal_joint_angles, current_joint_angles);
// Initialize a trajectory point which we will be constantly
// feeding to the robot untilwe are in the final configuration.
TrajectoryPoint point;
point.InitStruct();
memset(&point, 0, sizeof (point));
point.Position.Actuators.Actuator1 = 0.0;
point.Position.Actuators.Actuator2 = 0.0;
point.Position.Actuators.Actuator3 = 0.0;
point.Position.Actuators.Actuator4 = 0.0;
point.Position.Actuators.Actuator5 = 0.0;
point.Position.Actuators.Actuator6 = 0.0;
// The trajectory consists of angular velocity waypoints
point.Position.Type = ANGULAR_VELOCITY;
reached_goal = true;
bool exit_now = false;//is something really wrong
setPControllerJointVelocity(point.Position.Actuators.Actuator1, error.Actuator1, reached_goal, exit_now);
setPControllerJointVelocity(point.Position.Actuators.Actuator2, error.Actuator2, reached_goal, exit_now);
setPControllerJointVelocity(point.Position.Actuators.Actuator3, error.Actuator3, reached_goal, exit_now);
setPControllerJointVelocity(point.Position.Actuators.Actuator4, error.Actuator4, reached_goal, exit_now);
setPControllerJointVelocity(point.Position.Actuators.Actuator5, error.Actuator5, reached_goal, exit_now);
setPControllerJointVelocity(point.Position.Actuators.Actuator6, error.Actuator6, reached_goal, exit_now);
if(exit_now)
{
ROS_ERROR_STREAM("Way to large of error in JacoTrajectoryActionServer actuator setting to aborted");
action_server_.setAborted();
ROS_INFO_STREAM("Leaving: JacoTrajectoryActionServer::actionCallback\n");
return;
}
// Adjust actuator velocities accordingly
arm_comm_.addTrajectoryPoint(point);
ros::spinOnce();
// Ensure execution at 100Hz
r.sleep();
}
// If we got here, it seems that we succeeded
action_server_.setSucceeded();
} catch (const std::exception& e) {
// Something rather terrible has happened - report it!
ROS_ERROR_STREAM(e.what());
action_server_.setAborted();
}
}
int main()
{
int result;
CartesianPosition currentCommand;
//Handle for the library's command layer.
void * commandLayer_handle;
//Function pointers to the functions we need
int (*MyInitAPI)();
int (*MyCloseAPI)();
int (*MySendBasicTrajectory)(TrajectoryPoint command);
int (*MyGetDevices)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result);
int (*MySetActiveDevice)(KinovaDevice device);
int (*MyMoveHome)();
int (*MyInitFingers)();
int (*MyGetQuickStatus)(QuickStatus &);
int (*MyGetCartesianCommand)(CartesianPosition &);
//We load the library
commandLayer_handle = dlopen("Kinova.API.USBCommandLayerUbuntu.so",RTLD_NOW|RTLD_GLOBAL);
//We load the functions from the library (Under Windows, use GetProcAddress)
MyInitAPI = (int (*)()) dlsym(commandLayer_handle,"InitAPI");
MyCloseAPI = (int (*)()) dlsym(commandLayer_handle,"CloseAPI");
MyMoveHome = (int (*)()) dlsym(commandLayer_handle,"MoveHome");
MyInitFingers = (int (*)()) dlsym(commandLayer_handle,"InitFingers");
MyGetDevices = (int (*)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result)) dlsym(commandLayer_handle,"GetDevices");
MySetActiveDevice = (int (*)(KinovaDevice devices)) dlsym(commandLayer_handle,"SetActiveDevice");
MySendBasicTrajectory = (int (*)(TrajectoryPoint)) dlsym(commandLayer_handle,"SendBasicTrajectory");
MyGetQuickStatus = (int (*)(QuickStatus &)) dlsym(commandLayer_handle,"GetQuickStatus");
MyGetCartesianCommand = (int (*)(CartesianPosition &)) dlsym(commandLayer_handle,"GetCartesianCommand");
if((MyInitAPI == NULL) || (MyCloseAPI == NULL) ||
(MyGetQuickStatus == NULL) || (MySendBasicTrajectory == NULL) ||
(MySendBasicTrajectory == NULL) || (MyMoveHome == NULL) || (MyInitFingers == NULL) ||
(MyGetCartesianCommand == NULL))
{
cout << "* * * E R R O R D U R I N G I N I T I A L I Z A T I O N * * *" << endl;
}
else
{
cout << "I N I T I A L I Z A T I O N C O M P L E T E D" << endl << endl;
result = (*MyInitAPI)();
cout << "Initialization's result :" << result << endl;
KinovaDevice list[MAX_KINOVA_DEVICE];
int devicesCount = MyGetDevices(list, result);
for(int i = 0; i < devicesCount; i++)
{
cout << "Found a robot on the USB bus (" << list[i].SerialNumber << ")" << endl;
//Setting the current device as the active device.
MySetActiveDevice(list[i]);
cout << "Send the robot to HOME position" << endl;
MyMoveHome();
cout << "Initializing the fingers" << endl;
MyInitFingers();
TrajectoryPoint pointToSend;
pointToSend.InitStruct();
//We specify that this point will be used an angular(joint by joint) velocity vector.
pointToSend.Position.Type = CARTESIAN_VELOCITY;
pointToSend.Position.CartesianPosition.X = 0;
pointToSend.Position.CartesianPosition.Y = -0.15; //Move along Y axis at 20 cm per second
pointToSend.Position.CartesianPosition.Z = 0;
pointToSend.Position.CartesianPosition.ThetaX = 0;
pointToSend.Position.CartesianPosition.ThetaY = 0;
pointToSend.Position.CartesianPosition.ThetaZ = 0;
pointToSend.Position.Fingers.Finger1 = 0;
pointToSend.Position.Fingers.Finger2 = 0;
pointToSend.Position.Fingers.Finger3 = 0;
for(int i = 0; i < 200; i++)
{
//We send the velocity vector every 5 ms as long as we want the robot to move along that vector.
MySendBasicTrajectory(pointToSend);
usleep(5000);
}
pointToSend.Position.CartesianPosition.Y = 0;
pointToSend.Position.CartesianPosition.Z = 0.1;
for(int i = 0; i < 200; i++)
{
//We send the velocity vector every 5 ms as long as we want the robot to move along that vector.
MySendBasicTrajectory(pointToSend);
usleep(5000);
}
cout << "Send the robot to HOME position" << endl;
MyMoveHome();
//We specify that this point will be an angular(joint by joint) position.
pointToSend.Position.Type = CARTESIAN_POSITION;
//We get the actual angular command of the robot.
MyGetCartesianCommand(currentCommand);
pointToSend.Position.CartesianPosition.X = currentCommand.Coordinates.X;
pointToSend.Position.CartesianPosition.Y = currentCommand.Coordinates.Y - 0.1f;
pointToSend.Position.CartesianPosition.Z = currentCommand.Coordinates.Z;
pointToSend.Position.CartesianPosition.ThetaX = currentCommand.Coordinates.ThetaX;
pointToSend.Position.CartesianPosition.ThetaY = currentCommand.Coordinates.ThetaY;
pointToSend.Position.CartesianPosition.ThetaZ = currentCommand.Coordinates.ThetaZ;
cout << "*********************************" << endl;
cout << "Sending the first point to the robot." << endl;
MySendBasicTrajectory(pointToSend);
pointToSend.Position.CartesianPosition.Z = currentCommand.Coordinates.Z + 0.1f;
cout << "Sending the second point to the robot." << endl;
MySendBasicTrajectory(pointToSend);
cout << "*********************************" << endl << endl << endl;
}
cout << endl << "C L O S I N G A P I" << endl;
result = (*MyCloseAPI)();
}
dlclose(commandLayer_handle);
return 0;
}
void ToolSemiTracking::mousePress( QMouseEvent *mouseEvent)
{
if (Qt::LeftButton == mouseEvent->button()) // left mouse button, add point and line
{
// check if the current vechleID is ok, same vehicle cannot appear two time at same time instance
int frmIndex = mView->mCurrentFrameIndex;
QPair<TrajectoryPoint*, QGraphicsTextItem*> pt_text_old = mView->mMainWind->findTrajPoint( mView, frmIndex, mCurrentVehicleID);
if( pt_text_old.first != NULL ) // two vehicle appear at same time
{
QMessageBox::information(mView, "same vehicle existed", "please change vehicle ID!");
return;
}
// add one point
// save point to list
QPointF pt = mView->mapToScene( QPoint(mouseEvent->x(), mouseEvent->y()) );
float x = pt.x();
float y = pt.y();
// if point not on image, return
if (x<0 || x>mView->mCurrentFrame.cols || y<0 || y>mView->mCurrentFrame.rows)
return;
// add sum image to Image Window
//
int xMin = (x - mSubWndWidth)>=0 ? x-mSubWndWidth : 0;
int yMin = (y - mSubWndWidth)>=0 ? y-mSubWndWidth : 0;
int xMax = (x + mSubWndWidth)>=mView->mCurrentFrame.cols ? mView->mCurrentFrame.cols : (x+mSubWndWidth);
int yMax = (y + mSubWndWidth)>=mView->mCurrentFrame.rows ? mView->mCurrentFrame.rows : (y+mSubWndWidth);
if ( xMax<=0 )
{
xMax=0;
}
if ( yMax<=0 )
{
yMax=0;
}
if ( xMax-xMin == 0)
return;
if ( yMax-yMin == 0)
return;
Mat mSubImage = mView->mCurrentFrame.colRange(xMin,xMax).rowRange(yMin,yMax);
Mat subImg = Mat( mSubImage.size(), mSubImage.type(), Scalar(0, 0, 0) );
mSubImage.copyTo( subImg );
// draw center and show in small window
//
resize( subImg, subImg, subImg.size()*mEnlargeRate );
circle( subImg, Point( subImg.cols/2, subImg.rows/2), 1, Scalar(230, 0, 0) );
circle( subImg, Point( subImg.cols/2, subImg.rows/2), 5, Scalar(230, 0, 0) );
mView->mMainWind->mMatWnd->showMatUp( subImg );
// create point item and show
//
TrajectoryPoint *pointItem = new TrajectoryPoint( mCurrentVehicleID, mView->mMainWind->getBikeType() );
pointItem->setPosition( pt.x(), pt.y() ); // 1, 1
//pointItem->setPos( pt ); this will cause point lost in GUI ??????????????????????????????????????????????????????????????
pointItem->setPen( QPen(Qt::red) );
mPointItems.append( pointItem );
mView->scene()->addItem( pointItem ); // show line
// create label for ID of vehicle and show
//
QGraphicsTextItem *textItem = new QGraphicsTextItem( QString("%1").arg(mCurrentVehicleID) );
//textItem->setPen( QPen(Qt::red) );
textItem->setPos( pt );
QFont font;
font.setPointSize( 6 );
textItem->setFont( font );
textItem->setOpacity( 0.6 );
mView->scene()->addItem( textItem );
// save trajectory point into main window
//
QPair<TrajectoryPoint*, QGraphicsTextItem*> pair;
pair.first = pointItem;
pair.second = textItem;
if ( !mView->mMainWind->mPrjFileObj.mTrajectory.contains(frmIndex) ) // not found points at time instance
{
QMap<int, QPair<TrajectoryPoint*, QGraphicsTextItem*>> timePoints;
timePoints[ mCurrentVehicleID ] = pair;
mView->mMainWind->mPrjFileObj.mTrajectory[frmIndex] = timePoints;
} else { //found point at given time instance
mView->mMainWind->mPrjFileObj.mTrajectory[frmIndex][ mCurrentVehicleID ] = pair ;
}
}
}
int main()
{
cout << endl << endl;
cout << "======================================================" << endl;
cout << "===== Simulating A Spring Damper with Jaco2 =====" << endl;
cout << "======================================================" << endl;
cout << "code: Reza Ahmadzadeh (IRIM, 2016)." << endl;
int result;
int programResult = 0;
//Handle for the library's command layer.
void * commandLayer_handle;
//Function pointers to the functions we need
int(*MyInitAPI)();
int(*MyCloseAPI)();
int(*MyGetAngularCommand)(AngularPosition &);
int(*MyGetAngularPosition)(AngularPosition &);
int(*MyGetDevices)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result);
int(*MySetActiveDevice)(KinovaDevice device);
int(*MyGetActuatorAcceleration)(AngularAcceleration &Response);
int(*MyGetAngularVelocity)(AngularPosition &Response);
int(*MyMoveHome)();
int(*MyRunGravityZEstimationSequence)(ROBOT_TYPE type, double OptimalzParam[OPTIMAL_Z_PARAM_SIZE]);
int(*MySwitchTrajectoryTorque)(GENERALCONTROL_TYPE);
int(*MySetTorqueSafetyFactor)(float factor);
int(*MySendAngularTorqueCommand)(float Command[COMMAND_SIZE]);
int(*MySendCartesianForceCommand)(float Command[COMMAND_SIZE]);
int(*MySetGravityVector)(float Command[3]);
int(*MySetGravityPayload)(float Command[GRAVITY_PAYLOAD_SIZE]);
int(*MySetGravityOptimalZParam)(float Command[GRAVITY_PARAM_SIZE]);
int(*MySetGravityType)(GRAVITY_TYPE Type);
int(*MySetTorqueVibrationController)(float value);
int(*MyGetAngularForceGravityFree)(AngularPosition &);
int(*MyGetCartesianForce)(CartesianPosition &);
int(*MySetTorqueControlType)(TORQUECONTROL_TYPE type);
int(*MySetTorqueActuatorDamping)(float Command[COMMAND_SIZE]);
int (*MySetTorqueRobotProtection)(int);
int (*MySendBasicTrajectory)(TrajectoryPoint angcommand);
//We load the library (Under Windows, use the function LoadLibrary)
commandLayer_handle = dlopen("Kinova.API.USBCommandLayerUbuntu.so",RTLD_NOW|RTLD_GLOBAL);
MyInitAPI = (int(*)()) dlsym(commandLayer_handle, "InitAPI");
MyCloseAPI = (int(*)()) dlsym(commandLayer_handle, "CloseAPI");
MyGetAngularCommand = (int(*)(AngularPosition &)) dlsym(commandLayer_handle, "GetAngularCommand");
MyGetAngularPosition = (int(*)(AngularPosition &)) dlsym(commandLayer_handle, "GetAngularPosition");
MyGetDevices = (int(*)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result)) dlsym(commandLayer_handle, "GetDevices");
MySetActiveDevice = (int(*)(KinovaDevice devices)) dlsym(commandLayer_handle, "SetActiveDevice");
MyGetActuatorAcceleration = (int(*)(AngularAcceleration &)) dlsym(commandLayer_handle, "GetActuatorAcceleration");
MyGetAngularVelocity = (int(*)(AngularPosition &)) dlsym(commandLayer_handle, "GetAngularVelocity");
MyRunGravityZEstimationSequence = (int(*)(ROBOT_TYPE, double OptimalzParam[OPTIMAL_Z_PARAM_SIZE])) dlsym(commandLayer_handle, "RunGravityZEstimationSequence");
MySwitchTrajectoryTorque = (int(*)(GENERALCONTROL_TYPE)) dlsym(commandLayer_handle, "SwitchTrajectoryTorque");
MySetTorqueSafetyFactor = (int(*)(float)) dlsym(commandLayer_handle, "SetTorqueSafetyFactor");
MySendAngularTorqueCommand = (int(*)(float Command[COMMAND_SIZE])) dlsym(commandLayer_handle, "SendAngularTorqueCommand");
MySendCartesianForceCommand = (int(*)(float Command[COMMAND_SIZE])) dlsym(commandLayer_handle, "SendCartesianForceCommand");
MySetGravityVector = (int(*)(float Command[3])) dlsym(commandLayer_handle, "SetGravityVector");
MySetGravityPayload = (int(*)(float Command[GRAVITY_PAYLOAD_SIZE])) dlsym(commandLayer_handle, "SetGravityPayload");
MySetGravityOptimalZParam = (int(*)(float Command[GRAVITY_PARAM_SIZE])) dlsym(commandLayer_handle, "SetGravityOptimalZParam");
MySetGravityType = (int(*)(GRAVITY_TYPE Type)) dlsym(commandLayer_handle, "SetGravityType");
MyGetAngularForceGravityFree = (int(*)(AngularPosition &)) dlsym(commandLayer_handle, "GetAngularForceGravityFree");
MyGetCartesianForce = (int(*)(CartesianPosition &)) dlsym(commandLayer_handle, "GetCartesianForce");
MySetTorqueVibrationController = (int(*)(float)) dlsym(commandLayer_handle, "SetTorqueVibrationController");
MySetTorqueControlType = (int(*)(TORQUECONTROL_TYPE)) dlsym(commandLayer_handle, "SetTorqueControlType");
MyMoveHome = (int(*)()) dlsym(commandLayer_handle, "MoveHome");
MySetTorqueActuatorDamping = (int(*)(float Command[COMMAND_SIZE])) dlsym(commandLayer_handle, "SetTorqueActuatorDamping");
MySetTorqueRobotProtection = (int (*)(int)) dlsym(commandLayer_handle,"SetTorqueRobotProtection");
MySendBasicTrajectory = (int (*)(TrajectoryPoint)) dlsym(commandLayer_handle,"SendBasicTrajectory");
//Verify that all functions has been loaded correctly
if ((MyInitAPI == NULL) || (MyCloseAPI == NULL) || (MyGetAngularCommand == NULL) || (MyGetAngularPosition == NULL)
|| (MySetActiveDevice == NULL) || (MyGetDevices == NULL) || (MyGetAngularVelocity == NULL))
{
cout << "*** Initialization Failed! ***" << endl;
programResult = 0;
}
else
{
result = (*MyInitAPI)();
int resultComm;
AngularPosition DataCommand;
// Get the angular command to test the communication with the robot
resultComm = MyGetAngularCommand(DataCommand);
string strResult = "Failed";
getResult(strResult,result);
cout << "Initialization's result :" << strResult << endl;
getResult(strResult,resultComm);
cout << "Communication result :" << strResult << endl;
cout << "current angular data: " << DataCommand.Actuators.Actuator1 << ", " << DataCommand.Actuators.Actuator2 <<
", " << DataCommand.Actuators.Actuator3 << ", " << DataCommand.Actuators.Actuator4 << ", " << DataCommand.Actuators.Actuator5 << ", " << DataCommand.Actuators.Actuator6 << endl;
// If the API is initialized and the communication with the robot is working
if (result == 1 && resultComm == 1)
{
cout << "API initialized: OK" << endl;
result = MySwitchTrajectoryTorque(POSITION);
getResult(strResult,result);
cout << "switched to position mode: " << strResult << endl;
// Get the reference position
cout << "sending the robot to the inital pose ..." << endl;
TrajectoryPoint trajectoryPoint;
trajectoryPoint.InitStruct();
trajectoryPoint.Position.Type = ANGULAR_POSITION; //226.287 202.525 97.8848 178.22 29.9561 75.1491 0.509246
trajectoryPoint.Position.Actuators.Actuator1 = -60;//-60;//-50;//-59;//-227;//-245; //312;
trajectoryPoint.Position.Actuators.Actuator2 = 180;//239;//180;//230;//205;//135; //164; //245;
trajectoryPoint.Position.Actuators.Actuator3 = 102;//112;//148;//139;//292;//300;//117;
trajectoryPoint.Position.Actuators.Actuator4 = 270;//314;//316;//160.6;//208;//-15;
trajectoryPoint.Position.Actuators.Actuator5 = 0;//480;//495;//273.6;//308;//61;
trajectoryPoint.Position.Actuators.Actuator6 = 90;//132;//118;//29;//0;//112 -59.7233 180.358 101.769 269.942
(*MySendBasicTrajectory)(trajectoryPoint);
usleep(5000000);
cout << "Done." << endl;
//result = (*MyCloseAPI)();
//return 0;
AngularPosition PositionReference;
MyGetAngularPosition(PositionReference);
cout << "Reference position: " << PositionReference.Actuators.Actuator1 << " ," << PositionReference.Actuators.Actuator2 << " ," << PositionReference.Actuators.Actuator3
<< " ," << PositionReference.Actuators.Actuator4 << " ," << PositionReference.Actuators.Actuator5 << " ," << PositionReference.Actuators.Actuator6 << endl;
//result = (*MyCloseAPI)();
//return 0;
cout << "Enter the filename to execute the trajectory from: " << endl;
string fileName;
getline(cin, fileName);
ifstream ifs;
ifs.open(fileName.c_str());
if (!ifs.is_open())
{
cout << "no such file! try again." << endl;
result = (*MyCloseAPI)();
return 0;
}
int count;
float j1,j2,j3,j4,j5,j6,x,y,z,tx,ty,tz;
float M1,M2,M3,M4,M5,M6,Fx,Fy,Fz,Mx,My,Mz;
ifs >> count >> j1 >> j2 >> j3 >> j4 >> j5 >> j6 >> x >> y >> z >> tx >> ty >> tz >> M1 >> M2 >> M3 >> M4 >> M5 >> M6 >> Fx >> Fy >> Fz >> Mx >> My >> Mz;
printf("First Desired Pose: [%f\t%f\t%f\t%f\t%f\t%f]\n", x,y,z,tx,ty,tz);
printf("First Desired Angle: [%f\t%f\t%f\t%f\t%f\t%f]\n", j1,j2,j3,j4,j5,j6);
std::string answer;
std::string yes = "y"; //.c_str();
result = MySwitchTrajectoryTorque(TORQUE);
getResult(strResult,result);
cout << "switched to torque mode: " << strResult << endl;
result = MySetTorqueSafetyFactor(0.6); //0.6
getResult(strResult,result);
cout << "safety factor set: " << strResult << endl;
result = MySetTorqueRobotProtection(1);
getResult(strResult,result);
cout << "Protection zones off: " << strResult << endl;
result = MySetTorqueVibrationController(0.5); //0.5
getResult(strResult,result);
cout << "vibration controller set: " << strResult << endl;
float ActuatorDamping[COMMAND_SIZE];
for (int i = 0; i < COMMAND_SIZE; i++){ActuatorDamping[i] = 0;}
ActuatorDamping[0] = 0.4;
ActuatorDamping[1] = 0.4;
ActuatorDamping[2] = 0.4;
ActuatorDamping[3] = 0.4;
ActuatorDamping[4] = 0.4;
ActuatorDamping[5] = 0.4;
result = MySetTorqueActuatorDamping(ActuatorDamping);
getResult(strResult,result);
cout << "Dampings are set: " << strResult << endl;
// Get the actual position
AngularPosition PositionActual;
// Initialize the torque command
float TorqueCommand[COMMAND_SIZE];
for (int i = 0; i < COMMAND_SIZE; i++){TorqueCommand[i] = 0;}
float Stiffness[COMMAND_SIZE];
for (int i = 0; i < COMMAND_SIZE; i++){Stiffness[i] = 0.5;} // initialize the stiffness to 0.5
Stiffness[0] = 0.4;
Stiffness[1] = 0.4;
Stiffness[2] = 0.4;
Stiffness[3] = 0.4;
Stiffness[4] = 0.4;
Stiffness[5] = 0.4;
cout << "Start executing the trajectory?";
getline(cin, answer);
if (answer.compare(yes) == 0 )
{
while (ifs.is_open() && ifs >> count >> j1 >> j2 >> j3 >> j4 >> j5 >> j6 >> x >> y >> z >> tx >> ty >> tz >> M1 >> M2 >> M3 >> M4 >> M5 >> M6 >> Fx >> Fy >> Fz >> Mx >> My >> Mz)
{
MyGetAngularPosition(PositionActual);
TorqueCommand[0] = 0; TorqueCommand[1] = 0; TorqueCommand[2] = 0; TorqueCommand[3] = 0; TorqueCommand[4] = 0; TorqueCommand[5] = 0;
cout << "Reference:" << j1 << "," << j2 << "," << j3 << "," << j4 << "," << j5 << "," << j6 << endl;
cout << "error:" << PositionActual.Actuators.Actuator1 - j1 << "," << PositionActual.Actuators.Actuator2 - j2 << "," << PositionActual.Actuators.Actuator3 - j3 << "," << PositionActual.Actuators.Actuator4 - j4 << "," << PositionActual.Actuators.Actuator5 - j5 << "," << PositionActual.Actuators.Actuator6 - j6 << endl;
TorqueCommand[0] = -Stiffness[0] * ((PositionActual.Actuators.Actuator1) - (j1));
TorqueCommand[1] = -Stiffness[1] * ((PositionActual.Actuators.Actuator2) - (j2));
TorqueCommand[2] = -Stiffness[2] * ((PositionActual.Actuators.Actuator3) - (j3));
TorqueCommand[3] = -Stiffness[3] * ((PositionActual.Actuators.Actuator4) - (j4));
TorqueCommand[4] = -Stiffness[4] * ((PositionActual.Actuators.Actuator5) - (j5));
TorqueCommand[5] = -Stiffness[5] * ((PositionActual.Actuators.Actuator6) - (j6));
MySendAngularTorqueCommand(TorqueCommand);
usleep(10000);
}
}
else
{
ifs.close();
result = (*MyCloseAPI)();
return 0;
}
result = MySetTorqueRobotProtection(2);
getResult(strResult,result);
cout << endl << "Protection zones on: " << strResult << endl;
result = MySwitchTrajectoryTorque(POSITION);
getResult(strResult,result);
cout << "switch to position mode: " << strResult << endl;
if (result != 1)
{
cout << "could not switch back to position mode." << endl;
}
// Set the damping back to zero
for (int i = 0; i < COMMAND_SIZE; i++){ActuatorDamping[i] = 0;}
result = MySetTorqueActuatorDamping(ActuatorDamping);
getResult(strResult,result);
cout << "set damping back to normal: " << strResult << endl;
}
int main()
{
int result;
bool singlePointTest, incrementalTest, trajectoryTest, homeTorqueTest, homePoseTest;
CartesianPosition data;
AngularPosition angles;
cout << "Executing trajectories using SendBasicTrajectory" << endl;
//Handle for the library's command layer.
void * commandLayer_handle;
//Function pointers to the functions we need
int (*MyInitAPI)();
int (*MyCloseAPI)();
int (*MySendBasicTrajectory)(TrajectoryPoint command);
int (*MyStartControlAPI)();
int (*MyMoveHome)();
int (*MyGetCartesianCommand)(CartesianPosition &);
int (*MyGetCartesianPosition)(CartesianPosition &);
int (*MyGetAngularPosition)(AngularPosition &);
//We load the library (Under Windows, use the function LoadLibrary)
commandLayer_handle = dlopen("Kinova.API.USBCommandLayerUbuntu.so",RTLD_NOW|RTLD_GLOBAL);
//We load the functions from the library (Under Windows, use GetProcAddress)
MyInitAPI = (int (*)()) dlsym(commandLayer_handle,"InitAPI");
MyCloseAPI = (int (*)()) dlsym(commandLayer_handle,"CloseAPI");
MySendBasicTrajectory = (int (*)(TrajectoryPoint)) dlsym(commandLayer_handle,"SendBasicTrajectory");
MyStartControlAPI = (int (*)()) dlsym(commandLayer_handle,"StartControlAPI");
MyGetCartesianCommand = (int (*)(CartesianPosition &)) dlsym(commandLayer_handle,"GetCartesianCommand");
MyGetCartesianPosition = (int (*)(CartesianPosition &)) dlsym(commandLayer_handle,"GetCartesianPosition");
MyGetAngularPosition = (int(*)(AngularPosition &)) dlsym(commandLayer_handle,"GetAngularPosition");
MyMoveHome = (int (*)()) dlsym(commandLayer_handle,"MoveHome");
//If the was loaded correctly
if((MyInitAPI == NULL) || (MyCloseAPI == NULL) || (MySendBasicTrajectory == NULL) || (MyStartControlAPI == NULL) ||
(MyMoveHome == NULL) || (MyGetCartesianCommand == NULL) || (MyGetCartesianPosition == NULL))
{
cout << "Unable to initialize the command layer." << endl;
}
else
{
result = (*MyInitAPI)();
result = (*MyGetCartesianPosition)(data);
cout << " Home: [ " << data.Coordinates.X << "," << data.Coordinates.Y << "," << data.Coordinates.Z << " ]" << endl;
usleep(3000);
TrajectoryPoint trajectoryPoint;
// MyMoveHome(); //TODO Uncomment if you want to start from this position at each test.
usleep(10000);
ifstream ifs;
ifs.open(TRAJECTORY_FILENAME);
if (!ifs.is_open())
cout << "no such file! try again." << endl;
int count=0;
double j1,j2,j3,j4,j5,j6,x,y,z,tx,ty,tz;
int iter=0;
while (ifs.is_open() && ifs >> j1 >> j2 >> j3 >> j4 >> j5 >> j6 /*&& iter<ITER_MAX*/)
{
printf("%i\t%f\t%f\t%f\t%f\t%f\t%f\n", count,j1,j2,j3,j4,j5,j6);
trajectoryPoint.InitStruct();
trajectoryPoint.Position.Type = ANGULAR_POSITION;
// get the current pose of the end-effector
trajectoryPoint.Position.Actuators.Actuator1 = j1*R2D;
trajectoryPoint.Position.Actuators.Actuator2 = j2*R2D;
trajectoryPoint.Position.Actuators.Actuator3 = j3*R2D;
trajectoryPoint.Position.Actuators.Actuator4 = j4*R2D;
trajectoryPoint.Position.Actuators.Actuator5 = j5*R2D;
trajectoryPoint.Position.Actuators.Actuator6 = j6*R2D;
(*MySendBasicTrajectory)(trajectoryPoint);
usleep(3000);
count++;
iter++;
}
ifs.close();
result = (*MyCloseAPI)();
}
return 0;
}
int main()
{
AngularPosition currentCommand;
int result;
cout << "SetCartesianForceMinMax function example" << endl;
//Handle for the library's command layer.
void * commandLayer_handle;
//Function pointers to the functions we need
int (*MyInitAPI)();
int (*MyCloseAPI)();
int (*MyStartForceControl)();
int (*MySetCartesianForceMinMax)(CartesianInfo, CartesianInfo); // min and max forces
int (*MySetCartesianInertiaDamping)(CartesianInfo, CartesianInfo); // inertia and damping
int (*MySetAngularInertiaDamping)(AngularInfo, AngularInfo);
int (*MySendBasicTrajectory)(TrajectoryPoint command);
int (*MyGetDevices)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result);
int (*MySetActiveDevice)(KinovaDevice device);
int (*MyMoveHome)();
int (*MyInitFingers)();
int (*MyGetAngularCommand)(AngularPosition &);
//We load the library (Under Windows, use the function LoadLibrary)
commandLayer_handle = dlopen("Kinova.API.USBCommandLayerUbuntu.so",RTLD_NOW|RTLD_GLOBAL);
//We load the functions from the library (Under Windows, use GetProcAddress)
MyInitAPI = (int (*)()) dlsym(commandLayer_handle,"InitAPI");
MyCloseAPI = (int (*)()) dlsym(commandLayer_handle,"CloseAPI");
MySetCartesianForceMinMax = (int (*)(CartesianInfo, CartesianInfo)) dlsym(commandLayer_handle,"SetCartesianForceMinMax");
MySetCartesianInertiaDamping = (int (*)(CartesianInfo, CartesianInfo))dlsym(commandLayer_handle,"SetCartesianInertiaDamping");
MySetAngularInertiaDamping = (int (*)(AngularInfo, AngularInfo)) dlsym(commandLayer_handle,"SetAngularInertiaDamping");
MyStartForceControl = (int(*)()) dlsym(commandLayer_handle,"StartForceControl");
MyMoveHome = (int (*)()) dlsym(commandLayer_handle,"MoveHome");
MyInitFingers = (int (*)()) dlsym(commandLayer_handle,"InitFingers");
MyGetDevices = (int (*)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result)) dlsym(commandLayer_handle,"GetDevices");
MySetActiveDevice = (int (*)(KinovaDevice devices)) dlsym(commandLayer_handle,"SetActiveDevice");
MySendBasicTrajectory = (int (*)(TrajectoryPoint)) dlsym(commandLayer_handle,"SendBasicTrajectory");
MyGetAngularCommand = (int (*)(AngularPosition &)) dlsym(commandLayer_handle,"GetAngularCommand");
//If the was loaded correctly
if((MyInitAPI == NULL) || (MyCloseAPI == NULL) || (MySendBasicTrajectory == NULL) ||
(MySendBasicTrajectory == NULL) || (MyMoveHome == NULL) || (MyInitFingers == NULL) ||
(MySetCartesianForceMinMax == NULL) || (MySetAngularInertiaDamping == NULL) || (MySetCartesianInertiaDamping == NULL))
{
cout << "* * * E R R O R D U R I N G I N I T I A L I Z A T I O N * * *" << endl;
cout << MyInitAPI << MyCloseAPI << MySendBasicTrajectory << MySendBasicTrajectory << MyMoveHome << MyInitFingers << MySetCartesianForceMinMax << MySetAngularInertiaDamping << MySetCartesianInertiaDamping << 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;
KinovaDevice list[MAX_KINOVA_DEVICE];
int devicesCount = MyGetDevices(list, result);
for(int i = 0; i < devicesCount; i++)
{
cout << "Found a robot on the USB bus (" << list[i].SerialNumber << ")" << endl;
//Setting the current device as the active device.
MySetActiveDevice(list[i]);
cout << "Send the robot to HOME position" << endl;
MyMoveHome();
TrajectoryPoint pointToSend;
pointToSend.InitStruct();
//We specify that this point will be an angular(joint by joint) position.
pointToSend.Position.Type = ANGULAR_POSITION;
//We get the actual angular command of the robot.
MyGetAngularCommand(currentCommand);
pointToSend.Position.Actuators.Actuator1 = currentCommand.Actuators.Actuator1 + 30;
pointToSend.Position.Actuators.Actuator2 = currentCommand.Actuators.Actuator2;
pointToSend.Position.Actuators.Actuator3 = currentCommand.Actuators.Actuator3;
pointToSend.Position.Actuators.Actuator4 = currentCommand.Actuators.Actuator4;
pointToSend.Position.Actuators.Actuator5 = currentCommand.Actuators.Actuator5;
pointToSend.Position.Actuators.Actuator6 = currentCommand.Actuators.Actuator6;
cout << "*********************************" << endl;
cout << "Sending the first point to the robot." << endl;
MySendBasicTrajectory(pointToSend);
CartesianInfo commandMin;
CartesianInfo commandMax;
commandMin.InitStruct();
commandMax.InitStruct();
commandMin.X = 8.0f;
commandMin.Y = 8.0f;
commandMin.Z = 8.0f;
commandMin.ThetaX = 1.0f;
commandMin.ThetaY = 1.0f;
commandMin.ThetaZ = 1.0f;
//float cmax = 200.0;
commandMax.X = 12.0f;
commandMax.Y = 15.0f;
commandMax.Z = 20.0f;
commandMax.ThetaX = 2.0f;
commandMax.ThetaY = 2.0f;
commandMax.ThetaZ = 2.0f;
result = (*MySetCartesianForceMinMax)(commandMin, commandMax);
cout << "Cartesian force min and max has been set: " << result << endl;
/*
CartesianInfo Inertia;
CartesianInfo Damping;
// not setting the intertia yet
float inertiaValue = 1.0; // km*m^2
Inertia.X = inertiaValue;
Inertia.Y = inertiaValue;
Inertia.Z = inertiaValue;
Inertia.ThetaX = inertiaValue;
Inertia.ThetaY = inertiaValue;
Inertia.ThetaZ = inertiaValue;
float dmpValue = 1.0; // N*s/m, N*s/RAD
Damping.X = dmpValue;
Damping.Y = dmpValue;
Damping.Z = dmpValue;
Damping.ThetaX = dmpValue;
Damping.ThetaY = dmpValue;
Damping.ThetaZ = dmpValue;
cout << "reached here" << endl;
//result = (*MySetCartesianInertiaDamping)(Inertia,Damping);
cout << "Cartesian inertial and damping values have been set." << endl;
*/
AngularInfo Inertia;
AngularInfo Damping;
Inertia.InitStruct();
Damping.InitStruct();
Inertia.Actuator1 = 0.006f;
Inertia.Actuator2 = 0.006f;
Inertia.Actuator3 = 0.005f;
Inertia.Actuator4 = 0.005f; //0.050f;
Inertia.Actuator5 = 0.005f; //0.050f;
Inertia.Actuator6 = 0.005f; //0.050f;
Damping.Actuator1 = 0.015f;
Damping.Actuator2 = 0.015f;
Damping.Actuator3 = 0.01f;
Damping.Actuator4 = 0.01f;
Damping.Actuator5 = 0.01f;
Damping.Actuator6 = 0.01f;
result = (*MySetAngularInertiaDamping)(Inertia,Damping);
cout << "Angular inertia and damping values have been set: " << result << endl;
cout << "Damping " << Damping.Actuator1 << " Inertia " << Inertia.Actuator1 << endl;
result = MyStartForceControl();
cout << "Force Control is started: " << result << endl;
}
cout << endl << "WARNING: Your robot is now set to angular control. If you use the joystick, it will be a joint by joint movement." << endl;
cout << endl << "C L O S I N G A P I" << endl;
result = (*MyCloseAPI)();
}
dlclose(commandLayer_handle);
return 0;
}
