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
28 #include <VirtualRobot/XML/RobotIO.h>
29 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
30 
31 /* Coin includes */
32 #include <Inventor/actions/SoGetMatrixAction.h>
33 #include <Inventor/actions/SoSearchAction.h>
34 #include <Inventor/SbViewportRegion.h>
35 #include <Inventor/nodes/SoCube.h>
36 
37 
38 #include <iostream>
39 
40 ManipulatorVisualization::ManipulatorVisualization() : isVisualizing(false), hasEndEffectorVisualizer(false), localTransformation(Eigen::Matrix4f::Identity())
41 {
42  //Im gonna live forever :)
43  this->ref();
44 }
45 
47 {
48  //Life makes no sense anymore
49  this->unref();
50 }
51 
52 void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeSet)
53 {
54  //Completely forget anything we displayed earlier
55  //This also works when this is the first time we display something
56  this->removeVisualization();
57 
58  //First of all add the end effector
59  VirtualRobot::EndEffectorPtr endEffector = robot->getEndEffector(nodeSet->getTCP()->getName());
60  if (!endEffector)
61  {
62  std::vector<VirtualRobot::EndEffectorPtr> eefs;
63  robot->getEndEffectors(eefs);
64  for (auto eef : eefs)
65  {
66  if (eef->getTcp() == nodeSet->getTCP())
67  {
68  endEffector = eef;
69  break;
70  }
71  }
72  }
73  this->hasEndEffectorVisualizer = endEffector ? true : false;
74 
75  //Display all indicators in the same color and with transparency
76  this->material = new SoMaterial();
77  this->material->transparency = 0.5;
78  this->material->setOverride(true);
79  this->addChild(material);
80 
81  //if no end effector is defined for the chain, just add a placeholder
82  if (this->hasEndEffectorVisualizer)
83  {
84  VirtualRobot::RobotPtr endEffectorRobot = endEffector->createEefRobot("", "");
85  CoinVisualizationPtr endEffectorVisualization = endEffectorRobot->getVisualization<VirtualRobot::CoinVisualization>();
86  this->addChild(endEffectorVisualization->getCoinVisualization());
87  }
88  else
89  {
90  SoCube* cube = new SoCube();
91  cube->width = 0.1;
92  cube->height = 0.1;
93  cube->depth = 0.1;
94 
95  this->addChild(cube);
96  }
97 
98  //And now wrap this bad boy with our manipulator
99  this->manip.reset(new SoTransformerManip());
100 
101  //We need this null separator to remove the scale knobs
102  //This won't lead to memory leak, because this separator
103  //will be deleted when the manipulator itself gets removed
104  //from the scene graph
105  SoSeparator* nullSep = new SoSeparator;
106 
107  //Make all scale knobs disappear
108  manip->getDragger()->setPart("scale1", nullSep);
109  manip->getDragger()->setPart("scale2", nullSep);
110  manip->getDragger()->setPart("scale3", nullSep);
111  manip->getDragger()->setPart("scale4", nullSep);
112  manip->getDragger()->setPart("scale5", nullSep);
113  manip->getDragger()->setPart("scale6", nullSep);
114  manip->getDragger()->setPart("scale7", nullSep);
115  manip->getDragger()->setPart("scale8", nullSep);
116 
117  //Stores the global position and pose of tcp
118  Eigen::Matrix4f globalMat = nodeSet->getTCP()->getGlobalPose();
119 
120  if (this->hasEndEffectorVisualizer)
121  {
122  //Stores the local transformation from the endeffector base to the TCP node
123  this->localTransformation = endEffector->getBase()->getTransformationTo(nodeSet->getTCP());
124 
125  globalMat = endEffector->getBase()->getGlobalPose();
126  }
127 
128  //Now go ahead and set new position
129  //Factor 1000 to match coin unit system
130  globalMat(0, 3) /= 1000;
131  globalMat(1, 3) /= 1000;
132  globalMat(2, 3) /= 1000;
133  manip->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(globalMat));
134 
135  this->insertChild(manip.get(), 0);
136 
137  this->isVisualizing = true;
138 }
139 
141 {
142  //Remove all children and reset manip pointer
143  //This should bring ref counter of Inventor down to zero and free memory
144  this->removeAllChildren();
145  manip.reset();
146 
147  this->isVisualizing = false;
148  this->hasEndEffectorVisualizer = false;
149  this->localTransformation = Eigen::Matrix4f::Identity();
150 }
151 
152 void ManipulatorVisualization::setColor(float r, float g, float b)
153 {
154  if (this->getIsVisualizing())
155  {
156  this->material->ambientColor.setValue(r, g, b);
157  }
158 }
159 
160 void ManipulatorVisualization::addManipFinishCallback(SoDraggerCB* func, void* data)
161 {
162  if (this->getIsVisualizing())
163  {
164  this->manip->getDragger()->addFinishCallback(func, data);
165  }
166 }
167 
168 void ManipulatorVisualization::addManipMovedCallback(SoDraggerCB* func, void* data)
169 {
170  if (this->getIsVisualizing())
171  {
172  this->manip->getDragger()->addMotionCallback(func, data);
173  }
174 }
175 
177 {
178  if (this->getIsVisualizing())
179  {
180  SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
181  SoSearchAction sa;
182  sa.setNode(manip.get());
183  sa.setSearchingAll(TRUE); // Search all nodes
184  SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
185  sa.apply(this);
186 
187  action->apply(sa.getPath());
188 
189  SbMatrix matrix = action->getMatrix();
190 
192  mat(0, 0) = matrix[0][0];
193  mat(0, 1) = matrix[1][0];
194  mat(0, 2) = matrix[2][0];
195  mat(0, 3) = matrix[3][0] * 1000;
196 
197  mat(1, 0) = matrix[0][1];
198  mat(1, 1) = matrix[1][1];
199  mat(1, 2) = matrix[2][1];
200  mat(1, 3) = matrix[3][1] * 1000;
201 
202  mat(2, 0) = matrix[0][2];
203  mat(2, 1) = matrix[1][2];
204  mat(2, 2) = matrix[2][2];
205  mat(2, 3) = matrix[3][2] * 1000;
206 
207  mat(3, 0) = matrix[0][3];
208  mat(3, 1) = matrix[1][3];
209  mat(3, 2) = matrix[2][3];
210  mat(3, 3) = matrix[3][3];
211 
212  //We need to take local transformation into account, because otherwise the visualizer
213  //shows a wrong position for tcp (offset equals local transformation of tcp in node set then)
214  //Just apply local transformation to fix this
215  if (this->hasEndEffectorVisualizer)
216  {
217  mat = mat * this->localTransformation;
218  }
219 
220  return mat;
221  }
222 
223  return Eigen::Matrix4f::Identity();
224 }
225 
227 {
228  Eigen::Matrix4f mat = this->getUserDesiredPose();
229 
230  //Convert to string
231  std::stringstream buffer;
232  buffer << mat;
233  return buffer.str();
234 }
235 
236 
237 
ManipulatorVisualization::addManipMovedCallback
void addManipMovedCallback(SoDraggerCB *func, void *data)
Definition: ManipulatorVisualization.cpp:168
RemoteRobot.h
ManipulatorVisualization::getUserDesiredPose
Eigen::Matrix4f getUserDesiredPose()
Definition: ManipulatorVisualization.cpp:176
Eigen
Definition: Elements.h:36
ManipulatorVisualization::removeVisualization
void removeVisualization()
Definition: ManipulatorVisualization.cpp:140
boost::shared_ptr< VirtualRobot::CoinVisualization >
ManipulatorVisualization::getIsVisualizing
bool getIsVisualizing() const
Definition: ManipulatorVisualization.h:69
ManipulatorVisualization::ManipulatorVisualization
ManipulatorVisualization()
Definition: ManipulatorVisualization.cpp:40
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
ManipulatorVisualization.h
ManipulatorVisualization::setColor
void setColor(float r, float g, float b)
Definition: ManipulatorVisualization.cpp:152
ManipulatorVisualization::addManipFinishCallback
void addManipFinishCallback(SoDraggerCB *func, void *data)
Definition: ManipulatorVisualization.cpp:160
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ManipulatorVisualization::~ManipulatorVisualization
~ManipulatorVisualization() override
Definition: ManipulatorVisualization.cpp:46
ManipulatorVisualization::setVisualization
void setVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeSet)
Definition: ManipulatorVisualization.cpp:52
ManipulatorVisualization::getUserDesiredPoseString
std::string getUserDesiredPoseString()
Definition: ManipulatorVisualization.cpp:226
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18