25 #include <QGridLayout>
30 #include <VirtualRobot/MathTools.h>
33 #include <boost/format.hpp>
36 #include <ArmarXGui/libraries/qtpropertybrowser/src/QtProperty>
38 #include "../controller/RotateTranslateOperation.h"
52 connect(doubleManager,
60 std::string collection)
64 if (objectClass && collection.length() != 0)
66 this->setClassAttributes(objectClass, collection,
true);
67 this->setFactoryForManager(qobject_cast<QtDoublePropertyManager*>(doubleManager),
68 qobject_cast<QtDoubleSpinBoxFactory*>(spinBoxFactory));
79 currentSceneObject = sceneObject;
80 this->setInstanceAttributes();
81 this->setFactoryForManager(qobject_cast<QtDoublePropertyManager*>(doubleManager),
82 qobject_cast<QtDoubleSpinBoxFactory*>(spinBoxFactory));
87 gui::PropertyBrowserWidget::valueChanged(
QtProperty* property,
double newValue)
90 if (!propertyToId.contains(property) || !currentSceneObject ||
91 propertyToValue[property] == newValue)
98 propertyToValue[property] = newValue;
99 std::shared_ptr<std::vector<controller::OperationPtr>> operations(
100 new std::vector<controller::OperationPtr>());
105 currentSceneObject->getObjectId(),
106 currentSceneObject->getRotation(),
107 this->createNewRotation(property, newValue),
108 currentSceneObject->getTranslation(),
109 this->createNewTranslation(property, newValue)));
110 operations->push_back(operation);
119 gui::PropertyBrowserWidget::setInstanceAttributes()
121 QtProperty* instanceAttributes = groupManager->addProperty(tr(
"Instance Attributes"));
123 QtProperty* objectID = stringManager->addProperty(tr(
"Object ID"));
124 stringManager->setValue(objectID, QString::fromStdString(currentSceneObject->getObjectId()));
128 QtProperty* quaternion = groupManager->addProperty(tr(
"Rotation as Quaternion"));
130 float xQuat, yQuat, zQuat, wQuat;
131 currentSceneObject->getRotation().getValue(xQuat, yQuat, zQuat, wQuat);
133 QtProperty* quaternionX = doubleManager->addProperty(tr(
"x"));
134 QtProperty* quaternionY = doubleManager->addProperty(tr(
"y"));
135 QtProperty* quaternionZ = doubleManager->addProperty(tr(
"z"));
136 QtProperty* quaternionW = doubleManager->addProperty(tr(
"w"));
139 propertyToValue[quaternionX] = (double)xQuat;
140 propertyToValue[quaternionY] = (double)yQuat;
141 propertyToValue[quaternionZ] = (double)zQuat;
142 propertyToValue[quaternionW] = (double)wQuat;
144 doubleManager->setValue(quaternionX, (
double)xQuat);
145 doubleManager->setValue(quaternionY, (
double)yQuat);
146 doubleManager->setValue(quaternionZ, (
double)zQuat);
147 doubleManager->setValue(quaternionW, (
double)wQuat);
149 doubleManager->setSingleStep(quaternionW, 0.01);
150 doubleManager->setSingleStep(quaternionX, 0.01);
151 doubleManager->setSingleStep(quaternionY, 0.01);
152 doubleManager->setSingleStep(quaternionZ, 0.01);
160 QtProperty* rpy = groupManager->addProperty(tr(
"Roll-Pitch-Yaw"));
162 QtProperty* roll = doubleManager->addProperty(tr(
"Roll in deg"));
163 QtProperty* pitch = doubleManager->addProperty(tr(
"Pitch in deg"));
164 QtProperty* yaw = doubleManager->addProperty(tr(
"Yaw in deg"));
166 Eigen::Vector3f rpyVector;
167 VirtualRobot::MathTools::eigen4f2rpy(
168 VirtualRobot::MathTools::quat2eigen4f(xQuat, yQuat, zQuat, wQuat), rpyVector);
170 float rollDeg, pitchDeg, yawDeg;
172 rollDeg = VirtualRobot::MathTools::rad2deg(rpyVector(0));
173 pitchDeg = VirtualRobot::MathTools::rad2deg(rpyVector(1));
174 yawDeg = VirtualRobot::MathTools::rad2deg(rpyVector(2));
176 propertyToValue[roll] = rollDeg;
177 propertyToValue[pitch] = pitchDeg;
178 propertyToValue[yaw] = yawDeg;
180 doubleManager->setValue(roll, rollDeg);
181 doubleManager->setValue(pitch, pitchDeg);
182 doubleManager->setValue(yaw, yawDeg);
184 doubleManager->setSingleStep(roll, 1);
185 doubleManager->setSingleStep(pitch, 1);
186 doubleManager->setSingleStep(yaw, 1);
193 QtProperty* translation = groupManager->addProperty(tr(
"Translation"));
195 float xPos, yPos, zPos;
196 currentSceneObject->getTranslation().getValue(xPos, yPos, zPos);
198 QtProperty* translationX = doubleManager->addProperty(tr(
"X Position"));
199 QtProperty* translationY = doubleManager->addProperty(tr(
"Y Position"));
200 QtProperty* translationZ = doubleManager->addProperty(tr(
"Z Position"));
203 propertyToValue[translationX] = (double)xPos;
204 propertyToValue[translationY] = (double)yPos;
205 propertyToValue[translationZ] = (double)zPos;
207 doubleManager->setValue(translationX, (
double)xPos);
208 doubleManager->setValue(translationY, (
double)yPos);
209 doubleManager->setValue(translationZ, (
double)zPos);
211 doubleManager->setSingleStep(translationX, 0.01);
212 doubleManager->setSingleStep(translationY, 0.01);
213 doubleManager->setSingleStep(translationZ, 0.01);
219 QtProperty* matrix = stringManager->addProperty(tr(
"Matrix"));
221 stringManager->setValue(matrix, QString::fromStdString(this->getMatrix()));
229 this->insertInMap(quaternionX,
"x");
230 this->insertInMap(quaternionY,
"y");
231 this->insertInMap(quaternionZ,
"z");
232 this->insertInMap(quaternionW,
"w");
233 this->insertInMap(roll,
"Roll in deg");
234 this->insertInMap(pitch,
"Pitch in deg");
235 this->insertInMap(yaw,
"Yaw in deg");
236 this->insertInMap(translationX,
"X Position");
237 this->insertInMap(translationY,
"Y Position");
238 this->insertInMap(translationZ,
"Z Position");
239 this->insertInMap(matrix,
"Matrix");
241 QtBrowserItem* instancesSection = this->addProperty(instanceAttributes);
242 setExpanded(instancesSection,
true);
244 std::string collection = currentSceneObject->getCollection();
245 std::string objectClass = currentSceneObject->getClassId();
249 this->setClassAttributes(
250 controller->getMemoryXController()->getPriorKnowlegdeController()->getObjectClassPtr(
251 objectClass, collection),
259 const std::string& collection,
262 QtProperty* classAttributes = groupManager->addProperty(tr(
"Class Attributes"));
264 QtProperty* className = stringManager->addProperty(tr(
"Class Name"));
265 stringManager->setValue(className, QString::fromStdString(objectClass->getName()));
268 QtProperty* collectionName = stringManager->addProperty(tr(
"Collection"));
269 stringManager->setValue(collectionName, QString::fromStdString(collection));
272 QtProperty* objectClassID = stringManager->addProperty(tr(
"Object Class ID"));
273 stringManager->setValue(objectClassID, QString::fromStdString(objectClass->getId()));
276 std::map<std::string, std::string>::iterator iterator;
281 std::map<std::string, std::string> attributes =
282 controller->getMemoryXController()->getPriorKnowlegdeController()->getAllAttributes(
285 for (iterator = attributes.begin(); iterator != attributes.end(); ++iterator)
288 stringManager->addProperty(QString::fromStdString(iterator->first));
289 stringManager->setValue(property, QString::fromStdString(iterator->second));
294 QtBrowserItem* classesSection = this->addProperty(classAttributes);
296 setExpanded(classesSection, expanded);
300 gui::PropertyBrowserWidget::clearAll()
303 groupManager->clear();
304 doubleManager->clear();
305 stringManager->clear();
306 propertyToId.clear();
307 idToProperty.clear();
308 propertyToValue.clear();
309 currentSceneObject = NULL;
315 if (!currentSceneObject || objectID != currentSceneObject->getObjectId())
321 float xQuat, yQuat, zQuat, wQuat;
322 currentSceneObject->getRotation().getValue(xQuat, yQuat, zQuat, wQuat);
325 propertyToValue[idToProperty[QString(
"x")]] = (double)xQuat;
326 propertyToValue[idToProperty[QString(
"y")]] = (double)yQuat;
327 propertyToValue[idToProperty[QString(
"z")]] = (double)zQuat;
328 propertyToValue[idToProperty[QString(
"w")]] = (double)wQuat;
330 doubleManager->setValue(idToProperty[QString(
"x")], (double)xQuat);
331 doubleManager->setValue(idToProperty[QString(
"y")], (double)yQuat);
332 doubleManager->setValue(idToProperty[QString(
"z")], (double)zQuat);
333 doubleManager->setValue(idToProperty[QString(
"w")], (double)wQuat);
335 Eigen::Vector3f rpyVector;
336 VirtualRobot::MathTools::eigen4f2rpy(
337 VirtualRobot::MathTools::quat2eigen4f(xQuat, yQuat, zQuat, wQuat), rpyVector);
339 float rollDeg, pitchDeg, yawDeg;
341 rollDeg = VirtualRobot::MathTools::rad2deg(rpyVector(0));
342 pitchDeg = VirtualRobot::MathTools::rad2deg(rpyVector(1));
343 yawDeg = VirtualRobot::MathTools::rad2deg(rpyVector(2));
346 propertyToValue[idToProperty[QString(
"Roll in deg")]] = rollDeg;
347 propertyToValue[idToProperty[QString(
"Pitch in deg")]] = pitchDeg;
348 propertyToValue[idToProperty[QString(
"Yaw in deg")]] = yawDeg;
350 doubleManager->setValue(idToProperty[QString(
"Roll in deg")], rollDeg);
351 doubleManager->setValue(idToProperty[QString(
"Pitch in deg")], pitchDeg);
352 doubleManager->setValue(idToProperty[QString(
"Yaw in deg")], yawDeg);
355 float xPos, yPos, zPos;
356 currentSceneObject->getTranslation().getValue(xPos, yPos, zPos);
359 propertyToValue[idToProperty[QString(
"X Position")]] = (double)xPos;
360 propertyToValue[idToProperty[QString(
"Y Position")]] = (double)yPos;
361 propertyToValue[idToProperty[QString(
"Z Position")]] = (double)zPos;
363 doubleManager->setValue(idToProperty[QString(
"X Position")], (double)xPos);
364 doubleManager->setValue(idToProperty[QString(
"Y Position")], (double)yPos);
365 doubleManager->setValue(idToProperty[QString(
"Z Position")], (double)zPos);
369 stringManager->setValue(idToProperty[QString(
"Matrix")],
370 QString::fromStdString(this->getMatrix()));
374 gui::PropertyBrowserWidget::insertInMap(
QtProperty* property, QString
id)
376 propertyToId[property] = id;
377 idToProperty[id] = property;
381 gui::PropertyBrowserWidget::createNewRotation(
QtProperty* property,
double val)
383 SbRotation newRotation = currentSceneObject->getRotation();
384 float xQuat, yQuat, zQuat, wQuat;
385 newRotation.getValue(xQuat, yQuat, zQuat, wQuat);
388 VirtualRobot::MathTools::eigen4f2rpy(
389 VirtualRobot::MathTools::quat2eigen4f(xQuat, yQuat, zQuat, wQuat), rpy);
393 QString
id = propertyToId[property];
394 if (
id == QString(
"x") &&
395 !((
float)val == yQuat && yQuat == zQuat && zQuat == wQuat && wQuat == 0.0f))
397 newRotation = SbRotation((
float)val, yQuat, zQuat, wQuat);
399 else if (
id == QString(
"y") &&
400 !((
float)val == xQuat && xQuat == zQuat && zQuat == wQuat && wQuat == 0.0f))
402 newRotation = SbRotation(xQuat, (
float)val, zQuat, wQuat);
404 else if (
id == QString(
"z") &&
405 !((
float)val == xQuat && xQuat == yQuat && yQuat == wQuat && wQuat == 0.0f))
407 newRotation = SbRotation(xQuat, yQuat, (
float)val, wQuat);
409 else if (
id == QString(
"w") &&
410 !((
float)val == xQuat && xQuat == yQuat && yQuat == zQuat && zQuat == 0.0f))
412 newRotation = SbRotation(xQuat, yQuat, zQuat, (
float)val);
414 else if (
id == QString(
"Roll in deg"))
416 VirtualRobot::MathTools::rpy2eigen4f(
417 VirtualRobot::MathTools::deg2rad((
float)val), rpy(1), rpy(2), rotationMatrix);
418 quaternion = VirtualRobot::MathTools::eigen4f2quat(rotationMatrix);
419 newRotation = SbRotation(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
421 else if (
id == QString(
"Pitch in deg"))
423 VirtualRobot::MathTools::rpy2eigen4f(
424 rpy(0), VirtualRobot::MathTools::deg2rad((
float)val), rpy(2), rotationMatrix);
425 quaternion = VirtualRobot::MathTools::eigen4f2quat(rotationMatrix);
426 newRotation = SbRotation(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
428 else if (
id == QString(
"Yaw in deg"))
430 VirtualRobot::MathTools::rpy2eigen4f(
431 rpy(0), rpy(1), VirtualRobot::MathTools::deg2rad((
float)val), rotationMatrix);
432 quaternion = VirtualRobot::MathTools::eigen4f2quat(rotationMatrix);
433 newRotation = SbRotation(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
437 newRotation = currentSceneObject->getRotation();
442 newRotation.getValue(x, y, z, w);
444 VirtualRobot::MathTools::eigen4f2rpy(VirtualRobot::MathTools::quat2eigen4f(x, y, z, w), rot);
446 ARMARX_INFO_S <<
"Quat: \n" << x <<
", \n" << y <<
", \n" << z <<
", \n" << w;
453 gui::PropertyBrowserWidget::createNewTranslation(
QtProperty* property,
double val)
456 float xPos, yPos, zPos;
457 currentSceneObject->getTranslation().getValue(xPos, yPos, zPos);
459 QString
id = propertyToId[property];
461 if (
id == QString(
"X Position"))
463 newPosition = SbVec3f((
float)val, yPos, zPos);
465 else if (
id == QString(
"Y Position"))
467 newPosition = SbVec3f(xPos, (
float)val, zPos);
469 else if (
id == QString(
"Z Position"))
471 newPosition = SbVec3f(xPos, yPos, (
float)val);
475 newPosition = currentSceneObject->getTranslation();
482 gui::PropertyBrowserWidget::getMatrix()
485 sbMatrix.setRotate(currentSceneObject->getRotation());
486 sbMatrix.setTranslate(currentSceneObject->getTranslation());
487 std::string matrixToString;
489 for (
int i = 0; i < 4; ++i)
491 matrixToString.append(
boost::str(boost::format(
"%3.2g %3.2g %3.2g %3.2g") % sbMatrix[i][0] %
492 sbMatrix[i][1] % sbMatrix[i][2] % sbMatrix[i][3]));
496 matrixToString.append(
"\n");
500 return matrixToString;