27 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
28 #include <VirtualRobot/XML/RobotIO.h>
35 #include <Inventor/SbViewportRegion.h>
36 #include <Inventor/actions/SoGetMatrixAction.h>
37 #include <Inventor/actions/SoSearchAction.h>
38 #include <Inventor/nodes/SoCube.h>
42 hasEndEffectorVisualizer(false),
57 VirtualRobot::RobotNodeSetPtr nodeSet)
64 VirtualRobot::EndEffectorPtr endEffector = robot->getEndEffector(nodeSet->getTCP()->getName());
67 std::vector<VirtualRobot::EndEffectorPtr> eefs;
68 robot->getEndEffectors(eefs);
71 if (eef->getTcp() == nodeSet->getTCP())
78 this->hasEndEffectorVisualizer = endEffector ? true :
false;
81 this->material =
new SoMaterial();
82 this->material->transparency = 0.5;
83 this->material->setOverride(
true);
84 this->addChild(material);
87 if (this->hasEndEffectorVisualizer)
91 endEffectorRobot->getVisualization<VirtualRobot::CoinVisualization>();
92 this->addChild(endEffectorVisualization->getCoinVisualization());
96 SoCube* cube =
new SoCube();
101 this->addChild(cube);
105 this->manip.reset(
new SoTransformerManip());
111 SoSeparator* nullSep =
new SoSeparator;
114 manip->getDragger()->setPart(
"scale1", nullSep);
115 manip->getDragger()->setPart(
"scale2", nullSep);
116 manip->getDragger()->setPart(
"scale3", nullSep);
117 manip->getDragger()->setPart(
"scale4", nullSep);
118 manip->getDragger()->setPart(
"scale5", nullSep);
119 manip->getDragger()->setPart(
"scale6", nullSep);
120 manip->getDragger()->setPart(
"scale7", nullSep);
121 manip->getDragger()->setPart(
"scale8", nullSep);
126 if (this->hasEndEffectorVisualizer)
129 this->localTransformation = endEffector->getBase()->getTransformationTo(nodeSet->getTCP());
131 globalMat = endEffector->getBase()->getGlobalPose();
136 globalMat(0, 3) /= 1000;
137 globalMat(1, 3) /= 1000;
138 globalMat(2, 3) /= 1000;
139 manip->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(globalMat));
141 this->insertChild(manip.get(), 0);
143 this->isVisualizing =
true;
151 this->removeAllChildren();
154 this->isVisualizing =
false;
155 this->hasEndEffectorVisualizer =
false;
164 this->material->ambientColor.setValue(r, g, b);
173 this->manip->getDragger()->addFinishCallback(func,
data);
182 this->manip->getDragger()->addMotionCallback(func,
data);
191 SoGetMatrixAction* action =
new SoGetMatrixAction(SbViewportRegion());
193 sa.setNode(manip.get());
194 sa.setSearchingAll(TRUE);
195 SoBaseKit::setSearchingChildren(TRUE);
198 action->apply(sa.getPath());
200 SbMatrix matrix = action->getMatrix();
203 mat(0, 0) = matrix[0][0];
204 mat(0, 1) = matrix[1][0];
205 mat(0, 2) = matrix[2][0];
206 mat(0, 3) = matrix[3][0] * 1000;
208 mat(1, 0) = matrix[0][1];
209 mat(1, 1) = matrix[1][1];
210 mat(1, 2) = matrix[2][1];
211 mat(1, 3) = matrix[3][1] * 1000;
213 mat(2, 0) = matrix[0][2];
214 mat(2, 1) = matrix[1][2];
215 mat(2, 2) = matrix[2][2];
216 mat(2, 3) = matrix[3][2] * 1000;
218 mat(3, 0) = matrix[0][3];
219 mat(3, 1) = matrix[1][3];
220 mat(3, 2) = matrix[2][3];
221 mat(3, 3) = matrix[3][3];
226 if (this->hasEndEffectorVisualizer)
228 mat = mat * this->localTransformation;
243 std::stringstream buffer;