ManipulatorVisualization.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package
19  * @author
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
25 
26 //Virtual Robot includes
27 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
28 #include <VirtualRobot/XML/RobotIO.h>
29 
31 
32 /* Coin includes */
33 #include <iostream>
34 
35 #include <Inventor/SbViewportRegion.h>
36 #include <Inventor/actions/SoGetMatrixAction.h>
37 #include <Inventor/actions/SoSearchAction.h>
38 #include <Inventor/nodes/SoCube.h>
39 
41  isVisualizing(false),
42  hasEndEffectorVisualizer(false),
43  localTransformation(Eigen::Matrix4f::Identity())
44 {
45  //Im gonna live forever :)
46  this->ref();
47 }
48 
50 {
51  //Life makes no sense anymore
52  this->unref();
53 }
54 
55 void
57  VirtualRobot::RobotNodeSetPtr nodeSet)
58 {
59  //Completely forget anything we displayed earlier
60  //This also works when this is the first time we display something
61  this->removeVisualization();
62 
63  //First of all add the end effector
64  VirtualRobot::EndEffectorPtr endEffector = robot->getEndEffector(nodeSet->getTCP()->getName());
65  if (!endEffector)
66  {
67  std::vector<VirtualRobot::EndEffectorPtr> eefs;
68  robot->getEndEffectors(eefs);
69  for (auto eef : eefs)
70  {
71  if (eef->getTcp() == nodeSet->getTCP())
72  {
73  endEffector = eef;
74  break;
75  }
76  }
77  }
78  this->hasEndEffectorVisualizer = endEffector ? true : false;
79 
80  //Display all indicators in the same color and with transparency
81  this->material = new SoMaterial();
82  this->material->transparency = 0.5;
83  this->material->setOverride(true);
84  this->addChild(material);
85 
86  //if no end effector is defined for the chain, just add a placeholder
87  if (this->hasEndEffectorVisualizer)
88  {
89  VirtualRobot::RobotPtr endEffectorRobot = endEffector->createEefRobot("", "");
90  CoinVisualizationPtr endEffectorVisualization =
91  endEffectorRobot->getVisualization<VirtualRobot::CoinVisualization>();
92  this->addChild(endEffectorVisualization->getCoinVisualization());
93  }
94  else
95  {
96  SoCube* cube = new SoCube();
97  cube->width = 0.1;
98  cube->height = 0.1;
99  cube->depth = 0.1;
100 
101  this->addChild(cube);
102  }
103 
104  //And now wrap this bad boy with our manipulator
105  this->manip.reset(new SoTransformerManip());
106 
107  //We need this null separator to remove the scale knobs
108  //This won't lead to memory leak, because this separator
109  //will be deleted when the manipulator itself gets removed
110  //from the scene graph
111  SoSeparator* nullSep = new SoSeparator;
112 
113  //Make all scale knobs disappear
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);
122 
123  //Stores the global position and pose of tcp
124  Eigen::Matrix4f globalMat = nodeSet->getTCP()->getGlobalPose();
125 
126  if (this->hasEndEffectorVisualizer)
127  {
128  //Stores the local transformation from the endeffector base to the TCP node
129  this->localTransformation = endEffector->getBase()->getTransformationTo(nodeSet->getTCP());
130 
131  globalMat = endEffector->getBase()->getGlobalPose();
132  }
133 
134  //Now go ahead and set new position
135  //Factor 1000 to match coin unit system
136  globalMat(0, 3) /= 1000;
137  globalMat(1, 3) /= 1000;
138  globalMat(2, 3) /= 1000;
139  manip->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(globalMat));
140 
141  this->insertChild(manip.get(), 0);
142 
143  this->isVisualizing = true;
144 }
145 
146 void
148 {
149  //Remove all children and reset manip pointer
150  //This should bring ref counter of Inventor down to zero and free memory
151  this->removeAllChildren();
152  manip.reset();
153 
154  this->isVisualizing = false;
155  this->hasEndEffectorVisualizer = false;
156  this->localTransformation = Eigen::Matrix4f::Identity();
157 }
158 
159 void
160 ManipulatorVisualization::setColor(float r, float g, float b)
161 {
162  if (this->getIsVisualizing())
163  {
164  this->material->ambientColor.setValue(r, g, b);
165  }
166 }
167 
168 void
170 {
171  if (this->getIsVisualizing())
172  {
173  this->manip->getDragger()->addFinishCallback(func, data);
174  }
175 }
176 
177 void
179 {
180  if (this->getIsVisualizing())
181  {
182  this->manip->getDragger()->addMotionCallback(func, data);
183  }
184 }
185 
188 {
189  if (this->getIsVisualizing())
190  {
191  SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
192  SoSearchAction sa;
193  sa.setNode(manip.get());
194  sa.setSearchingAll(TRUE); // Search all nodes
195  SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
196  sa.apply(this);
197 
198  action->apply(sa.getPath());
199 
200  SbMatrix matrix = action->getMatrix();
201 
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;
207 
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;
212 
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;
217 
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];
222 
223  //We need to take local transformation into account, because otherwise the visualizer
224  //shows a wrong position for tcp (offset equals local transformation of tcp in node set then)
225  //Just apply local transformation to fix this
226  if (this->hasEndEffectorVisualizer)
227  {
228  mat = mat * this->localTransformation;
229  }
230 
231  return mat;
232  }
233 
234  return Eigen::Matrix4f::Identity();
235 }
236 
237 std::string
239 {
240  Eigen::Matrix4f mat = this->getUserDesiredPose();
241 
242  //Convert to string
243  std::stringstream buffer;
244  buffer << mat;
245  return buffer.str();
246 }
ManipulatorVisualization::addManipMovedCallback
void addManipMovedCallback(SoDraggerCB *func, void *data)
Definition: ManipulatorVisualization.cpp:179
RemoteRobot.h
ManipulatorVisualization::getUserDesiredPose
Eigen::Matrix4f getUserDesiredPose()
Definition: ManipulatorVisualization.cpp:188
Eigen
Definition: Elements.h:32
ManipulatorVisualization::removeVisualization
void removeVisualization()
Definition: ManipulatorVisualization.cpp:148
boost::shared_ptr< VirtualRobot::CoinVisualization >
ManipulatorVisualization::getIsVisualizing
bool getIsVisualizing() const
Definition: ManipulatorVisualization.h:74
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
ManipulatorVisualization::ManipulatorVisualization
ManipulatorVisualization()
Definition: ManipulatorVisualization.cpp:41
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
ManipulatorVisualization.h
ManipulatorVisualization::setColor
void setColor(float r, float g, float b)
Definition: ManipulatorVisualization.cpp:161
ManipulatorVisualization::addManipFinishCallback
void addManipFinishCallback(SoDraggerCB *func, void *data)
Definition: ManipulatorVisualization.cpp:170
ManipulatorVisualization::~ManipulatorVisualization
~ManipulatorVisualization() override
Definition: ManipulatorVisualization.cpp:50
ManipulatorVisualization::setVisualization
void setVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeSet)
Definition: ManipulatorVisualization.cpp:57
ManipulatorVisualization::getUserDesiredPoseString
std::string getUserDesiredPoseString()
Definition: ManipulatorVisualization.cpp:239
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19