diff --git a/source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp
index be21f60ca772819417482a00d63d881e5b19c5c5..0ad1eb7f632e3c86396195bd14c913cb9cf7a7da 100644
--- a/source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp
@@ -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());
diff --git a/source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h
index ce82fe48c595c45cdff30dcc4b8982f5a43c17a0..3edc1a6d8bd0f84654d0da53e13669e6848d9706 100644
--- a/source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h
@@ -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;