/*

* Dynamic Helipad Control

*
* This module is the main control module for the Dynamic Helipad Project.
* It runs the threads that run the simulation and send the commands to
* the actual robot. All helicopter logic is implemented here. Associated
* control modules implement the orientation and position control.
*
* Joseph Lee
* Vikram Chauhan
* Adam Abdulhamid
* Kelsie Zhao
*/
#include <scl/DataTypes.hpp>
#include <scl/Singletons.hpp>
#include <scl/data_structs/SGcModel.hpp>
#include <scl/dynamics/scl/CDynamicsScl.hpp>
#include <scl/dynamics/tao/CDynamicsTao.hpp>
#include <scl/parser/sclparser/CParserScl.hpp>
#include <scl/graphics/chai/CGraphicsChai.hpp>
#include <scl/graphics/chai/ChaiGlutHandlers.hpp>
#include <scl/control/task/CControllerMultiTask.hpp>
#include <scl/control/task/tasks/data_structs/STaskOpPos.hpp>
//scl functions to simplify dynamic typing and data sharing
#include <scl/robot/DbRegisterFunctions.hpp>
#include <scl/util/DatabaseUtils.hpp>
#include <scl/util/HelperFunctions.hpp>
#include "CTaskOrientation.hpp"
#include "STaskOrientation.hpp"
#include <zmqpp/zmqpp.hpp>
#include "./optitrack/src/OptiTrack.h"
#include "./optitrack/src/exampleCode.h"
// Trajectory Generation
//#include "TypeIVRML1.2.8_Academic/include/ReflexxesAPI.h"
#include <ReflexxesAPI.h>
// Custom Task
#include "CTaskCustom.hpp"
#include "STaskCustom.hpp"
//For timing
#include <sutil/CSystemClock.hpp>
//Eigen 3rd party lib
#include <Eigen/Dense>
#include <Eigen/Geometry>
//Standard includes (for printing and multi-threading)
#include <iostream>
#include <omp.h>
// Math header
#include <cmath>
//Freeglut windowing environment
#include <GL/freeglut.h>
//#include <math/CMatrix3d.h>

#include <chai3d.h>
#define MESH_SCALE 0.001
#define PI 3.14159265359
// definitions
#define ZMIN_WS 0.8
#define ZMIN_INIT 0.5
#define Z_OFFSET 0.3
#define RMAX
0.8
#define RMIN
0.5
#define C
0.9
#define REDUCE_CYL_OUT 0.1
#define REDUCE_CYL_IN 0.11
// trajectory generation
#define VMAX 1
#define AMAX 7
#define JMAX 7
#define VMAX_Z 0.05
#define ERROR_ZONE 0.2
// Optitrack offsets
#define X_OPTITRACK_OFFSET -0.84
#define Y_OPTITRACK_OFFSET 0.95
#define Z_OPTITRACK_OFFSET 0.05
#define LANDING_THRESHOLD 0.1
// Module level variables
scl::SDatabase *db;

// Position states
// default: followKeys
// KEY 1: followKeys
// KEY 2: followOptitrack
// KEY 3: followNothing
typedef enum { FOLLOW_KEYS, FOLLOW_OPTITRACK, FOLLOW_NOTHING} PositionState;
typedef enum { ORIENTATION_UP, ORIENTATION_DOWN} OrientationState;
bool sendRobot = false; // variable to know if we are sending commands to robot
// function Prototypes
//void updateOptitrack();
PositionState checkPositionKeys(void);
OrientationState checkOrientationKey(void);
void checkWSBoundary(Eigen::Vector3d& x_goal);
void toJointLimits(Eigen::VectorXd &q);
/** A sample application to demonstrate how to set up your own task in SCL...
int main(int argc, char** argv)
{
std::cout << "\n***********************************************\n";
std::cout << "Standard Control Library Tutorial : Custom Tasks";
std::cout << "\n***********************************************\n";

*/

