Skip to content
Snippets Groups Projects
Commit e2e10a85 authored by Philipp Schmidt's avatar Philipp Schmidt
Browse files

Rearranged methods to clean up code

parent c36bcfdb
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp
+ 157
− 155
View file @ e2e10a85
......@@ -48,7 +48,7 @@ armarx::RobotIKGuiPlugin::RobotIKGuiPlugin()
addWidget<RobotIKWidgetController>();
}
armarx::RobotIKWidgetController::RobotIKWidgetController() : startUpCameraPositioning(true)
armarx::RobotIKWidgetController::RobotIKWidgetController() : startUpCameraPositioningFlag(true)
{
//Initialize Gui
ui.setupUi(getWidget());
......@@ -57,10 +57,9 @@ armarx::RobotIKWidgetController::RobotIKWidgetController() : startUpCameraPositi
//Alignment for ui
this->ui.verticalLayout->setAlignment(Qt::AlignTop);
//Label color
//Label color can not be set in designer, so we do it here
this->ui.reachableLabel->setStyleSheet("QLabel { color : red; }");
//Component names
robotStateComponentName = "";
robotIKComponentName = "";
......@@ -68,12 +67,15 @@ armarx::RobotIKWidgetController::RobotIKWidgetController() : startUpCameraPositi
manipSeparator = NULL;
currentSeparator = NULL;
tcpManip = NULL;
tcpCurrent = NULL;
tcpCurrentTransform = NULL;
tcpDesired = NULL;
tcpManipTransform = NULL;
tcpManipColor = NULL;
tcpManipSphere = NULL;
tcpCurrentTransform = NULL;
tcpCurrentColor = NULL;
tcpCurrentSphere = NULL;
robotUpdateSensor = NULL;
textFieldUpdateSensor = NULL;
}
void armarx::RobotIKWidgetController::onInitComponent()
......@@ -86,6 +88,7 @@ void armarx::RobotIKWidgetController::onInitComponent()
void armarx::RobotIKWidgetController::onConnectComponent()
{
//Get all proxies
robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitComponentName);
robotIKPrx = getProxy<RobotIKInterfacePrx>(robotIKComponentName);
......@@ -128,49 +131,6 @@ void armarx::RobotIKWidgetController::onConnectComponent()
enableMainWidgetAsync(true);
}
armarx::StringList armarx::RobotIKWidgetController::getIncludePaths()
{
StringList includePaths;
try {
StringList packages = robotStateComponentPrx->getArmarXPackages();
packages.push_back(Application::GetProjectName());
for(const std::string &projectName : packages)
{
if(projectName.empty())
continue;
CMakePackageFinder project(projectName);
StringList projectIncludePaths;
auto pathsString = project.getDataDir();
boost::split(projectIncludePaths,
pathsString,
boost::is_any_of(";,"),
boost::token_compress_on);
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
} catch (...)
{
ARMARX_ERROR << "Unable to retrieve robot filename." << std::endl;
}
return includePaths;
}
VirtualRobot::RobotPtr armarx::RobotIKWidgetController::loadRobot(StringList includePaths)
{
try
{
std::string rfile = robotStateComponentPrx->getRobotFilename();
ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
return VirtualRobot::RobotIO::loadRobot(rfile);
}
catch(...)
{
ARMARX_ERROR << "Unable to load robot from file" << std::endl;
return VirtualRobot::RobotPtr();
}
}
void armarx::RobotIKWidgetController::onDisconnectComponent()
{
robotStateComponentPrx = NULL;
......@@ -228,98 +188,6 @@ void armarx::RobotIKWidgetController::solveIK()
}
}
void armarx::RobotIKWidgetController::connectSlots()
{
connect(ui.resetManip, SIGNAL(clicked()), this, SLOT(resetManip()), Qt::QueuedConnection);
connect(ui.pushButton, SIGNAL(clicked()), this, SLOT(solveIK()), Qt::QueuedConnection);
connect(ui.comboBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(kinematicChainChanged(QString)), Qt::UniqueConnection);
}
void armarx::RobotIKWidgetController::robotUpdateTimerCB(void *data, SoSensor *sensor)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
if (!controller)
return;
try
{
armarx::RemoteRobot::synchronizeLocalClone(controller->robot,controller->robotStateComponentPrx);
if(controller->currentSeparator) {
Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
//Apply tcp position to indicator
tcpMatrix(0, 3) /= 1000;
tcpMatrix(1, 3) /= 1000;
tcpMatrix(2, 3) /= 1000;
controller->tcpCurrentTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(tcpMatrix));
}
if(controller->startUpCameraPositioning) {
controller->ui.robotViewer->getRobotViewer()->cameraViewAll();
controller->startUpCameraPositioning = false;
}
} catch (...){};
}
void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void *data, SoSensor *sensor)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
if (!controller)
return;
if(controller->currentSeparator) {
Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
//Set text label to tcp matrix
std::stringstream buffer;
buffer << tcpMatrix;
std::string matrixText = buffer.str();
controller->ui.currentPoseMatrix->setText(QString::fromStdString(matrixText));
//Set text label for desired tcp pose
//Therefore calculate current manipulator pose
SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
SoSearchAction sa;
sa.setNode(controller->tcpDesired);
sa.setSearchingAll( TRUE ); // Search all nodes
SoBaseKit::setSearchingChildren( TRUE ); // Even inside nodekits
sa.apply(controller->ui.robotViewer->getRobotViewer()->getRootNode());
action->apply(sa.getPath());
SbMatrix matrix = action->getMatrix();
Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
mat (0,0) = matrix[0][0];
mat (0,1) = matrix[1][0];
mat (0,2) = matrix[2][0];
mat (0,3) = matrix[3][0] * 1000;
mat (1,0) = matrix[0][1];
mat (1,1) = matrix[1][1];
mat (1,2) = matrix[2][1];
mat (1,3) = matrix[3][1] * 1000;
mat (2,0) = matrix[0][2];
mat (2,1) = matrix[1][2];
mat (2,2) = matrix[2][2];
mat (2,3) = matrix[3][2] * 1000;
mat (3,0) = matrix[0][3];
mat (3,1) = matrix[1][3];
mat (3,2) = matrix[2][3];
mat (3,3) = matrix[3][3];
//Set text label to manip matrix
std::stringstream buffer2;
buffer2 << mat;
std::string matrixText2 = buffer2.str();
controller->ui.desiredPoseMatrix->setText(QString::fromStdString(matrixText2));
delete(action);
}
}
void armarx::RobotIKWidgetController::kinematicChainChanged(const QString &arg1)
{
this->ui.moveTCP->setEnabled(true);
......@@ -351,13 +219,13 @@ void armarx::RobotIKWidgetController::kinematicChainChanged(const QString &arg1)
tcpCurrentColor = new SoMaterial;
tcpCurrentColor->transparency = 0.6f;
tcpCurrent = new SoSphere;
tcpCurrent->radius = 0.06f;
tcpDesiredColor = new SoMaterial;
tcpDesiredColor->ambientColor.setValue(0, 1, 0);
tcpDesiredColor->transparency = 0.3f;
tcpDesired = new SoSphere;
tcpDesired->radius = 0.06f;
tcpCurrentSphere = new SoSphere;
tcpCurrentSphere->radius = 0.06f;
tcpManipColor = new SoMaterial;
tcpManipColor->ambientColor.setValue(0, 1, 0);
tcpManipColor->transparency = 0.3f;
tcpManipSphere = new SoSphere;
tcpManipSphere->radius = 0.06f;
tcpManip = new SoTransformerManip;
SoSeparator* nullSep = new SoSeparator;
......@@ -377,12 +245,12 @@ void armarx::RobotIKWidgetController::kinematicChainChanged(const QString &arg1)
manipSeparator->addChild(tcpManipTransform);
manipSeparator->addChild(tcpManip);
manipSeparator->addChild(tcpDesiredColor);
manipSeparator->addChild(tcpDesired);
manipSeparator->addChild(tcpManipColor);
manipSeparator->addChild(tcpManipSphere);
currentSeparator->addChild(tcpCurrentTransform);
currentSeparator->addChild(tcpCurrentColor);
currentSeparator->addChild(tcpCurrent);
currentSeparator->addChild(tcpCurrentSphere);
this->ui.robotViewer->getRobotViewer()->getRootNode()->insertChild(currentSeparator, 0);
this->ui.robotViewer->getRobotViewer()->getRootNode()->insertChild(manipSeparator, 0);
......@@ -405,6 +273,56 @@ void armarx::RobotIKWidgetController::resetManip()
kinematicChainChanged(this->ui.comboBox->currentText());
}
void armarx::RobotIKWidgetController::connectSlots()
{
connect(ui.resetManip, SIGNAL(clicked()), this, SLOT(resetManip()), Qt::QueuedConnection);
connect(ui.pushButton, SIGNAL(clicked()), this, SLOT(solveIK()), Qt::QueuedConnection);
connect(ui.comboBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(kinematicChainChanged(QString)), Qt::UniqueConnection);
}
armarx::StringList armarx::RobotIKWidgetController::getIncludePaths()
{
StringList includePaths;
try {
StringList packages = robotStateComponentPrx->getArmarXPackages();
packages.push_back(Application::GetProjectName());
for(const std::string &projectName : packages)
{
if(projectName.empty())
continue;
CMakePackageFinder project(projectName);
StringList projectIncludePaths;
auto pathsString = project.getDataDir();
boost::split(projectIncludePaths,
pathsString,
boost::is_any_of(";,"),
boost::token_compress_on);
includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
} catch (...)
{
ARMARX_ERROR << "Unable to retrieve robot filename." << std::endl;
}
return includePaths;
}
VirtualRobot::RobotPtr armarx::RobotIKWidgetController::loadRobot(StringList includePaths)
{
try
{
std::string rfile = robotStateComponentPrx->getRobotFilename();
ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
return VirtualRobot::RobotIO::loadRobot(rfile);
}
catch(...)
{
ARMARX_ERROR << "Unable to load robot from file" << std::endl;
return VirtualRobot::RobotPtr();
}
}
void armarx::RobotIKWidgetController::manipFinishCallback(void* data, SoDragger* dragger) {
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
......@@ -412,25 +330,109 @@ void armarx::RobotIKWidgetController::manipFinishCallback(void* data, SoDragger*
if(targetJointAngles.isReachable) {
//Green
controller->tcpDesiredColor->ambientColor.setValue(0, 1, 0);
controller->tcpManipColor->ambientColor.setValue(0, 1, 0);
controller->ui.reachableLabel->setText(QString::fromStdString("Pose reachable!"));
controller->ui.reachableLabel->setStyleSheet("QLabel { color : green; }");
controller->ui.pushButton->setEnabled(true);
}
else {
//Red
controller->tcpDesiredColor->ambientColor.setValue(1, 0, 0);
controller->tcpManipColor->ambientColor.setValue(1, 0, 0);
controller->ui.reachableLabel->setText(QString::fromStdString("Pose unreachable!"));
controller->ui.reachableLabel->setStyleSheet("QLabel { color : red; }");
controller->ui.pushButton->setEnabled(false);
}
}
void armarx::RobotIKWidgetController::robotUpdateTimerCB(void *data, SoSensor *sensor)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
if (!controller)
return;
try
{
armarx::RemoteRobot::synchronizeLocalClone(controller->robot,controller->robotStateComponentPrx);
if(controller->currentSeparator) {
Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
//Apply tcp position to indicator
tcpMatrix(0, 3) /= 1000;
tcpMatrix(1, 3) /= 1000;
tcpMatrix(2, 3) /= 1000;
controller->tcpCurrentTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(tcpMatrix));
}
if(controller->startUpCameraPositioningFlag) {
controller->ui.robotViewer->getRobotViewer()->cameraViewAll();
controller->startUpCameraPositioningFlag = false;
}
} catch (...){};
}
void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void *data, SoSensor *sensor)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
if (!controller)
return;
if(controller->currentSeparator) {
Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
//Set text label to tcp matrix
std::stringstream buffer;
buffer << tcpMatrix;
std::string matrixText = buffer.str();
controller->ui.currentPoseMatrix->setText(QString::fromStdString(matrixText));
//Set text label for desired tcp pose
//Therefore calculate current manipulator pose
SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
SoSearchAction sa;
sa.setNode(controller->tcpManipSphere);
sa.setSearchingAll( TRUE ); // Search all nodes
SoBaseKit::setSearchingChildren( TRUE ); // Even inside nodekits
sa.apply(controller->ui.robotViewer->getRobotViewer()->getRootNode());
action->apply(sa.getPath());
SbMatrix matrix = action->getMatrix();
Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
mat (0,0) = matrix[0][0];
mat (0,1) = matrix[1][0];
mat (0,2) = matrix[2][0];
mat (0,3) = matrix[3][0] * 1000;
mat (1,0) = matrix[0][1];
mat (1,1) = matrix[1][1];
mat (1,2) = matrix[2][1];
mat (1,3) = matrix[3][1] * 1000;
mat (2,0) = matrix[0][2];
mat (2,1) = matrix[1][2];
mat (2,2) = matrix[2][2];
mat (2,3) = matrix[3][2] * 1000;
mat (3,0) = matrix[0][3];
mat (3,1) = matrix[1][3];
mat (3,2) = matrix[2][3];
mat (3,3) = matrix[3][3];
//Set text label to manip matrix
std::stringstream buffer2;
buffer2 << mat;
std::string matrixText2 = buffer2.str();
controller->ui.desiredPoseMatrix->setText(QString::fromStdString(matrixText2));
delete(action);
}
}
armarx::ExtendedIKResult armarx::RobotIKWidgetController::getIKSolution()
{
SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
SoSearchAction sa;
sa.setNode(tcpDesired);
sa.setNode(tcpManipSphere);
sa.setSearchingAll( TRUE ); // Search all nodes
SoBaseKit::setSearchingChildren( TRUE ); // Even inside nodekits
sa.apply(this->ui.robotViewer->getRobotViewer()->getRootNode());
......
source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h
+ 11
− 10
View file @ e2e10a85
......@@ -99,28 +99,31 @@ namespace armarx
RobotIKInterfacePrx robotIKPrx;
Ui::RobotIKGuiPlugin ui;
private:
void connectSlots();
StringList getIncludePaths();
VirtualRobot::RobotPtr loadRobot(StringList includePaths);
std::string robotStateComponentName;
std::string kinematicUnitComponentName;
std::string robotIKComponentName;
QPointer<RobotIKConfigDialog> dialog;
VirtualRobot::RobotPtr robot;
StringList getIncludePaths();
VirtualRobot::RobotPtr loadRobot(StringList includePaths);
SoSeparator* manipSeparator;
SoSeparator* currentSeparator;
SoTransformerManip* tcpManip;
static void manipFinishCallback(void *data, SoDragger* manip);
SoTransform* tcpManipTransform;
SoMaterial* tcpDesiredColor;
SoSphere* tcpDesired;
SoMaterial* tcpManipColor;
SoSphere* tcpManipSphere;
SoTransform* tcpCurrentTransform;
SoMaterial* tcpCurrentColor;
SoSphere* tcpCurrent;
SoSphere* tcpCurrentSphere;
SoTimerSensor* robotUpdateSensor;
static void robotUpdateTimerCB(void *data, SoSensor *sensor);
......@@ -128,11 +131,9 @@ namespace armarx
SoTimerSensor* textFieldUpdateSensor;
static void textFieldUpdateTimerCB(void *data, SoSensor *sensor);
static void manipFinishCallback(void *data, SoDragger* manip);
ExtendedIKResult getIKSolution();
bool startUpCameraPositioning;
bool startUpCameraPositioningFlag;
};
typedef boost::shared_ptr<RobotIKWidgetController> RobotIKGuiPluginPtr;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment