// this is a debugging function used to see what paths are available from a given point on the path
// calling this function repeatedly cycles throught the shortest path
void pathPlan::togglePathPoint(int dir)
{
if(m_GP.points.isEmpty()) return;
if(m_CS) delete m_CS;
m_CS = 0;
hitPoints.clear();
contactPoints.clear();
m_spinDirection = 0;
m_linkViewIndex += dir; // increment the index
if(m_linkViewIndex >= m_GP.points.size()) m_linkViewIndex = 0; // make sure the index is in bounds
else if(m_linkViewIndex < 0) m_linkViewIndex = m_GP.points.size()-1;
if(!m_displayDebug) displayDebug(true); // turn on debug drawing
rankPoint here = m_GP.points[m_linkViewIndex]; // get the point to view from using the index
here.object = 0;
m_CS = new cSpace(here.point,m_range,m_margin,m_blocks,m_view); // create a new Configuration Space based on the start point
contactPoints = getVisablePointsFrom(here); // gather all objects extreme vertices
if(!contactPoints.isEmpty()) hitPoints << contactPoints[0].point; // show the most likely path choice
contactPoints.prepend(m_GP.points[m_linkViewIndex]); // push the point the current view is from for drawing
if(m_view) {
m_CS->drawCspace(true);
m_view->getCamera()->cameraSetDirection(here.point); // set the camera view to the path point
m_view->updateGL();
}
}
/////////////////////////////////////////
// Path Debugging
/////////////
void pathPlan::togglePathReset()
{
displayPath(true);
displayBuildPath(false);
displayDebug(false);
if(m_CS) delete m_CS;
m_CS = 0;
hitPoints.clear();
contactPoints.clear();
}
void MainWindow::connectToHost()
{
client = new AnonymousClient(this);
connect(client, SIGNAL(sslErrors(QList<QSslError>)), SLOT(sslErrors(QList<QSslError>)));
connect(client, SIGNAL(messageAvailable(QString)), SLOT(displayMessage(QString)));
connect(client, SIGNAL(debugAvailable(QString)), debugConsole, SLOT(displayDebug(QString)));
connect(client, SIGNAL(setPhase0()), SLOT(phase0()));
connect(client, SIGNAL(setPhase1()), SLOT(phase1()));
connect(client, SIGNAL(disconnected()), SLOT(disconnectFromhost()));
connectButton->setEnabled(false);
disconnectButton->setEnabled(true);
displayMessage(tr("Connecting..."));
client->connectToHostEncrypted(settings->value("HostAddress", "localhost").toString(), settings->value("port", 9001).toInt());
}
void CUI::drawUI(void)
{
if (data->debugMode)
{
float color[4] = { 1.0f, 0.0f, 0.0f, 1.0f };
glLightModelfv(GL_LIGHT_MODEL_AMBIENT, color);
}
else
{
float color[4] = { 1.0f, 1.0f, 1.0f, 1.0f };
glLightModelfv(GL_LIGHT_MODEL_AMBIENT, color);
}
//reset light
float amb[] = { 1.0f, 1.0f, 1.0f };
float dif[] = { 0.0f, 0.0f, 0.0f };
float spe[] = { 0.0f, 0.0f, 0.0f };
glMaterialfv(GL_FRONT, GL_AMBIENT, amb);
glMaterialfv(GL_FRONT, GL_DIFFUSE, dif);
glMaterialfv(GL_FRONT, GL_SPECULAR, spe);
glDisable(GL_DEPTH_TEST);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(0.0, glutGet(GLUT_WINDOW_WIDTH), glutGet(GLUT_WINDOW_HEIGHT), 0.0, -1.0, 10.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glClear(GL_DEPTH_BUFFER_BIT);
glColor3f(0.1f, 0.1f, 0.1f);
displayHUD();
displayFPS();
displayDebug();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glEnable(GL_DEPTH_TEST);
textLines = 0;
}
pathPlan::pathPlan(obstacles *obs,simGLView* glView)
:
simGLObject(glView),
m_blocks(obs),
m_CS(NULL),
m_range(0),
m_margin(SPACEMARGIN),
m_step(0.25),
m_visibilityType(false),
m_goalOccluded(NULL),
m_efficiencyLimit(0.3),
m_spinProgress(6),
m_spinProgressBase(0),
m_state(PS_SEARCHING),
m_drawSwitch(true),
m_linkViewIndex(0)
{
// create callbacks to all the drawing methods, these are added to/removed from the display list
// make sure these remain in order to match with the enumeration
ACallback<pathPlan> drawCB(this, &pathPlan::drawDebugPath); m_drawingList << drawCB;
drawCB.SetCallback(this,&pathPlan::drawCrowFlyLine); m_drawingList << drawCB;
drawCB.SetCallback(this,&pathPlan::drawCurrentSearchPath); m_drawingList << drawCB;
drawCB.SetCallback(this,&pathPlan::drawRangeFan); m_drawingList << drawCB;
drawCB.SetCallback(this,&pathPlan::drawPathBaseLine); m_drawingList << drawCB;
drawCB.SetCallback(this,&pathPlan::drawLightTrail); m_drawingList << drawCB;
drawCB.SetCallback(this,&pathPlan::drawPathBuildLine); m_drawingList << drawCB;
m_GP.length = 0;
m_GP.time = 0;
m_GP.efficiency = 0;
displayBuildPath(false);
displayPath(false);
displayLightTrail(false);
displayCrowFly(false);
displayDebug(false);
displayCspace(false);
}
void selectOption(unsigned char menuOpt)
{
char init2_msg[16];
printSPIHeader(GPIOA,RESET_OFF|0x3); //choose both Chip Selects
printSPIHeader(GPIOB,0b00111111); //turn display ON
glcdStrobe(0x3);
// initSPI();
//clearScreen();
if(menuOpt>0)
{
if(menuOpt!=lastOpt)
{
lastOpt=menuOpt;
}
else if(lastOpt>0&&menuOpt>0)
{
switch(menuDepth)
{
case 0:
{
//draw menu options
if(selected>0)
{
chosenName=menuOpt;
displayOptions(0);
menuDepth=1;
lastOpt=0;
selected=0;
}
else
{
displayAccounts(storeList, menuOpt);
selected=1;
}
break;
};
case 1:
{
if(selected>0)
{
//display choices
chosenState=menuOpt;
menuDepth=2;
lastOpt=0;
selected=0;
}
else
{
displayOptions(menuOpt);
selected=1;
}
break;
};
case 2://reset
{
displayDebug();
menuDepth=0;
lastOpt=0;
selected=0;
break;
};
default:
{
displayAccounts(storeList,0);
menuDepth=0;
break;
};
}
}
}
else
{
lastOpt=0;
if(menuDepth==0)
{
displayAccounts(storeList,0);
}
else if(menuDepth==1)
{
displayOptions(0);
}else if(menuDepth==2)
{
displayDebug();
}
}
// CloseSPI1();
}
bool swTeleop::SWIcubArm::init( yarp::os::ResourceFinder &oRf, bool bLeftArm)
{
if(m_bInitialized)
{
std::cerr << "Icub Arm is already initialized. " << std::endl;
return true;
}
if(bLeftArm)
{
m_sArm = "left";
}
else
{
m_sArm = "right";
}
// gets the module name which will form the stem of all module port names
m_sModuleName = oRf.check("name", yarp::os::Value("teleoperation_iCub"), "Teleoperation/iCub Module name (string)").asString();
m_sRobotName = oRf.check("robot",yarp::os::Value("icubSim"), "Robot name (string)").asString();
// robot parts to control
m_bArmHandActivated = oRf.check(std::string(m_sArm + "ArmHandActivated").c_str(), yarp::os::Value(m_bArmHandActivatedDefault), std::string(m_sArm + " Arm/hand activated (int)").c_str()).asInt() != 0;
m_bFingersActivated = oRf.check(std::string(m_sArm + "FingersActivated").c_str(), yarp::os::Value(m_bFingersActivatedDefault), std::string(m_sArm + " Fingers activated (int)").c_str()).asInt() != 0;
m_i32RateVelocityControl = oRf.check("armsRateVelocityControl", yarp::os::Value(m_i32RateVelocityControlDefault), "Arms rate velocity control (int)").asInt();
if(!m_bArmHandActivated && !m_bFingersActivated)
{
std::cout << m_sArm + " arm/hand and " + m_sArm +" fingers not activated, icub " + m_sArm + " arm initialization aborted. " << std::endl;
return (m_bInitialized = false);
}
// min / max values for iCub Torso joints
for(uint ii = 0; ii < m_vArmJointVelocityAcceleration.size(); ++ii)
{
std::ostringstream l_os;
l_os << ii;
std::string l_sMinJoint(m_sArm + "ArmMinValueJoint" + l_os.str());
std::string l_sMaxJoint(m_sArm + "ArmMaxValueJoint" + l_os.str());
std::string l_sArmResetPosition(m_sArm + "ArmResetPosition" + l_os.str());
std::string l_sArmJointVelocityAcceleration(m_sArm + "ArmJointVelocityAcceleration" + l_os.str());
std::string l_sArmJointVelocityK(m_sArm + "ArmJointVelocityK" + l_os.str());
std::string l_sArmJointPositionAcceleration(m_sArm + "ArmJointPositionAcceleration" + l_os.str());
std::string l_sArmJointPositionSpeed(m_sArm + "ArmJointPositionSpeed" + l_os.str());
std::string l_sMinJointInfo(m_sArm + " arm minimum joint" + l_os.str() + " Value (double)");
std::string l_sMaxJointInfo(m_sArm + " arm maximum joint" + l_os.str() + " Value (double)");
std::string l_sArmResetPositionInfo(m_sArm + " arm reset position " + l_os.str() + " Value (double)");
std::string l_sArmJointVelocityAccelerationInfo(m_sArm + " arm joint velocity acceleration " + l_os.str() + " Value (double)");
std::string l_sArmJointVelocityKInfo(m_sArm + " arm joint velocity K coeff"+ l_os.str() + " Value (double)");
std::string l_sArmJointPositionAccelerationInfo(m_sArm + " arm joint position acceleration " + l_os.str() + " Value (double)");
std::string l_sArmJointPositionSpeedInfo(m_sArm + " arm joint position speed " + l_os.str() + " Value (double)");
m_vArmMinJoint[ii] = oRf.check(l_sMinJoint.c_str(), m_vArmMinJointDefault[ii], l_sMinJointInfo.c_str()).asDouble();
m_vArmMaxJoint[ii] = oRf.check(l_sMaxJoint.c_str(), m_vArmMaxJointDefault[ii], l_sMaxJointInfo.c_str()).asDouble();
m_vArmResetPosition[ii] = oRf.check(l_sArmResetPosition.c_str(), m_vArmResetPositionDefault[ii], l_sArmResetPositionInfo.c_str()).asDouble();
m_vArmJointVelocityAcceleration[ii]= oRf.check(l_sArmJointVelocityAcceleration.c_str(), m_vArmJointVelocityAccelerationDefault[ii], l_sArmJointVelocityAccelerationInfo.c_str()).asDouble();
m_vArmJointPositionAcceleration[ii]= oRf.check(l_sArmJointPositionAcceleration.c_str(), m_vArmJointPositionAccelerationDefault[ii], l_sArmJointPositionAccelerationInfo.c_str()).asDouble();
m_vArmJointPositionSpeed[ii] = oRf.check(l_sArmJointPositionSpeed.c_str(), m_vArmJointPositionSpeedDefault[ii], l_sArmJointPositionSpeedInfo.c_str()).asDouble();
m_vArmJointVelocityK[ii] = oRf.check(l_sArmJointVelocityK.c_str(), m_vArmJointVelocityKDefault[ii], l_sArmJointVelocityKInfo.c_str()).asDouble();
}
// miscellaneous
m_i32TimeoutArmReset = oRf.check(std::string(m_sArm + "ArmTimeoutReset").c_str(), yarp::os::Value(m_i32TimeoutArmResetDefault), std::string(m_sArm + " arm timeout reset iCub (int)").c_str()).asInt();
// set polydriver options
m_oArmOptions.put("robot", m_sRobotName.c_str());
m_oArmOptions.put("device", "remote_controlboard");
m_oArmOptions.put("local", ("/local/" + m_sRobotName + "/" + m_sArm + "_arm").c_str());
m_oArmOptions.put("name", ("/" + m_sRobotName + "/" + m_sArm + "_arm").c_str());
m_oArmOptions.put("remote", ("/" + m_sRobotName + "/" + m_sArm + "_arm").c_str());
// init polydriver
m_oRobotArm.open(m_oArmOptions);
if(!m_oRobotArm.isValid())
{
std::cerr << std::endl <<"-ERROR: " << m_sArm << " robotArm is not valid, escape arm initialization. " << std::endl <<std::endl;
return (m_bInitialized=false);
}
// initializing controllers
if (!m_oRobotArm.view(m_pIArmVelocity) || !m_oRobotArm.view(m_pIArmPosition) || !m_oRobotArm.view(m_pIArmEncoders))
{
std::cerr << std::endl << "-ERROR: " << m_sArm << " while getting required robot Arm interfaces." << std::endl <<std::endl;
m_oRobotArm.close();
return (m_bInitialized=false);
}
// init ports
m_sHandTrackerPortName = "/teleoperation/" + m_sRobotName + "/" + m_sArm + "_arm/hand";
m_sHandFingersTrackerPortName = "/teleoperation/" + m_sRobotName + "/" + m_sArm + "_arm/hand_fingers";
// open ports
bool l_bPortOpeningSuccess = true;
if(m_bArmHandActivated || m_bFingersActivated)
{
l_bPortOpeningSuccess = m_oHandFingersTrackerPort.open(m_sHandFingersTrackerPortName.c_str());
if(l_bPortOpeningSuccess)
l_bPortOpeningSuccess = m_oHandTrackerPort.open(m_sHandTrackerPortName.c_str());
}
if(!l_bPortOpeningSuccess)
{
std::cerr << std::endl <<"-ERROR: Unable to open ports." << std::endl <<std::endl;
m_oRobotArm.close();
return (m_bInitialized=false);
}
// retrieve Torso number of joints
m_pIArmPosition->getAxes(&m_i32ArmJointsNb);
// set accelerations/speeds
for(int ii = 0; ii < m_i32ArmJointsNb; ++ii)
{
m_pIArmPosition->setRefAcceleration(ii, m_vArmJointPositionAcceleration[ii]);
m_pIArmPosition->setRefSpeed(ii, m_vArmJointPositionSpeed[ii]);
m_pIArmVelocity->setRefAcceleration(ii, m_vArmJointVelocityAcceleration[ii]);
}
// init controller
m_pVelocityController = new swTeleop::SWArmVelocityController(m_pIArmEncoders, m_pIArmVelocity, m_vArmJointVelocityK, m_i32RateVelocityControl);
m_pVelocityController->enable(m_bArmHandActivated, m_bFingersActivated);
// display parameters
std::cout << std::endl << std::endl;
displayDebug(m_sArm + std::string(" arm/hand activated"), m_bArmHandActivated);
displayDebug(m_sArm + std::string(" fingers activated"), m_bFingersActivated);
displayDebug(std::string("Gaze activated"), m_i32TimeoutArmReset);
displayDebug(std::string("Rate velocity control"), m_i32RateVelocityControl);
std::cout << std::endl;
displayVectorDebug(m_sArm + std::string(" arm min joint : "), m_vArmMinJoint);
displayVectorDebug(m_sArm + std::string(" arm max joint : "), m_vArmMaxJoint);
displayVectorDebug(m_sArm + std::string(" arm reset position joint : "), m_vArmResetPosition);
displayVectorDebug(m_sArm + std::string(" arm joint velocity acceleration: "), m_vArmJointVelocityAcceleration);
displayVectorDebug(m_sArm + std::string(" arm joint position acceleration: "), m_vArmJointPositionAcceleration);
displayVectorDebug(m_sArm + std::string(" arm joint position speed : "), m_vArmJointPositionSpeed);
displayVectorDebug(m_sArm + std::string(" arm joint velocity : "), m_vArmJointVelocityK);
std::cout << std::endl << std::endl;
return (m_bIsRunning=m_bInitialized=true);
}