bool flag;
bool zmqIntialized = false;
scl::SRobotParsed rds;
//Robot data structure.... //node names, actuators,
joint limits, damping..
scl::SGraphicsParsed rgr; //Robot graphics data structure...
scl::SGcModel rgcm; //Robot data structure with dynamic quantities... //M, q, dq,
transformation matrices, every important things
scl::SRobotIO rio;
//I/O data structure
scl::CGraphicsChai rchai; //Chai interface (updates graphics rendering tree
etc.)
scl::CDynamicsScl dyn_scl; //Robot kinematics and dynamics computation object...
compute Transformation matrices, Jacobians
scl::CDynamicsTao dyn_tao; //Robot physics integrator.. compute Position and
Velocity
scl::CParserScl p;
//This time, we'll parse the tree from a file...
scl::SControllerMultiTask rctr_ds; //A multi-task controller data structure
//list all tasks
scl::CControllerMultiTask rctr;
//A multi-task controller
std::vector<scl::STaskBase*> rtasks;
//A set of executable tasks
std::vector<scl::SNonControlTaskBase*> rtasks_nc; //A set of non-control tasks
std::vector<scl::sString2> ctrl_params;
//Used to parse extra xml tags
scl::STaskOpPos* rtask_hand; //Will need to set hand desired positions etc.
sutil::CSystemClock::start(); //Start the clock
/******************************Set up Dynamic Type for our new
Task************************************/
scl_app::STaskOrientation *rtask_Ori; //A pointer for our new task
flag = scl_app::registerType_TaskOrientation(); //Let SCL parse our new task
type..
if (false == flag) {
std::cout << "\n Could not register custom task type\n";
return 1;
}
// NOTE : The task hand name should match the XML Config file.
std::string Ori_parent_link("Ori");
/******************************Set up Shared Memory
(Database)************************************/
// This will us share data between the GUI and the controller
db = scl::CDatabase::getData();
if (NULL == db) {
std::cout << "\n ERROR : Could not initialize global data storage";
return 1;
}
scl_util::getCurrentDir(db->cwd_);
db->dir_specs_ = db->cwd_ + std::string("./");
/******************************Load Robot
Specification************************************/
//We will use a slightly more complex xml spec than the first few tutorials
const std::string fname("./iiwaCfg.xml");
flag = p.readRobotFromFile(fname, "./", "iiwaBot", rds);
flag = flag && rgcm.init(rds);
//Simple way to set up dynamic tree...
flag = flag && dyn_tao.init(rds);
//Set up integrator object
flag = flag && dyn_scl.init(rds);
//Set up kinematics and dynamics object
flag = flag && rio.init(rds);
//Set up the I/O data structure

for (unsigned int i = 0; i < rds.dof_; ++i) {

rio.sensors_.q_[i] = rds.rb_tree_.at(i)->joint_default_pos_;
}
if (false == flag) {
return 1;
}
//Error check.
/******************************Set up Controller
Specification************************************/
// Read xml file info into task specifications.
flag = p.readTaskControllerFromFile(fname, "opc", rtasks, rtasks_nc,
ctrl_params);
flag = flag && rctr_ds.init("opc", &rds, &rio, &rgcm); //Set up the control data
structure..
// Tasks are initialized after find their type with dynamic typing.
flag = flag && scl_registry::registerNativeDynamicTypes();
flag = flag && scl_util::initMultiTaskCtrlDsFromParsedTasks(rtasks, rtasks_nc,
rctr_ds);
flag = flag && rctr.init(&rctr_ds, &dyn_scl);
//Set up the controller
(needs parsed data and a dyn object)
if (false == flag) {
return 1;
}
//Error check.
rtask_hand = dynamic_cast<scl::STaskOpPos*>(*(rctr_ds.tasks_.at("hand")));
if (NULL == rtask_hand) {
return 1;
}
//Orientation task
rtask_Ori =
dynamic_cast<scl_app::STaskOrientation*>(*(rctr_ds.tasks_.at(Ori_parent_link)));
if (NULL == rtask_Ori) {
return 1;
}
/******************************ChaiGlut
Graphics************************************/
glutInit(&argc, argv); // We will use glut for the window pane (not the
graphics).
flag = p.readGraphicsFromFile(fname, "iiwaBotStdView", rgr);
flag = flag && rchai.initGraphics(&rgr);
flag = flag && rchai.addRobotToRender(&rds, &rio);
flag = flag && scl_chai_glut_interface::initializeGlutForChai(&rgr, &rchai);
if (false == flag) {
std::cout << "\nCouldn't initialize chai graphics\n";
return 1;
}
/****************************** VISION **************************************/
initOptitrack();
/******************************Simulation************************************/
std::cout
<< "\nIntegrating the robot's physics. \nWill test two different
controllers.\n Press (x) to exit at anytime.\n";
long long iter = 0;
double dt = 0.001;

/* ------------------------------------------- GRAPHICS/KEYSTROKE INIT---------------------------------------------------*/

/*std::cout << "rtask initial orientation is:\n" << rtask_Ori->Ori << std::endl;
std::cout << "rtask initial Pos is:\n" << rtask_hand->x_ << std::endl;*/
//compute once to initialize position and orientation.
rctr.computeDynamics();
rctr.computeControlForces();
/*std::cout << "rtask after 1 computation: orientation is:\n" << rtask_Ori->Ori
<< std::endl;
std::cout << "rtask after 1 computation: Pos is:\n" << rtask_hand->x_ <<
std::endl;*/
// Initial user input (target ball) position
//Eigen::Vector3d targetInitialPos(0.25, -0.25, 1);
db->s_gui_.ui_point_[0] = rtask_hand->x_;
Eigen::Matrix3d rot;
rot = rtask_Ori->Ori;
std::cout << "Initial Orienation: " << rot << "\n";
// Draw ball at initial position
chai3d::cGenericObject *target;
Eigen::Vector3d sphere_pos(db->s_gui_.ui_point_[0][0], db>s_gui_.ui_point_[0][1], db->s_gui_.ui_point_[0][2]); //(0.5, 0.5, 0.5)
Eigen::Vector3d sphere_vel(0, 0, 0);
flag = rchai.addSphereToRender(sphere_pos, target, 0.05);
if (false == flag) {
std::cout << "\nCould not load the sphere for Target";
return 1;
}
// Draw plate at initial position and orientation
//Eigen::Matrix3d plateInitRot = MESH_SCALE * Eigen::Matrix3d::Identity();
flag = rchai.addMeshToRender("platform", "./graphics/platform2.obj", rtask_hand>x_, rtask_Ori->Ori);
if (false == flag) {
std::cout << "\nCould not load the platform";
return 1;
}
scl::SGraphicsChaiMesh *plate = rchai.getChaiData()>meshes_rendered_.at("platform");
/******************************TRAJECTORY
INITIALIZATION************************************/
ReflexxesAPI *RML = NULL;
RMLPositionInputParameters *IP = NULL;
RMLPositionOutputParameters *OP = NULL;
RMLPositionFlags Flags;
// Note: this dof is end effector position dof, not robot dof; will be moved in
future release

int dof = 3;
// Initialize variables
RML = new ReflexxesAPI(dof, dt);
IP = new RMLPositionInputParameters(dof);
OP = new RMLPositionOutputParameters(dof);
// Set initial values for Traj Gen
// Initial position for Traj Gen, should be the same as initial position of end
effector
bool selectVec[3] = {true, true, true};
for (int i = 0; i < dof; i++) {
//IP->CurrentPositionVector->VecData[i] = plateInitPos[i];
IP->CurrentPositionVector->VecData[i] = rtask_hand->x_[i];
IP->CurrentVelocityVector->VecData[i] = 0;
IP->CurrentAccelerationVector->VecData[i] = 0;
IP->MaxVelocityVector->VecData[i] = VMAX;
IP->MaxAccelerationVector->VecData[i] = AMAX;
IP->MaxJerkVector->VecData[i] = JMAX;
IP->SelectionVector->VecData[i] = selectVec[i];
//setting OP pos for initialization purpose; don't have to set vel and acc coz
initial value is 0
//OP->NewPositionVector->VecData[i] = plateInitPos[i];
OP->NewPositionVector->VecData[i] = rtask_hand->x_[i];
}
/*********************************** Threading Starts Below
*************************************/
omp_set_num_threads(2);
int thread_id;
double tstart, tcurr, tCelebrate;
flag = false;
int dflag = 0;
bool celebrateFlag;
double cosineParam, controlPos;
//std::cout << rio.sensors_.q_.transpose() << std::endl;
// PRINT STATEMENTS FOR USER
std::cout << "Press 1 to FOLLOW KEYS" << std::endl
<< "Press 2 to FOLLOW OPTITRACK" << std::endl
<< "Press 3 to HOLD POSITION (FOLLOW NOTHING)" << std::endl
<< std::endl
<< "Press 0 to FLIP ORIENTATION" << std::endl
<< std::endl
<< "Press 5 to SEND TO ROBOT" << std::endl;
#pragma omp parallel private(thread_id)
{
thread_id = omp_get_thread_num();
if (thread_id == 1) {
tstart = sutil::CSystemClock::getSysTime();

iter = 0;
while (true == scl_chai_glut_interface::CChaiGlobals::getData()>chai_glut_running) {
tcurr = sutil::CSystemClock::getSysTime();
/*************************************** start of POSITION stuff
*****************************/
// READ ALL KEY STROKES for position command
static Eigen::Vector3d x_goal_temp;
// temp vector to start
x_goal_temp
Eigen::Vector3d x_goal_debug;
static bool lastYPlus = db->s_gui_.ui_flag_[8];
bool currYPlus = db->s_gui_.ui_flag_[8];
Eigen::Vector3d endEffectorPos = rtask_hand->x_;
Eigen::Vector3d endEffectorErr;
bool inErrorZone = false;
double cur_x, cur_y, cur_z;
// read key strokes to determine states
PositionState X_GOAL_STATE= checkPositionKeys();
// swtich statement to determine x_goal_temp
switch(X_GOAL_STATE)
{
case FOLLOW_KEYS:
// assign UI points to x_goal_temp
for (int i = 0; i < 3; i++)
{
x_goal_temp[i] = db->s_gui_.ui_point_[0][i];
}
break;
case FOLLOW_OPTITRACK:
// assign Optitrack position to x_goal_temp
if (iter%5==0) // update every 5 ms (each control loop is 1ms)
UpdateOptitrack();
// Updates optitrack to
update new target position
double* currentPosition;
currentPosition = getPositionOpti();
// get current Position of
target
// If we are potentially allowed to celebrate
if(celebrateFlag){
tCelebrate = tcurr - tstart;
if(tCelebrate>5){
//amp = (endEffectorPos[2] - ZMIN_WS)/2;
//avg = (endEffectorPos[2] + ZMIN_WS)/2;
cosineParam = 1*(tCelebrate);
//controlPos = (amp)*cos(cosineParam) + avg;
controlPos = (0.2)*cos(cosineParam) + 1.0;
//std::cout << endEffectorPos[0] << " " << endEffectorPos[1] << "
" << endEffectorPos[2] << " " << std::endl;

inErrorZone = false;
// Put copter in
x_goal_temp[2] =
x_goal_temp[0] =
x_goal_temp[1] =

restart position
controlPos;
0.068;
0.50;

}
}
else {
tstart = tcurr;
tCelebrate = 0;
}

// Stop robot if we think we have landed

//if (iter%100==0) {
if((currentPosition[1] - rtask_hand->x_[2]) <= LANDING_THRESHOLD) {
celebrateFlag = true;
break;
} else {
celebrateFlag = false;
}
//}
// assign Optitrack position to x_goal_temp
// Note: axis are different. Offset and axis reassignment is necessary
x_goal_temp[0] = currentPosition[2] - X_OPTITRACK_OFFSET; //-0.79;
x_goal_temp[1] = currentPosition[0] - Y_OPTITRACK_OFFSET; //1.03;
x_goal_temp[2] = currentPosition[1] - Z_OPTITRACK_OFFSET; // This
offset is artificial to make end effector not hit the target
// if we are in goal zone, then track opti very fast
endEffectorErr = endEffectorPos-x_goal_temp;
if( endEffectorErr[0] > -ERROR_ZONE && endEffectorErr[0] < ERROR_ZONE
&&
endEffectorErr[1] > -ERROR_ZONE && endEffectorErr[1] < ERROR_ZONE
&&
endEffectorErr[2] > -ERROR_ZONE && endEffectorErr[2] < ERROR_ZONE)
{
//std::cout << "IN ERROR ZONE" << std::endl;
inErrorZone = true;
}
else
{
//std::cout << "OUTSIDE ERROR ZONE" << std::endl;
inErrorZone = false;
}

//std::cout << "In Optitrack\nx_goal:" << x_goal_temp.transpose() <<

std::endl;
break;

case FOLLOW_NOTHING:
// do nothing
// static x_goal_temp will remain the same as previous iteration
break;
default:
;
}
// make sure x_goal_temp is within workspace boundaries
checkWSBoundary(x_goal_temp);
// DEBUG: print x_goal_temp
// if (iter % 20 == 0)
//
std::cout << "x_goal_temp: " << x_goal_temp.transpose() << std::endl;
// assign position parameters for trajectory generation
for (int i = 0; i < 3; i++)
{
IP->TargetPositionVector->VecData[i] = x_goal_temp[i];
}
*IP->CurrentPositionVector = *OP->NewPositionVector;
*IP->CurrentVelocityVector = *OP->NewVelocityVector;
*IP->CurrentAccelerationVector = *OP->NewAccelerationVector;
// Calling the Reflexxes OTG algorithm
int ResultValue = RML->RMLPosition(*IP, OP, Flags);
if (ResultValue < 0) {
printf("An error occurred (%d).\n", ResultValue);
break;
}
if (!inErrorZone)
{
//Set goal position
for (int i = 0; i < dof; i++) {
x_goal_temp[i] = OP->NewPositionVector->VecData[i];
}
}
// write x_goal_temp
rtask_hand->x_goal_ = x_goal_temp;

/*************************************** end of POSITION stuff

*****************************/
/*************************************** start ORIENTATION stuff
***************************/
// declare temporary goal orientation
static Eigen::Matrix3d Ori_goal_temp;

static double OriStep = PI;

const double ORI_INC = 0.001;
OrientationState ORIENTATION_STATE = checkOrientationKey();
switch (ORIENTATION_STATE)
{
case ORIENTATION_UP:
if (OriStep > 0)
// decrement OriStep if not identity
OriStep -= ORI_INC;
else
OriStep = 0;
break;
case ORIENTATION_DOWN:
if (OriStep < PI)
OriStep += ORI_INC;
else
OriStep = PI;
break;
default:
;
}
Ori_goal_temp << cos(OriStep), 0 , sin(OriStep),
0 , 1 ,
0,
-sin(OriStep), 0 , cos(OriStep);
// Write to Ori_goal & x_goal
rtask_Ori->Ori_goal = Ori_goal_temp;
/****************************** end ORIENTATION stuff
**************************************/
// Compute control forces (note that these directly have access to the io
data ds).
rctr.computeDynamics();
rctr.computeControlForces();
// Integrate the dynamics
dyn_tao.integrate(rio, dt);
//std::cout << rio.sensors_.q_.transpose() <<std::endl;
//std::cout << rtask_Ori->Ori <<std::endl;
//getchar();
iter++;
//press 5 to send to physical robot
if (db->s_gui_.ui_flag_[5])
{
//std::cout << "Sending to robot" << std::endl;
//send to robot
sendRobot = true;

if (iter%5 == 0)
{
static zmqpp::context context;
static zmqpp::socket pub(context, zmqpp::socket_type::publish);
if (!zmqIntialized)
{
// Need to pair this with the endpoint port in ROS code
pub.bind("tcp://*:3883");
zmqIntialized = true;
}
// TODO: Is data lock needed here?
else
{
zmqpp::message msg;
Eigen::VectorXd q;
//q = rctr_ds.getGeneralizedCoordinates();
q = rio.sensors_.q_;
toJointLimits(q);
//std::cout << q.transpose() << std::endl;
msg << std::to_string(q[0]) + " " +
std::to_string(q[1]) + " " +
std::to_string(q[2]) + " " +
std::to_string(-q[3]) + " " +
std::to_string(q[4]) + " " +
std::to_string(q[5]) + " " +
std::to_string(q[6]);

// joint 3 is inverted

pub.send(msg);
//std::cout << "Sending" << q.transpose() << "\n";
}
}
}
else
sendRobot = false;
/* ------------------------------------------- UI Update ---------------------------------------------------*/
// Update the position of the target GUI element
sphere_pos[0] = db->s_gui_.ui_point_[0](0);
sphere_pos[1] = db->s_gui_.ui_point_[0](1);
sphere_pos[2] = db->s_gui_.ui_point_[0](2);
// Update graphics
target->setLocalPos(sphere_pos[0], sphere_pos[1], sphere_pos[2]);

//use current hand pos to render plate. <-- use this so the plate does not
drift
plate->graphics_obj_->setLocalPos(rtask_hand->x_[0],
rtask_hand->x_[1], rtask_hand->x_[2] - 0.02);
// use current hand orientation to render plate <-- use this so the drawing
reflects current end-effector orientation.
chai3d::cMatrix3d plateCurrOri(

MESH_SCALE * rtask_Ori->Ori(0, 0), MESH_SCALE *

MESH_SCALE * rtask_Ori->Ori(0, 2),
MESH_SCALE * rtask_Ori->Ori(1, 0), MESH_SCALE *
MESH_SCALE * rtask_Ori->Ori(1, 2),
MESH_SCALE * rtask_Ori->Ori(2, 0), MESH_SCALE *
MESH_SCALE * rtask_Ori->Ori(2, 2));
plate->graphics_obj_->setLocalRot(plateCurrOri); //
method

rtask_Ori->Ori(0, 1),
rtask_Ori->Ori(1, 1),
rtask_Ori->Ori(2, 1),
<----No set global

/* Testing plate rotation, rotate in sine wave*/

/*chai3d::cMatrix3d temp2(0.001, 0, 0,
0, 0.001*cos(0.1*(tcurr-tstart)), -0.001* sin(0.1*(tcurrtstart)),
0, 0.001*sin(0.1*(tcurr-tstart)), 0.001*cos(0.1*(tcurrtstart)));
plate->graphics_obj_->setLocalRot(temp2);*/
/*if (iter % 100 == 0) {
std::cout << "Rotation: \n" << rtask_Ori->Ori << "\n";
}*/
/******************************* Update Time Step
************************************/
/** Slow down sim to real time */
sutil::CSystemClock::tick(dt);
double tcurr = sutil::CSystemClock::getSysTime();
double tdiff = sutil::CSystemClock::getSimTime() - tcurr;
timespec ts = { 0, 0 };
if (tdiff > 0) {
ts.tv_sec = static_cast<int>(tdiff);
tdiff -= static_cast<int>(tdiff);
ts.tv_nsec = tdiff * 1e9;
nanosleep(&ts, NULL);
}
/*
if (iter%100 == 0)
{
std::cout << sutil::CSystemClock::getSysTime() << std::endl;
}
*/
}
//Then terminate
scl_chai_glut_interface::CChaiGlobals::getData()->chai_glut_running = false;
}
else
//Read the rio data structure and updated rendererd robot..nbObjects
while (true == scl_chai_glut_interface::CChaiGlobals::getData()>chai_glut_running) {
glutMainLoopEvent();
const timespec ts =
{ 0, 15000000 };/*15ms*/
nanosleep(&ts, NULL);
}
}
/******************************Exit
Gracefully************************************/
std::cout << "\n\n\tSystem time = " << sutil::CSystemClock::getSysTime() -

tstart;
std::cout << "\n\tSimulated time = " << static_cast<double>(iter) * dt;
std::cout << "\n\nExecuted Successfully";
std::cout << "\n**********************************\n" << std::flush;
return 0;
}

// Function checkPositionKeys()
// Description: function to read Keys 1,2,3. If a key is pressed, it will return
// the corresponding state. If no key is pressed, it will return its previous
state (through
// the use of the static variable ReturnState)
//
//
//
//

default: followKeys
KEY 1: followKeys
KEY 2: followOptitrack
KEY 3: followNothing

PositionState
{
static bool
static bool
static bool

checkPositionKeys(void)
LastFollowKeysFlag;
LastFollowOptitrackFlag;
LastFollowNothingFlag;

bool CurFollowKeysFlag = db->s_gui_.ui_flag_[1];

bool CurFollowOptitrackFlag = db->s_gui_.ui_flag_[2];
bool CurFollowNothingFlag = db->s_gui_.ui_flag_[3];
static PositionState ReturnState = FOLLOW_KEYS;
ReturnState

// default value for

// If FollowKeys Flag has changed

if (CurFollowKeysFlag != LastFollowKeysFlag)
{
ReturnState = FOLLOW_KEYS;
LastFollowKeysFlag = CurFollowKeysFlag;
std::cout << "\nPosition Command: Follow Keys\n";
}
// if FollowOptitrack Flag has changed
else if (CurFollowOptitrackFlag != LastFollowOptitrackFlag)
{
ReturnState = FOLLOW_OPTITRACK;
// protection
//ReturnState = FOLLOW_KEYS;
LastFollowOptitrackFlag = CurFollowOptitrackFlag;
std::cout << "\nPosition Command: Follow Optitrack\n";
}
// if FollowNothing Flag has changed
else if (CurFollowNothingFlag != LastFollowNothingFlag)
{
ReturnState = FOLLOW_NOTHING;
LastFollowNothingFlag = CurFollowNothingFlag;
std::cout << "\nPosition Command: Follow Nothing\n";

}
return ReturnState;
}
// Function: checkOrientationKEY()
// Description: checkts keystrock for change in Orientation Key
// returns OrientationState for whether to flip orientation up or down
// USES KEY 0
OrientationState checkOrientationKey(void)
{
static bool LastOrientationFlag;
bool CurOrientationFlag = db->s_gui_.ui_flag_[0];
// Initialize to ORIENTATION_DOWN
static OrientationState ReturnState = ORIENTATION_DOWN;
// If FollowKeys Flag has changed
if (CurOrientationFlag != LastOrientationFlag)
{
if (ReturnState == ORIENTATION_DOWN)
ReturnState = ORIENTATION_UP;
else
ReturnState = ORIENTATION_DOWN;
LastOrientationFlag = CurOrientationFlag;
}
return ReturnState;
}
// new spherical WS Boundary
void checkWSBoundary(Eigen::Vector3d& x_goal)
{
const double A = RMAX;
const double B = RMAX;
double x = x_goal[0];
double y = x_goal[1];
double z = x_goal[2] - Z_OFFSET;

// must shift coordinates up

//static bool enteringCyl = false;

static bool outsideCyl = true;
static bool insideCyl = false;
// Check hysteresis
if(((x*x)+(y*y) > (RMAX*RMAX - REDUCE_CYL_OUT))){
outsideCyl = true;
insideCyl = false;
//enteringCyl = true;
} else if((((x*x)+(y*y)) < (RMAX*RMAX - REDUCE_CYL_IN))){
outsideCyl = false;
insideCyl = true;
//enteringCyl = false;
}

// check to see if outside of workspace

// If the drone is outside of ellipsoid and outside cylinder
if ( (((x*x)/(A*A) + (y*y)/(B*B) + (z*z)/(C*C)) > 1) && outsideCyl)
{
// calculate theta = (atan(y/x))
double theta = atan(y/x);
// determine quadrant
int Quadrant = 0;
if (x >= 0 && y >= 0)
Quadrant = 1;
else if (x < 0 && y >= 0)
Quadrant = 2;
else if (x < 0 && y < 0)
Quadrant = 3;
else if (x >= 0 && y < 0)
Quadrant = 4;
// calculate phi = (acos(z/r))
// since phi is always between 0 and 180 we don't need to check quadrants
double phi = acos(z/(sqrt(x*x+y*y+z*z)));
// determine x & y goal from trig
switch (Quadrant)
{
case 1:
x = A*cos(theta)*sin(phi);
y = B*sin(theta)*sin(phi);
z = C*cos(phi);
break;
case 2:
x = -A*cos(theta)*sin(phi);
y = -B*sin(theta)*sin(phi);
z = C*cos(phi);
break;
case 3:
x = -A*cos(theta)*sin(phi);
y = -B*sin(theta)*sin(phi);
z = C*cos(phi);
break;
case 4:
x = A*cos(theta)*sin(phi);
y = B*sin(theta)*sin(phi);
z = C*cos(phi);
break;
default:
;
}
} else if ( (((x*x)/(A*A) + (y*y)/(B*B) + (z*z)/(C*C)) > 1) && insideCyl) {
z = (C*sqrt(A*A*B*B-A*A*y*y-B*B*x*x))/(A*B);
}
else if ( (x*x + y*y + z*z) < (RMIN*RMIN))
{
// calculate theta = (atan(y/x))
double theta = atan(y/x);
// determine quadrant

int Quadrant = 0;
if (x >= 0 && y >= 0)
Quadrant = 1;
else if (x < 0 && y >= 0)
Quadrant = 2;
else if (x < 0 && y < 0)
Quadrant = 3;
else if (x >= 0 && y < 0)
Quadrant = 4;
// calculate phi = (acos(z/r))
// since phi is always between 0 and 180 we don't need to check quadrants
double phi = acos(z/(sqrt(x*x+y*y+z*z)));
// determine x & y goal from trig
switch (Quadrant)
{
case 1:
x = RMIN*cos(theta)*sin(phi);
y = RMIN*sin(theta)*sin(phi);
z = RMIN*cos(phi);
break;
case 2:
x = -RMIN*cos(theta)*sin(phi);
y = -RMIN*sin(theta)*sin(phi);
z = RMIN*cos(phi);
break;
case 3:
x = -RMIN*cos(theta)*sin(phi);
y = -RMIN*sin(theta)*sin(phi);
z = RMIN*cos(phi);
break;
case 4:
x = RMIN*cos(theta)*sin(phi);
y = RMIN*sin(theta)*sin(phi);
z = RMIN*cos(phi);
break;
default:
;
}
}
// CHECK Z minimum BOUNDS
z += Z_OFFSET;
if (sendRobot)
{
if (z<ZMIN_WS)
z = ZMIN_WS;
}
else
{
if (z < ZMIN_INIT)
z = ZMIN_INIT;
}
// reassign x to x_goal
x_goal[0] = x;
x_goal[1] = y;

x_goal[2] = z;
} // end CheckWSBoundary
// more WS
void toJointLimits(Eigen::VectorXd &q) {
q = 180*q/PI;
if(q[0] < -160) {
q[0] = -160;
} else if (q[0]>160){
q[0] = 160;
}
if(q[1] < -110) {
q[1] = -110;
} else if (q[1]>110){
q[1] = 110;
}
if(q[2] < -160) {
q[2] = -160;
} else if (q[2]>160){
q[2] = 160;
}
if(q[3] < -110) {
q[3] = -110;
} else if (q[3]>110){
q[3] = 110;
}
if(q[4] < -160) {
q[4] = -160;
} else if (q[4]>160){
q[4] = 160;
}
if(q[5] < -110) {
q[5] = -110;
} else if (q[5]>110){
q[5] = 110;
}
if(q[6] < -165) {
q[6] = -165;
} else if (q[6]>165){
q[6] = 165;
}
q = PI*q/180;
}