From b390a088069350502a13aa830e8ee0ddb13ac878 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Tue, 22 Dec 2020 12:19:17 +0100
Subject: [PATCH] Remove deactivated code
---
.../GraspPlanner/GraspPlannerWindow.cpp | 34 ---
.../GraspQuality/GraspQualityWindow.cpp | 151 ----------
MotionPlanning/examples/IKRRT/IKRRTWindow.cpp | 22 --
.../BulletEngine/BulletObject.cpp | 4 -
.../BulletOpenGL/GL_DialogWindow.cpp | 5 -
.../BulletOpenGL/GL_ShapeDrawer.cpp | 40 ---
.../BulletOpenGL/Win32DemoApplication.cpp | 24 --
.../BulletEngine/BulletRobot.cpp | 15 -
.../BulletEngine/SimoxMotionState.cpp | 14 +-
SimDynamics/DynamicsEngine/DynamicsRobot.cpp | 16 --
SimDynamics/DynamicsWorld.cpp | 4 -
.../BulletDebugViewerGlut.cpp | 51 ----
.../BulletDebugViewerWin32.cpp | 40 ---
.../SimDynamicsViewer/simDynamicsWindow.cpp | 30 --
SimoxUtility/color/GlasbeyLUT.cpp | 267 ------------------
.../PQP/CollisionCheckerPQP.cpp | 110 +-------
.../PQP/tritri_isectline.cpp | 39 ---
.../GraspEditor/GraspEditorWindow.cpp | 28 --
VirtualRobot/IK/AdvancedIKSolver.cpp | 5 -
VirtualRobot/Util/xml/tinyxml2.cpp | 6 -
.../Workspace/WorkspaceRepresentation.cpp | 16 --
.../ConstrainedIK/ConstrainedIKWindow.cpp | 6 -
.../examples/GraspEditor/GraspEditor.cpp | 5 +-
.../Legacy/ColladaViewer/showRobotWindow.cpp | 76 -----
.../examples/RobotViewer/showRobotWindow.cpp | 260 -----------------
.../reachability/reachabilityWindow.cpp | 7 -
26 files changed, 3 insertions(+), 1272 deletions(-)
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
index 343194de2..137af831a 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
@@ -61,10 +61,6 @@ GraspPlannerWindow::GraspPlannerWindow(std::string& robFile, std::string& eefNam
graspsSep = new SoSeparator;
graspsSep->ref();
-#if 0
- SoSeparator* s = CoinVisualizationFactory::CreateCoordSystemVisualization();
- sceneSep->addChild(s);
-#endif
sceneSep->addChild(robotSep);
sceneSep->addChild(objectSep);
sceneSep->addChild(frictionConeSep);
@@ -314,12 +310,6 @@ void GraspPlannerWindow::loadObject(const string& objFile)
object = Obstacle::createBox(50.0f, 50.0f, 10.0f);
}
-#if 0
- TriMeshModelPtr tm = object->getVisualization()->getTriMeshModel();
- MathTools::ConvexHull3DPtr cv = ConvexHullGenerator::CreateConvexHull(tm->vertices);
- object = GraspStudio::MeshConverter::CreateManipulationObject(object->getName(), cv);
-#endif
-
//Eigen::Vector3f minS,maxS;
//object->getCollisionModel()->getTriMeshModel()->getSize(minS,maxS);
//cout << "minS: \n" << minS << "\nMaxS:\n" << maxS << std::endl;
@@ -422,30 +412,6 @@ void GraspPlannerWindow::plan()
void GraspPlannerWindow::closeEEF()
{
-#if 0
- static int pp = 0;
- static Eigen::Vector3f approachDir;
- /*object->getCollisionModel()->getTriMeshModel()->print();
- object->getCollisionModel()->getTriMeshModel()->checkAndCorrectNormals(false);*/
- object->getCollisionModel()->getTriMeshModel()->print();
-
- if (pp == 0)
- {
- Eigen::Vector3f position;
- approach->getPositionOnObject(position, approachDir);
-
- // set new pose
- approach->setEEFToApproachPose(position, approachDir);
- //eefCloned->getEndEffector(eefName)->getGCP()->showCoordinateSystem(true);
- }
- else
- {
- approach->moveEEFAway(approachDir, 3.0f, 5);
- }
-
- pp = (pp + 1) % 5;
- return;
-#endif
contacts.clear();
if (eefCloned && eefCloned->getEndEffector(eefName))
diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
index 318ee82a2..bd5f92f97 100644
--- a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
+++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.cpp
@@ -601,157 +601,6 @@ void GraspQualityWindow::showGWS()
ows1Sep->removeAllChildren();
ows2Sep->removeAllChildren();
-#if 0
- // test
-
- GraspStudio::ContactConeGeneratorPtr cgMM(new GraspStudio::ContactConeGenerator(8, 0.25f, 100.0f));
- std::vector<MathTools::ContactPoint> resultsMM;
- VirtualRobot::EndEffector::ContactInfoVector::const_iterator objPointsIter;
-
- for (objPointsIter = contacts.begin(); objPointsIter != contacts.end(); objPointsIter++)
- {
- MathTools::ContactPoint point;
-
- point.p = objPointsIter->contactPointObstacleLocal;
- //point.p -= centerOfModel;
-
- point.n = objPointsIter->contactPointFingerLocal - objPointsIter->contactPointObstacleLocal;
- point.n.normalize();
-
- cgMM->computeConePoints(point, resultsMM);
-
- SoSeparator* pV2 = CoinVisualizationFactory::CreatePointVisualization(point, true);
- gws1Sep->addChild(pV2);
-
- }
-
- SoSeparator* pV = CoinVisualizationFactory::CreatePointsVisualization(resultsMM, true);
- SoSeparator* scaledPoints = new SoSeparator;
- SoScale* sc0 = new SoScale;
- float sf0 = 1.0f;
- sc0->scaleFactor.setValue(sf0, sf0, sf0);
- scaledPoints->addChild(sc0);
- scaledPoints->addChild(pV);
- gws1Sep->addChild(scaledPoints);
-
-
-#endif
-
-#if 0
- // test
- robotSep->removeAllChildren();
- // plane
- Eigen::Vector3f posZero(0, 0, 0);
- Eigen::Vector3f posZeroN(0, 0, 1.0f);
- SoSeparator* pV3 = CoinVisualizationFactory::CreatePlaneVisualization(posZero, posZeroN, 1000.0f, 0.5f);
- gws1Sep->addChild(pV3);
-
-
- std::vector<MathTools::ContactPoint> resultsM;
- std::vector<MathTools::ContactPoint> resultsMM;
- MathTools::ContactPoint pointM;
- MathTools::ContactPoint pointMM;
- float pos = 0.0f;
- float length = 0.2f; //m
- float lengthMM = length * 1000.0f; //m
- pointM.p(0) = pos;
- pointM.p(1) = pos;
- pointM.p(2) = pos;
- pointM.n(0) = 1.0f;
- pointM.n(1) = 1.0f;
- pointM.n(2) = 1.0f;
- pointM.n.normalize();
- pointMM.n = pointM.n;
- pointMM.p = pointM.p * 1000.0f;
- MathTools::ContactPoint pointM2;
- MathTools::ContactPoint pointMM2;
- pointM2.p(0) = pos;
- pointM2.p(1) = pos - length;
- pointM2.p(2) = pos;
- pointM2.n(0) = 0.0f;
- pointM2.n(1) = -1.0f;
- pointM2.n(2) = 0.0f;
- pointMM2.n = pointM2.n;
- pointMM2.p = pointM2.p * 1000.0f;
- MathTools::ContactPoint pointM3;
- MathTools::ContactPoint pointMM3;
- pointM3.p(0) = pos;
- pointM3.p(1) = pos;
- pointM3.p(2) = pos + length;
- pointM3.n(0) = 0.0f;
- pointM3.n(1) = 0.0f;
- pointM3.n(2) = 1.0f;
- pointMM3.n = pointM3.n;
- pointMM3.p = pointM3.p * 1000.0f;
- GraspStudio::ContactConeGeneratorPtr cg(new GraspStudio::ContactConeGenerator(8, 0.25f, 1.0f));
- GraspStudio::ContactConeGeneratorPtr cgMM(new GraspStudio::ContactConeGenerator(8, 0.25f, 100.0f));
- cg->computeConePoints(pointM, resultsM);
- cg->computeConePoints(pointM2, resultsM);
- cg->computeConePoints(pointM3, resultsM);
- cgMM->computeConePoints(pointMM, resultsMM);
- cgMM->computeConePoints(pointMM2, resultsMM);
- cgMM->computeConePoints(pointMM3, resultsMM);
-
- SoSeparator* pV = CoinVisualizationFactory::CreatePointsVisualization(resultsMM, true);
- SoSeparator* scaledPoints = new SoSeparator;
- SoScale* sc0 = new SoScale;
- float sf0 = 1.0f;
- sc0->scaleFactor.setValue(sf0, sf0, sf0);
- scaledPoints->addChild(sc0);
- scaledPoints->addChild(pV);
- gws1Sep->addChild(scaledPoints);
- SoSeparator* pV2 = CoinVisualizationFactory::CreatePointVisualization(pointMM, true);
- SoSeparator* pV2b = CoinVisualizationFactory::CreatePointVisualization(pointMM2, true);
- SoSeparator* pV2c = CoinVisualizationFactory::CreatePointVisualization(pointMM3, true);
- gws1Sep->addChild(pV2);
- gws1Sep->addChild(pV2b);
- gws1Sep->addChild(pV2c);
-
- // plane
- Eigen::Vector3f posZero(0, 0, 0);
- Eigen::Vector3f posZeroN(0, 0, 1.0f);
- SoSeparator* pV3 = CoinVisualizationFactory::CreatePlaneVisualization(posZero, posZeroN, 1000.0f, 0.5f);
- gws1Sep->addChild(pV3);
-
-
- // wrench
- Eigen::Vector3f com = 0.333f * (pointM.p + pointM2.p + pointM3.p);
- std::vector<VirtualRobot::MathTools::ContactPoint> wrenchP = GraspStudio::GraspQualityMeasureWrenchSpace::createWrenchPoints(resultsM, com, lengthMM);
- MathTools::print(wrenchP);
- // convex hull
- VirtualRobot::MathTools::ConvexHull6DPtr ch1 = GraspStudio::ConvexHullGenerator::CreateConvexHull(wrenchP);
- float minO = GraspStudio::GraspQualityMeasureWrenchSpace::minOffset(ch1);
- std::cout << "minOffset:" << minO << std::endl;
- std::vector<MathTools::TriangleFace6D>::iterator faceIter;
- std::cout << "Distances to Origin:" << std::endl;
-
- for (faceIter = ch1->faces.begin(); faceIter != ch1->faces.end(); faceIter++)
- {
- std::cout << faceIter->distPlaneZero << ", ";
-
- if (faceIter->distPlaneZero > 1e-4)
- {
- std::cout << "<-- not force closure " << std::endl;
- }
-
- }
-
- std::cout << std::endl;
- // ch visu
- GraspStudio::CoinConvexHullVisualizationPtr visu(new GraspStudio::CoinConvexHullVisualization(ch1, false));
- SoSeparator* chV = visu->getCoinVisualization();
- SoSeparator* scaledCH = new SoSeparator;
- SoScale* sc1 = new SoScale;
- float sf1 = 1.0f / 4.0f;
- sc1->scaleFactor.setValue(sf1, sf1, sf1);
- scaledCH->addChild(sc1);
- scaledCH->addChild(chV);
- gws1Sep->addChild(scaledCH);
-
- return;
-#endif
-
-
if (!qualityMeasure)
{
return;
diff --git a/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp
index 1fdc67198..1ed881c76 100644
--- a/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp
+++ b/MotionPlanning/examples/IKRRT/IKRRTWindow.cpp
@@ -720,28 +720,6 @@ void IKRRTWindow::planIKRRT()
void IKRRTWindow::colModel()
{
-#if 0
-
- if (reachSpace && graspSet && object)
- {
- Eigen::Matrix4f m = reachSpace->sampleReachablePose();
- std::cout << "getEntry: " << (int)reachSpace->getEntry(m) << std::endl;
- /*SoSeparator* sep1 = new SoSeparator;
- SoSeparator *cs = CoinVisualizationFactory::CreateCoordSystemVisualization();
- SoMatrixTransform *mt = new SoMatrixTransform;
- SbMatrix ma(reinterpret_cast<SbMat*>(m.data()));
- mt->matrix.setValue(ma);
- sep1->addChild(mt);
- sep1->addChild(cs);
- sceneSep->addChild(sep1);*/
-
-
- GraspPtr g = graspSet->getGrasp(0);
- m = g->getObjectTargetPoseGlobal(m);
- object->setGlobalPose(m);
- }
-
-#endif
buildVisu();
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index b87992927..5a13734ce 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -164,10 +164,6 @@ namespace SimDynamics
rigidBody.reset(new btRigidBody(btRBInfo));
rigidBody->setUserPointer((void*)(this));
-#if 0
- rigidBody->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
- std::cout << "TEST3" << std::endl;
-#endif
setPoseIntern(o->getGlobalPose());
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.cpp
index 9b929b58a..b94cac2c4 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.cpp
@@ -210,11 +210,6 @@ void GL_DialogWindow::draw(btScalar deltaTime)
void GL_DialogWindow::saveOpenGLState()
{
-#if 0
- glPushAttrib(GL_ALL_ATTRIB_BITS);
- glPushClientAttrib(GL_CLIENT_ALL_ATTRIB_BITS);
-#endif
-
glMatrixMode(GL_TEXTURE);
glPushMatrix();
glLoadIdentity();
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp
index edf3f4485..58a5e6285 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp
@@ -666,46 +666,6 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
break;
}
-
-
-#if 0
-
- case CONE_SHAPE_PROXYTYPE:
- {
- const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
- int upIndex = coneShape->getConeUpIndex();
- float radius = coneShape->getRadius();//+coneShape->getMargin();
- float height = coneShape->getHeight();//+coneShape->getMargin();
-
- switch (upIndex)
- {
- case 0:
- glRotatef(90.0, 0.0, 1.0, 0.0);
- break;
-
- case 1:
- glRotatef(-90.0, 1.0, 0.0, 0.0);
- break;
-
- case 2:
- break;
-
- default:
- {
- }
- };
-
- glTranslatef(0.0, 0.0, -0.5 * height);
-
- glutSolidCone(radius, height, 10, 10);
-
-
- break;
-
- }
-
-#endif
-
case STATIC_PLANE_PROXYTYPE:
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/Win32DemoApplication.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/Win32DemoApplication.cpp
index a9b54aed5..4a5605fcf 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/Win32DemoApplication.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/Win32DemoApplication.cpp
@@ -17,30 +17,6 @@ subject to the following restrictions:
#include "Win32DemoApplication.h"
-
-
-
-#if 0
-void Win32DemoApplication::renderme()
-{
-}
-void Win32DemoApplication::setTexturing(bool useTexture)
-{
-}
-
-void Win32DemoApplication::setShadows(bool useShadows)
-{
-}
-
-void Win32DemoApplication::setCameraDistance(float camDist)
-{
-}
-void Win32DemoApplication::clientResetScene()
-{
-
-}
-#endif
-
void Win32DemoApplication::updateModifierKeys()
{
//not yet
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index aac254033..466460a2a 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -490,21 +490,6 @@ namespace SimDynamics
void BulletRobot::ensureKinematicConstraints()
{
// results in strange behavior?!
-#if 0
- // update globalpose of robot
- Eigen::Matrix4f gpRoot = robot->getRootNode()->getGlobalPoseVisualization();
- Eigen::Matrix4f rootPreJoint = robot->getRootNode()->getPreJointTransformation();
- robot->setGlobalPose(gpRoot * rootPreJoint.inverse());
- //robot->applyJointValues();
- std::map<VirtualRobot::RobotNodePtr, DynamicsObjectPtr>::iterator i = dynamicRobotNodes.begin();
-
- while (i != dynamicRobotNodes.end())
- {
- i->second->setPose(i->first->getGlobalPoseVisualization());
- i++;
- }
-
-#endif
}
void BulletRobot::actuateJoints(double dt)
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
index b3a2feac6..ff26ef993 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.cpp
@@ -157,19 +157,7 @@ namespace SimDynamics
// we assume that all models are handled by Bullet, so we do not need to update children
robotNodeActuator->updateVisualizationPose(resPose, false);
-#if 0
-
- if (rn->getName() == "Shoulder 1 L")
- {
- std::cout << "Shoulder 1 L:" << ja << ", speed:" << bdr->getJointSpeed(rn) << std::endl;
- }
-
-#endif
- } /*else
-
- {
- VR_WARNING << "Could not determine dynamic robot?!" << std::endl;
- }*/
+ }
}
else
{
diff --git a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
index 079587e57..c5b0136c5 100644
--- a/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsRobot.cpp
@@ -116,22 +116,6 @@ namespace SimDynamics
VR_ASSERT(node);
VR_ASSERT(robot->hasRobotNode(node));
- // if node is a joint without model, there is no dyn node!
- //DynamicsObjectPtr dnyRN;
- //if (hasDynamicsRobotNode(node))
- // dnyRN = getDynamicsRobotNode(node);
- // createDynamicsNode(node);
-
-
-#if 0
-
- if (node->getName() == "Elbow R")
- {
- std::cout << "##### +++++ New Node target:" << node->getName() << ", jointValue:" << jointValue << std::endl;
- }
-
-#endif
-
robotNodeActuationTarget target;
target.actuation.modes.position = 1;
target.node = node;
diff --git a/SimDynamics/DynamicsWorld.cpp b/SimDynamics/DynamicsWorld.cpp
index 6dcccfff4..b6fe255f8 100644
--- a/SimDynamics/DynamicsWorld.cpp
+++ b/SimDynamics/DynamicsWorld.cpp
@@ -14,11 +14,7 @@ namespace SimDynamics
DynamicsWorldPtr DynamicsWorld::world;
-#if 0
- bool DynamicsWorld::convertMM2M = false;
-#else
bool DynamicsWorld::convertMM2M = true;
-#endif
DynamicsWorld::Cleanup::~Cleanup()
{
diff --git a/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerGlut.cpp b/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerGlut.cpp
index 614cfb751..c1fd17ecf 100644
--- a/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerGlut.cpp
+++ b/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerGlut.cpp
@@ -170,25 +170,6 @@ int main(int argc, char* argv[])
world->addObject(dynObj);
o->print();
-
-#if 0
- std::string f = "/home/niko/coding/armarx/SimulationX/data/environment/KIT_Robot_Kitchen.xml";
- ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
- SimDynamics::DynamicsObjectPtr dynObj2 = world->CreateDynamicsObject(mo, DynamicsObject::eKinematic);
- world->addObject(dynObj2);
-#endif
-
-
-#if 0
- std::string f = "/home/SMBAD/vahrenka/.armarx/mongo/.cache/files/lowersink.xml";
- ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
- SimDynamics::DynamicsObjectPtr dynObj2 = world->CreateDynamicsObject(mo);
- //dynObj->setPosition(Eigen::Vector3f(3000,3000,1000.0f));
- world->addObject(dynObj2);
-#endif
-
-
-
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile);
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robFile);
@@ -208,37 +189,5 @@ int main(int argc, char* argv[])
BulletOpenGLViewer viewer(world);
//viewer.enableContraintsDebugDrawing();
-#if 0
- std::cout << "TEST7" << std::endl;
- ObstaclePtr o = Obstacle::createBox(10, 10, 1500);
- DynamicsObjectPtr do1 = DynamicsWorld::GetWorld()->CreateDynamicsObject(o, DynamicsObject::eStatic);
- ObstaclePtr o2 = Obstacle::createBox(10, 10, 1000);
- Eigen::Matrix4f gpxy = Eigen::Matrix4f::Identity();
- //gpxy(1,3) -= 213.0f;
- gpxy(0, 3) += 3000.0f;
- o2->setGlobalPose(gpxy);
- DynamicsObjectPtr do2 = DynamicsWorld::GetWorld()->CreateDynamicsObject(o2, DynamicsObject::eStatic);
- DynamicsEnginePtr e = DynamicsWorld::GetWorld()->getEngine();
- e->disableCollision(do1.get());
- e->disableCollision(do2.get());
- /*
- std::vector<DynamicsObjectPtr> dos = e->getObjects();
- for (size_t i=0;i<dos.size();i++)
- {
- e->disableCollision(do1.get(),dos[i].get());
- e->disableCollision(do2.get(),dos[i].get());
- if (e->checkCollisionEnabled(do1.get(),dos[i].get()))
- {
- std::cout << "OOPS" << std::endl;
- }
- if (e->checkCollisionEnabled(do2.get(),dos[i].get()))
- {
- std::cout << "OOPS" << std::endl;
- }
- }*/
- e->addObject(do1);
- e->addObject(do2);
-#endif
-
return glutmain(argc, argv, 640, 480, "Show SimDynamics Scene", &viewer);
}
diff --git a/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerWin32.cpp b/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerWin32.cpp
index 38d61b4d6..60f2e5e52 100644
--- a/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerWin32.cpp
+++ b/SimDynamics/examples/BulletDebugViewer/BulletDebugViewerWin32.cpp
@@ -163,14 +163,6 @@ DemoApplication* createDemo()
dynObj->setPosition(Eigen::Vector3f(3000, 3000, 1000.0f));
world->addObject(dynObj);
-#if 0
- std::string f = "/home/niko/coding/armarx/SimulationX/data/environment/KIT_Robot_Kitchen.xml";
- ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
- SimDynamics::DynamicsObjectPtr dynObj2 = world->CreateDynamicsObject(mo, DynamicsObject::eKinematic);
- //dynObj->setPosition(Eigen::Vector3f(3000,3000,1000.0f));
- world->addObject(dynObj2);
-#endif
-
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile);
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robFile);
@@ -188,37 +180,5 @@ DemoApplication* createDemo()
BulletOpenGLViewer* viewer = new BulletOpenGLViewer(world);
//viewer.enableContraintsDebugDrawing();
-#if 0
- cout << "TEST7" << endl;
- ObstaclePtr o = Obstacle::createBox(10, 10, 1500);
- DynamicsObjectPtr do1 = DynamicsWorld::GetWorld()->CreateDynamicsObject(o, DynamicsObject::eStatic);
- ObstaclePtr o2 = Obstacle::createBox(10, 10, 1000);
- Eigen::Matrix4f gpxy = Eigen::Matrix4f::Identity();
- //gpxy(1,3) -= 213.0f;
- gpxy(0, 3) += 3000.0f;
- o2->setGlobalPose(gpxy);
- DynamicsObjectPtr do2 = DynamicsWorld::GetWorld()->CreateDynamicsObject(o2, DynamicsObject::eStatic);
- DynamicsEnginePtr e = DynamicsWorld::GetWorld()->getEngine();
- e->disableCollision(do1.get());
- e->disableCollision(do2.get());
- /*
- std::vector<DynamicsObjectPtr> dos = e->getObjects();
- for (size_t i=0;i<dos.size();i++)
- {
- e->disableCollision(do1.get(),dos[i].get());
- e->disableCollision(do2.get(),dos[i].get());
- if (e->checkCollisionEnabled(do1.get(),dos[i].get()))
- {
- cout << "OOPS" << endl;
- }
- if (e->checkCollisionEnabled(do2.get(),dos[i].get()))
- {
- cout << "OOPS" << endl;
- }
- }*/
- e->addObject(do1);
- e->addObject(do2);
-#endif
-
return viewer;
}
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index 788f59343..3ba15ec79 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -74,26 +74,6 @@ SimDynamicsWindow::SimDynamicsWindow(std::string& sRobotFilename)
addObject();
-#if 0
- std::string f = "/home/niko/coding/armarx/SimulationX/data/environment/KIT_Robot_Kitchen.xml";
- ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
- mo->setSimulationType(VirtualRobot::SceneObject::Physics::eStatic);
- dynamicsObject2 = dynamicsWorld->CreateDynamicsObject(mo);
- //dynObj->setPosition(Eigen::Vector3f(3000,3000,1000.0f));
- dynamicsWorld->addObject(dynamicsObject2);
-#endif
-
-#if 0
- std::string f = "/home/SMBAD/vahrenka/.armarx/mongo/.cache/files/lowersink.xml";
- ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
- mo->setSimulationType(SceneObject::Physics::eDynamic);
- SimDynamics::DynamicsObjectPtr dynObj2 = dynamicsWorld->CreateDynamicsObject(mo);
- //dynObj2->setSimType(VirtualRobot::SceneObject::Physics::eDynamic);
- dynObj2->setPosition(Eigen::Vector3f(0, 0, -200.0f));
- dynamicsWorld->addObject(dynObj2);
- dynamicsObjects.push_back(dynObj2);
-#endif
-
setupUI();
loadRobot(robotFilename);
@@ -734,16 +714,6 @@ void SimDynamicsWindow::updateJointInfo()
UI.label_RNRPYGP->setText(qGPRPY);
UI.label_RNPosVisu->setText(qVisu);
UI.label_RNPosCom->setText(qCom);
-
-#if 0
-
- // print some joint info
- if (viewer->engineRunning())
- {
- std::cout << info << std::endl;
- }
-
-#endif
}
void SimDynamicsWindow::updateRobotInfo()
diff --git a/SimoxUtility/color/GlasbeyLUT.cpp b/SimoxUtility/color/GlasbeyLUT.cpp
index 5aa62689f..13a3be7f1 100644
--- a/SimoxUtility/color/GlasbeyLUT.cpp
+++ b/SimoxUtility/color/GlasbeyLUT.cpp
@@ -293,271 +293,4 @@ namespace simox::color
{
return GLASBEY_LUT;
}
-
-
-#if 0
-
- /// Viridis lookup table
- static const unsigned char VIRIDIS_LUT[] =
- {
- 68, 1, 84,
- 68, 2, 85,
- 69, 3, 87,
- 69, 5, 88,
- 69, 6, 90,
- 70, 8, 91,
- 70, 9, 93,
- 70, 11, 94,
- 70, 12, 96,
- 71, 14, 97,
- 71, 15, 98,
- 71, 17, 100,
- 71, 18, 101,
- 71, 20, 102,
- 72, 21, 104,
- 72, 22, 105,
- 72, 24, 106,
- 72, 25, 108,
- 72, 26, 109,
- 72, 28, 110,
- 72, 29, 111,
- 72, 30, 112,
- 72, 32, 113,
- 72, 33, 115,
- 72, 34, 116,
- 72, 36, 117,
- 72, 37, 118,
- 72, 38, 119,
- 72, 39, 120,
- 71, 41, 121,
- 71, 42, 121,
- 71, 43, 122,
- 71, 44, 123,
- 71, 46, 124,
- 70, 47, 125,
- 70, 48, 126,
- 70, 49, 126,
- 70, 51, 127,
- 69, 52, 128,
- 69, 53, 129,
- 69, 54, 129,
- 68, 56, 130,
- 68, 57, 131,
- 68, 58, 131,
- 67, 59, 132,
- 67, 60, 132,
- 67, 62, 133,
- 66, 63, 133,
- 66, 64, 134,
- 65, 65, 134,
- 65, 66, 135,
- 65, 67, 135,
- 64, 69, 136,
- 64, 70, 136,
- 63, 71, 136,
- 63, 72, 137,
- 62, 73, 137,
- 62, 74, 137,
- 61, 75, 138,
- 61, 77, 138,
- 60, 78, 138,
- 60, 79, 138,
- 59, 80, 139,
- 59, 81, 139,
- 58, 82, 139,
- 58, 83, 139,
- 57, 84, 140,
- 57, 85, 140,
- 56, 86, 140,
- 56, 87, 140,
- 55, 88, 140,
- 55, 89, 140,
- 54, 91, 141,
- 54, 92, 141,
- 53, 93, 141,
- 53, 94, 141,
- 52, 95, 141,
- 52, 96, 141,
- 51, 97, 141,
- 51, 98, 141,
- 51, 99, 141,
- 50, 100, 142,
- 50, 101, 142,
- 49, 102, 142,
- 49, 103, 142,
- 48, 104, 142,
- 48, 105, 142,
- 47, 106, 142,
- 47, 107, 142,
- 47, 108, 142,
- 46, 109, 142,
- 46, 110, 142,
- 45, 111, 142,
- 45, 112, 142,
- 45, 112, 142,
- 44, 113, 142,
- 44, 114, 142,
- 43, 115, 142,
- 43, 116, 142,
- 43, 117, 142,
- 42, 118, 142,
- 42, 119, 142,
- 41, 120, 142,
- 41, 121, 142,
- 41, 122, 142,
- 40, 123, 142,
- 40, 124, 142,
- 40, 125, 142,
- 39, 126, 142,
- 39, 127, 142,
- 38, 128, 142,
- 38, 129, 142,
- 38, 130, 142,
- 37, 131, 142,
- 37, 131, 142,
- 37, 132, 142,
- 36, 133, 142,
- 36, 134, 142,
- 35, 135, 142,
- 35, 136, 142,
- 35, 137, 142,
- 34, 138, 141,
- 34, 139, 141,
- 34, 140, 141,
- 33, 141, 141,
- 33, 142, 141,
- 33, 143, 141,
- 32, 144, 141,
- 32, 145, 140,
- 32, 146, 140,
- 32, 147, 140,
- 31, 147, 140,
- 31, 148, 140,
- 31, 149, 139,
- 31, 150, 139,
- 31, 151, 139,
- 30, 152, 139,
- 30, 153, 138,
- 30, 154, 138,
- 30, 155, 138,
- 30, 156, 137,
- 30, 157, 137,
- 30, 158, 137,
- 30, 159, 136,
- 30, 160, 136,
- 31, 161, 136,
- 31, 162, 135,
- 31, 163, 135,
- 31, 163, 134,
- 32, 164, 134,
- 32, 165, 134,
- 33, 166, 133,
- 33, 167, 133,
- 34, 168, 132,
- 35, 169, 131,
- 35, 170, 131,
- 36, 171, 130,
- 37, 172, 130,
- 38, 173, 129,
- 39, 174, 129,
- 40, 175, 128,
- 41, 175, 127,
- 42, 176, 127,
- 43, 177, 126,
- 44, 178, 125,
- 46, 179, 124,
- 47, 180, 124,
- 48, 181, 123,
- 50, 182, 122,
- 51, 183, 121,
- 53, 183, 121,
- 54, 184, 120,
- 56, 185, 119,
- 57, 186, 118,
- 59, 187, 117,
- 61, 188, 116,
- 62, 189, 115,
- 64, 190, 114,
- 66, 190, 113,
- 68, 191, 112,
- 70, 192, 111,
- 72, 193, 110,
- 73, 194, 109,
- 75, 194, 108,
- 77, 195, 107,
- 79, 196, 106,
- 81, 197, 105,
- 83, 198, 104,
- 85, 198, 102,
- 88, 199, 101,
- 90, 200, 100,
- 92, 201, 99,
- 94, 201, 98,
- 96, 202, 96,
- 98, 203, 95,
- 101, 204, 94,
- 103, 204, 92,
- 105, 205, 91,
- 108, 206, 90,
- 110, 206, 88,
- 112, 207, 87,
- 115, 208, 85,
- 117, 208, 84,
- 119, 209, 82,
- 122, 210, 81,
- 124, 210, 79,
- 127, 211, 78,
- 129, 212, 76,
- 132, 212, 75,
- 134, 213, 73,
- 137, 213, 72,
- 139, 214, 70,
- 142, 215, 68,
- 144, 215, 67,
- 147, 216, 65,
- 149, 216, 63,
- 152, 217, 62,
- 155, 217, 60,
- 157, 218, 58,
- 160, 218, 57,
- 163, 219, 55,
- 165, 219, 53,
- 168, 220, 51,
- 171, 220, 50,
- 173, 221, 48,
- 176, 221, 46,
- 179, 221, 45,
- 181, 222, 43,
- 184, 222, 41,
- 187, 223, 39,
- 189, 223, 38,
- 192, 223, 36,
- 195, 224, 35,
- 197, 224, 33,
- 200, 225, 32,
- 203, 225, 30,
- 205, 225, 29,
- 208, 226, 28,
- 211, 226, 27,
- 213, 226, 26,
- 216, 227, 25,
- 219, 227, 24,
- 221, 227, 24,
- 224, 228, 24,
- 226, 228, 24,
- 229, 228, 24,
- 232, 229, 25,
- 234, 229, 25,
- 237, 229, 26,
- 239, 230, 27,
- 242, 230, 28,
- 244, 230, 30,
- 247, 230, 31,
- 249, 231, 33,
- 251, 231, 35,
- 254, 231, 36,
- };
-
-#endif
-
}
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
index 434e7910a..6075d0d3b 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
@@ -405,113 +405,5 @@ namespace VirtualRobot
Rotation2, Translation2, model2.get(),
0, 0); // default: 0 error
}
- /*
- void CollisionCheckerPQP::PQP2Transform(PQP::PQP_REAL R[3][3], PQP::PQP_REAL T[3], Transform &t)
- {
- for(int i=0; i<3; i++)
- {
- for(int j=0; j<3; j++)
- {
- t.Rotation()[i][j]= R[i][j];
- }
- t.Translation()[i]= T[i];
- }
- }
- bool CollisionCheckerPQP::CheckContinuousCollision( CollisionModelPtr model1, SbMatrix &mGoalPose1, CollisionModelPtr model2, SbMatrix &mGoalPose2, float &fStoreTOC )
- {
- if (model1==NULL || model2==NULL)
- {
- printf ("CollisionCheckerPQP:CheckContinuousCollision - NULL data...\n");
- return false;
- }
- if (model1->GetCollisionChecker() != model2->GetCollisionChecker() || model1->GetCollisionChecker()->getCollisionCheckerImplementation()!=this)
- {
- printf ("CollisionCheckerPQP:CheckContinuousCollision - Could not go on, collision models are linked to different Collision Checker instances...\n");
- return false;
- }
-
- PQP::PQP_ModelPtr m1 = model1->getCollisionModelImplementation()->GetModel();
- PQP::PQP_ModelPtr m2 = model2->getCollisionModelImplementation()->GetModel();
- if (m1==NULL || m2==NULL)
- {
- printf ("CollisionCheckerPQP:CheckContinuousCollision - NULL internal data...\n");
- return false;
- }
-
- // check rotation!! In case rotation around axis is larger than ??PI?? we have to split the motion !!!!
- // -> not here, we do not know the requested rotation at this point, check at higher levels
-
- // todo: this can be optimized
- //PQP_CollideResult result;
-
- // start pose
- PQP::PQP_REAL R1[3][3];
- PQP::PQP_REAL T1[3];
- PQP::PQP_REAL R2[3][3];
- PQP::PQP_REAL T2[3];
- __convSb2Ar(model1->getCollisionModelImplementation()->getGlobalPose(),R1,T1);
- __convSb2Ar(model2->getCollisionModelImplementation()->getGlobalPose(),R2,T2);
- Transform trans00;
- PQP2Transform(R1,T1,trans00);
- Transform trans10;
- PQP2Transform(R2,T2,trans10);
-
- // goal pose
- __convSb2Ar(mGoalPose1,R1,T1);
- __convSb2Ar(mGoalPose2,R2,T2);
- Transform trans01;
- PQP2Transform(R1,T1,trans01);
- Transform trans11;
- PQP2Transform(R2,T2,trans11);
- Transform trans0;
- Transform trans1;
-
- PQP::PQP_REAL time_of_contact;
- int number_of_iteration;
- int number_of_contact;
- PQP::PQP_REAL th_ca = 0;
- PQP_TimeOfContactResult dres;
- PQP_Result ccdResult;
- ccdResult =
- PQP_Solve(&trans00,
- &trans01,
- m1,
- &trans10,
- &trans11,
- m2,
- trans0,
- trans1,
- time_of_contact,
- number_of_iteration,
- number_of_contact,
- th_ca,
- dres);
-
- float fResDist = (float)dres.Distance();
- int numCA = dres.numCA;
- bool flag_collision = !dres.collisionfree;
-
- #if 0
- if (flag_collision)
- {
- std::cout << "ccd result: fResDist:" << fResDist << std::endl;
- std::cout << "ccd result: numCA:" << numCA << std::endl;
- std::cout << "ccd result: time_of_contact:" << time_of_contact << std::endl;
- std::cout << "ccd result: flag_collision:" << flag_collision << std::endl;
- std::cout << "ccd result: number_of_iteration:" << number_of_iteration << std::endl;
- std::cout << "ccd result: trans0:" << trans0.Translation().X() << "," << trans0.Translation().Y() << "," << trans0.Translation().Z() << std::endl;
- std::cout << "ccd result: trans1:" << trans1.Translation().X() << "," << trans1.Translation().Y() << "," << trans1.Translation().Z() << std::endl;
- std::cout << "ccd result: ccdResult:" << ccdResult << std::endl;
-
- } else
- {
- std::cout << ".";
- }
- #endif
- fStoreTOC = (float)time_of_contact;
- return flag_collision;
- }*/
-
-} // namespace
-
+}
diff --git a/VirtualRobot/CollisionDetection/PQP/tritri_isectline.cpp b/VirtualRobot/CollisionDetection/PQP/tritri_isectline.cpp
index 3091ae4c7..205f7d6b9 100644
--- a/VirtualRobot/CollisionDetection/PQP/tritri_isectline.cpp
+++ b/VirtualRobot/CollisionDetection/PQP/tritri_isectline.cpp
@@ -483,19 +483,6 @@ inline void isect2(
}
-#if 0
-#define ISECT2(VTX0,VTX1,VTX2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1) \
- tmp=D0/(D0-D1); \
- isect0=VV0+(VV1-VV0)*tmp; \
- SUB(diff,VTX1,VTX0); \
- MULT(diff,diff,tmp); \
- ADD(isectpoint0,diff,VTX0); \
- tmp=D0/(D0-D2);
-/* isect1=VV0+(VV2-VV0)*tmp; \ */
-/* SUB(diff,VTX2,VTX0); \ */
-/* MULT(diff,diff,tmp); \ */
-/* ADD(isectpoint1,VTX0,diff); */
-#endif
inline int compute_intervals_isectline(
const float VERT0[3],const float VERT1[3],const float VERT2[3],
@@ -542,32 +529,6 @@ inline int compute_intervals_isectline(
/* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \
}
-#if 0
- else if(D0D2>0.0f) \
- { \
- /* here we know that d0d1<=0.0 */ \
- isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
- } \
- else if(D1*D2>0.0f || D0!=0.0f) \
- { \
- /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
- isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
- } \
- else if(D1!=0.0f) \
- { \
- isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
- } \
- else if(D2!=0.0f) \
- { \
- isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \
- } \
- else \
- { \
- /* triangles are coplanar */ \
- coplanar=1; \
- return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
- }
-#endif
int tri_tri_intersect_with_isectline(
const float V0[3],const float V1[3],const float V2[3],
diff --git a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp
index e5d76169e..6192a0f33 100644
--- a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp
+++ b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp
@@ -63,34 +63,6 @@ namespace VirtualRobot
sceneSep->addChild(objectSep);
sceneSep->addChild(graspSetVisu);
-#if 0
- // 2d map test
- Eigen::MatrixXf d(10, 10);
-
- for (int x = 0; x < 10; x++)
- for (int y = 0; y < 10; y++)
- {
- d(x, y) = (float)(x + y) / 20.0f;
- }
-
- SoSeparator* sep1 = CoinVisualizationFactory::Create2DMap(d, 10.0f, 10.0f, VirtualRobot::ColorMap::ColorMap(VirtualRobot::ColorMap::eHot), true);
- SoSeparator* sep2 = CoinVisualizationFactory::Create2DHeightMap(d, 10.0f, 10.0f, 50.0f);
- sceneSep->addChild(sep1);
- sceneSep->addChild(sep2);
-#endif
-
-#if 0
- SphereApproximatorPtr sa(new SphereApproximator());
- SphereApproximator::SphereApproximation app;
- sa->generateGraph(app, SphereApproximator::eIcosahedron, 3, 200.0f);
- std::cout << "nr faces:" << app.faces.size() << ", vert:" << app.vertices.size() << std::endl;
-
- TriMeshModelPtr tri = sa->generateTriMesh(app);
- std::cout << "2 nr faces:" << tri->faces.size() << ", vert:" << tri->vertices.size() << std::endl;
- SoNode* m = CoinVisualizationFactory::getCoinVisualization(tri, true);
- sceneSep->addChild(m);
-
-#endif
setupUI();
loadObject();
diff --git a/VirtualRobot/IK/AdvancedIKSolver.cpp b/VirtualRobot/IK/AdvancedIKSolver.cpp
index a352d6c50..8e80ddce3 100644
--- a/VirtualRobot/IK/AdvancedIKSolver.cpp
+++ b/VirtualRobot/IK/AdvancedIKSolver.cpp
@@ -201,11 +201,6 @@ namespace VirtualRobot
GraspPtr g = graspSet->getGrasp(pos);
Eigen::Matrix4f m = g->getTcpPoseGlobal(object->getGlobalPose());
-#if 0
-
- // assuming that reachability is checked in the solve() method, so we don't have ot check it here (->avoid double checks)
- if (checkReachable(m))
-#endif
if (_sampleSolution(m, selection, maxLoops))
{
return g;
diff --git a/VirtualRobot/Util/xml/tinyxml2.cpp b/VirtualRobot/Util/xml/tinyxml2.cpp
index f5077fca0..526de58c0 100644
--- a/VirtualRobot/Util/xml/tinyxml2.cpp
+++ b/VirtualRobot/Util/xml/tinyxml2.cpp
@@ -2057,12 +2057,6 @@ void XMLDocument::Clear()
_charBuffer = nullptr;
_parsingDepth = 0;
-#if 0
- _textPool.Trace( "text" );
- _elementPool.Trace( "element" );
- _commentPool.Trace( "comment" );
- _attributePool.Trace( "attribute" );
-#endif
#ifdef TINYXML2_DEBUG
if ( !hadError ) {
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
index 99b39eb57..b1528b88c 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
@@ -1883,23 +1883,7 @@ namespace VirtualRobot
if (getVoxelFromPose(x, v))
{
-#if 0
- std::cout << "pose:";
- for (int i = 0; i < 6; i++)
- {
- std::cout << x[i] << ",";
- }
-
- std::cout << "Voxel:";
-
- for (int i = 0; i < 6; i++)
- {
- std::cout << v[i] << ",";
- }
-
- std::cout << std::endl;
-#endif
data->setDatumCheckNeighbors(v, e, neighborVoxels);
}
diff --git a/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.cpp b/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.cpp
index 5b87967c2..f9ebc2e80 100644
--- a/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.cpp
+++ b/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.cpp
@@ -642,12 +642,6 @@ void ConstrainedIKWindow::performanceEvaluation()
ik->addConstraint(balanceConstraint);
}
-#if 0
- JointLimitAvoidanceConstraintPtr jointLimitConstraint(new JointLimitAvoidanceConstraint(robot, kc));
- ik->addConstraint(jointLimitConstraint);
- VR_INFO << "JLAC Added" << std::endl;
-#endif
-
ik->initialize();
clock_t startT = clock();
diff --git a/VirtualRobot/examples/GraspEditor/GraspEditor.cpp b/VirtualRobot/examples/GraspEditor/GraspEditor.cpp
index 780e59ae0..8b8c2e221 100644
--- a/VirtualRobot/examples/GraspEditor/GraspEditor.cpp
+++ b/VirtualRobot/examples/GraspEditor/GraspEditor.cpp
@@ -31,10 +31,7 @@ int main(int argc, char* argv[])
std::string filename1("objects/plate.xml");
std::string filename2("robots/ArmarIII/ArmarIII.xml");
-#if 0
- filename1 = "objects/iCub/LegoXWing_RightHand_300.xml";
- filename2 = "robots/iCub/iCub.xml";
-#endif
+
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename1);
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename2);
diff --git a/VirtualRobot/examples/Legacy/ColladaViewer/showRobotWindow.cpp b/VirtualRobot/examples/Legacy/ColladaViewer/showRobotWindow.cpp
index dd65f18e0..de271f94b 100644
--- a/VirtualRobot/examples/Legacy/ColladaViewer/showRobotWindow.cpp
+++ b/VirtualRobot/examples/Legacy/ColladaViewer/showRobotWindow.cpp
@@ -439,20 +439,6 @@ void showRobotWindow::jointValueChanged(int pos)
float fPos = currentRobotNodes[nr]->getJointLimitLo() + (float)pos / 1000.0f * (currentRobotNodes[nr]->getJointLimitHi() - currentRobotNodes[nr]->getJointLimitLo());
robot->setJointValue(currentRobotNodes[nr], fPos);
UI.lcdNumberJointValue->display((double)fPos);
-
-#if 0
- RobotNodePtr rnl = robot->getRobotNode("LeftLeg_TCP");
- RobotNodePtr rnr = robot->getRobotNode("RightLeg_TCP");
-
- if (rnl && rnr)
- {
- cout << "LEFT:" << endl;
- MathTools::printMat(rnl->getGlobalPose());
- cout << "RIGHT:" << endl;
- MathTools::printMat(rnr->getGlobalPose());
- }
-
-#endif
}
void showRobotWindow::showCoordSystem()
@@ -660,65 +646,3 @@ void showRobotWindow::robotCoordSystems()
// rebuild visualization
rebuildVisualization();
}
-/*
-void showRobotWindow::closeHand()
-{
- if (currentEEF)
- currentEEF->closeActors();
-}
-
-void showRobotWindow::openHand()
-{
-#if 0
- if (robot)
- {
- float randMult = (float)(1.0/(double)(RAND_MAX));
- std::vector<RobotNodePtr> rn = robot->getRobotNodes();
- std::vector<RobotNodePtr> rnJoints;
- for (size_t j=0;j<rn.size();j++)
- {
- if (rn[j]->isRotationalJoint())
- rnJoints.push_back(rn[j]);
- }
- int loops = 10000;
- clock_t startT = clock();
- for (int i=0;i<loops;i++)
- {
- std::vector<float> jv;
- for (size_t j=0;j<rnJoints.size();j++)
- {
- float t = (float)rand() * randMult; // value from 0 to 1
- t = rnJoints[j]->getJointLimitLo() + (rnJoints[j]->getJointLimitHi() - rnJoints[j]->getJointLimitLo())*t;
- jv.push_back(t);
- }
- robot->setJointValues(rnJoints,jv);
- }
- clock_t endT = clock();
-
- float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
- cout << "RobotNodes:" << rn.size() << endl;
- cout << "Joints:" << rnJoints.size() << endl;
- cout << "loops:" << loops << ". time (ms):" << diffClock << ". Per loop:" << diffClock/(float)loops << endl;
- }
-#endif
- if (currentEEF)
- currentEEF->openActors();
-}
-
-void showRobotWindow::selectEEF( int nr )
-{
- cout << "Selecting EEF nr " << nr << endl;
- if (nr<0 || nr>=(int)eefs.size())
- return;
- currentEEF = eefs[nr];
-}
-
-void showRobotWindow::updateEEFBox()
-{
- UI.comboBoxEndEffector->clear();
-
- for (unsigned int i=0;i<eefs.size();i++)
- {
- UI.comboBoxEndEffector->addItem(QString(eefs[i]->getName().c_str()));
- }
-}*/
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
index f6cf5e01b..dea185b4c 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
@@ -318,154 +318,6 @@ void showRobotWindow::exportVRML()
return;
}
-#if 0
- resetSceneryAll();
-
- std::string t1("LegL_Hip1_joint");
- std::string t2("LegR_Hip1_joint");
-
- Eigen::Matrix4f m1 = robot->getRobotNode(t1)->getGlobalPose();
- Eigen::Matrix4f m2 = robot->getRobotNode(t2)->getGlobalPose();
- std::cout << "global pose " << t1 << ":" << endl << m1 << std::endl;
- std::cout << "global pose " << t2 << ":" << endl << m2 << std::endl;
-
- Eigen::Matrix4f parentM1 = robot->getRobotNode(t1)->getParent()->getGlobalPose();
- Eigen::Matrix4f parentM2 = robot->getRobotNode(t2)->getParent()->getGlobalPose();
- std::cout << "global pose parent " << t1 << ":" << endl << parentM1 << std::endl;
- std::cout << "global pose parent " << t2 << ":" << endl << parentM2 << std::endl;
-
- Eigen::Matrix4f localM1 = robot->getRobotNode(t1)->getLocalTransformation();
- Eigen::Matrix4f localM2 = robot->getRobotNode(t2)->getLocalTransformation();
- std::cout << "local trafo " << t1 << ":" << endl << localM1 << std::endl;
- std::cout << "local trafo " << t2 << ":" << endl << localM2 << std::endl;
-
-
-
- Eigen::Matrix4f tmpRotMat1 = Eigen::Matrix4f::Identity();
- Eigen::Matrix4f tmpRotMat2 = Eigen::Matrix4f::Identity();
- Eigen::Vector3f rot1;
- rot1 << 0, 0, 1;
- Eigen::Vector3f rot2;
- rot2 << 0, 0, 1;
- tmpRotMat1.block(0, 0, 3, 3) = Eigen::AngleAxisf(0.0f, rot1).matrix();
- tmpRotMat2.block(0, 0, 3, 3) = Eigen::AngleAxisf(0.0f, rot2).matrix();
-
- m1 = parentM1 * localM1 /*getLocalTransformation()*/ * tmpRotMat1;
- m2 = parentM2 * localM2 /*getLocalTransformation()*/ * tmpRotMat2;
- std::cout << "rot mat " << t1 << ":" << endl << tmpRotMat1 << std::endl;
- std::cout << "rot mat " << t2 << ":" << endl << tmpRotMat2 << std::endl;
-
- std::cout << "gp custom " << t1 << ":" << endl << m1 << std::endl;
- std::cout << "gp custom " << t2 << ":" << endl << m2 << std::endl;
-
-
-
-
-
- /*std::string root1("Root_joint");
- std::string root2("RootRotated");
- Eigen::Matrix4f gpr1 = robot->getRobotNode(root1)->getGlobalPose();
- Eigen::Matrix4f gpr2 = robot->getRobotNode(root2)->getGlobalPose();
- */
-
-
- std::string knee1("LegL_Knee_joint");
- std::string knee2("LegR_Knee_joint");
- Eigen::Matrix4f gpr1 = robot->getRobotNode(knee1)->getGlobalPose();
- Eigen::Matrix4f gpr2 = robot->getRobotNode(knee2)->getGlobalPose();
-
- std::cout << "gp " << knee1 << ":" << endl << gpr1 << std::endl;
- std::cout << "gp " << knee2 << ":" << endl << gpr2 << std::endl;
- std::cout << "gp knee1->knee2 :" << endl << robot->getRobotNode(knee1)->toLocalCoordinateSystem(robot->getRobotNode(knee2)->getGlobalPose()) << std::endl;
-
-
-
-
-
-
- std::string n1("LegR_Ank2_joint");
- std::string n2("LegL_Ank2_joint");
- RobotNodePtr start1 = robot->getRobotNode(n1);
- RobotNodePtr tcp1 = robot->getRobotNode(n2);
-
- m1 = start1->toLocalCoordinateSystem(tcp1->getGlobalPose());
- std::cout << "trafo (" << n1 << " -> " << n2 << "):" << endl << m1 << std::endl;
-
-
- return;
-#endif
-
-#if 0
- resetSceneryAll();
-
- RobotNodePtr start1 = robot->getRobotNode("Shoulder 2 L");
- RobotNodePtr tcp1 = robot->getRobotNode("Wrist 1 L");
-
- Eigen::Matrix4f m1 = start1->toLocalCoordinateSystem(tcp1->getGlobalPose());
- std::cout << "OLD trafo (FW):" << endl << m1 << std::endl;
-
- /*
- RobotFactory::robotStructureDef newStructure;
- newStructure.rootName = "TCP L";
-
- RobotFactory::robotNodeDef rn1;
- rn1.name = "TCP L";
- rn1.children.push_back("Wrist 2 L");
- rn1.invertTransformation = true;
- newStructure.parentChildMapping.push_back(rn1);
-
- RobotFactory::robotNodeDef rn2;
- rn2.name = "Wrist 2 L";
- rn2.children.push_back("Wrist 1 L");
- rn2.invertTransformation = true;
- newStructure.parentChildMapping.push_back(rn2);
-
- RobotFactory::robotNodeDef rn3;
- rn3.name = "Wrist 1 L";
- rn3.children.push_back("Underarm L");
- rn3.invertTransformation = true;
- newStructure.parentChildMapping.push_back(rn3);
-
- RobotFactory::robotNodeDef rn4;
- rn4.name = "Underarm L";
- rn4.children.push_back("Elbow L");
- rn4.invertTransformation = true;
- newStructure.parentChildMapping.push_back(rn4);
-
- RobotFactory::robotNodeDef rn5;
- rn5.name = "Elbow L";
- rn5.children.push_back("Upperarm L");
- rn5.invertTransformation = true;
- newStructure.parentChildMapping.push_back(rn5);
-
- RobotFactory::robotNodeDef rn6;
- rn6.name = "Upperarm L";
- rn6.children.push_back("Shoulder 2 L");
- rn6.invertTransformation = true;
- newStructure.parentChildMapping.push_back(rn6);
-
- RobotFactory::robotNodeDef rn7;
- rn7.name = "Shoulder 2 L";
- rn7.children.push_back("Shoulder 1 L");
- rn7.invertTransformation = true;
- newStructure.parentChildMapping.push_back(rn7);
-
- robot = RobotFactory::cloneChangeStructure(robot, newStructure);
- */
- robot = RobotFactory::cloneChangeStructure(robot, "Shoulder 1 L", "TCP L");
-
- updatRobotInfo();
-
- RobotNodePtr start2 = robot->getRobotNode("Shoulder 2 L");
- RobotNodePtr tcp2 = robot->getRobotNode("Wrist 1 L");
-
- Eigen::Matrix4f m2 = start2->toLocalCoordinateSystem(tcp2->getGlobalPose());
- std::cout << "NEW trafo (INV):" << endl << m2 << std::endl;
-
- return;
-
-#endif
-
QString fi = QFileDialog::getSaveFileName(this, tr("VRML 2.0 File"), QString(), tr("VRML Files (*.wrl)"));
std::string s = std::string(fi.toLatin1());
@@ -725,19 +577,6 @@ void showRobotWindow::jointValueChanged(int pos)
robot->setJointValue(currentRobotNodes[nr], fPos);
UI.lcdNumberJointValue->display((double)fPos);
-#if 0
- RobotNodePtr rnl = robot->getRobotNode("LeftLeg_TCP");
- RobotNodePtr rnr = robot->getRobotNode("RightLeg_TCP");
-
- if (rnl && rnr)
- {
- std::cout << "LEFT:" << std::endl;
- MathTools::printMat(rnl->getGlobalPose());
- std::cout << "RIGHT:" << std::endl;
- MathTools::printMat(rnr->getGlobalPose());
- }
-
-#endif
updatePointDistanceVisu();
}
@@ -924,42 +763,6 @@ void showRobotWindow::loadRobot()
return;
}
- // just a simple test that inverts the kinematic structure of the robot
-#if 0
- if (robot->hasRobotNode("Index L J1"))
- {
- robot = RobotFactory::cloneInversed(robot, "Index L J1");
- }
-#endif
-
-#if 0
- if (robot->hasRobotNodeSet("LeftArm"))
- {
- robot = RobotFactory::cloneSubSet(robot, robot->getRobotNodeSet("LeftArm"), "LeftArmRobot");
- }
-#endif
-
-#if 0
- if (robot->hasRobotNode("Wrist 2 L") && robot->hasRobotNode("Wrist 2 R"))
- {
- std::vector<std::string> l;
- l.push_back("Wrist 2 L");
- l.push_back("Wrist 2 R");
- robot = RobotFactory::cloneUniteSubsets(robot, "RobotWithUnitedHands", l);
- }
- VR_INFO << "=========== PERFORMANCE orig ============" << std::endl;
- testPerformance(robot, robot->getRobotNodeSet("Joints_Revolute"));
- if (robot->hasRobotNode("LWy_joint") && robot->hasRobotNode("RWy_joint"))
- {
- std::vector<std::string> l;
- l.push_back("LWy_joint");
- l.push_back("RWy_joint");
- robot = RobotFactory::cloneUniteSubsets(robot, "RobotWithUnitedHands", l);
- }
- VR_INFO << "=========== PERFORMANCE clone ============" << std::endl;
- testPerformance(robot, robot->getRobotNodeSet("Joints_Revolute"));// ("BPy_joint"));
-#endif
-
updatRobotInfo();
}
@@ -1006,27 +809,6 @@ void showRobotWindow::updatRobotInfo()
displayTriangles();
-#if 0
- RobotPtr r2 = robot->clone(robot->getName(), robot->getCollisionChecker(), 2.0f);
- Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
- gp(0, 3) += 1000.0f;
-
- r2->setGlobalPose(gp);
- extraSep->removeAllChildren();
- useColModel = UI.checkBoxColModel->checkState() == Qt::Checked;
- SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked()) ? SceneObject::Collision : SceneObject::Full;
-
- VirtualRobot::CoinVisualizationPtr visualization = r2->getVisualization<CoinVisualization>(colModel);
- SoNode* visualisationNode = NULL;
-
- if (visualization)
- {
- visualisationNode = visualization->getCoinVisualization();
- }
-
- extraSep->addChild(visualisationNode);
-#endif
-
// build visualization
rebuildVisualization();
robotStructure();
@@ -1070,48 +852,6 @@ void showRobotWindow::closeHand()
void showRobotWindow::openHand()
{
-#if 0
-
- if (robot)
- {
- std::vector<RobotNodePtr> rn = robot->getRobotNodes();
- std::vector<RobotNodePtr> rnJoints;
-
- for (size_t j = 0; j < rn.size(); j++)
- {
- if (rn[j]->isRotationalJoint())
- {
- rnJoints.push_back(rn[j]);
- }
- }
-
- int loops = 10000;
- clock_t startT = clock();
-
- for (int i = 0; i < loops; i++)
- {
- std::vector<float> jv;
-
- for (size_t j = 0; j < rnJoints.size(); j++)
- {
- float t = RandomFloat(); // value from 0 to 1
- t = rnJoints[j]->getJointLimitLo() + (rnJoints[j]->getJointLimitHi() - rnJoints[j]->getJointLimitLo()) * t;
- jv.push_back(t);
- }
-
- robot->setJointValues(rnJoints, jv);
- }
-
- clock_t endT = clock();
-
- float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
- std::cout << "RobotNodes:" << rn.size() << std::endl;
- std::cout << "Joints:" << rnJoints.size() << std::endl;
- std::cout << "loops:" << loops << ". time (ms):" << diffClock << ". Per loop:" << diffClock / (float)loops << std::endl;
- }
-
-#endif
-
if (currentEEF)
{
currentEEF->openActors();
diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
index 594933b7c..ed0b8b445 100644
--- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
@@ -494,13 +494,6 @@ void reachabilityWindow::extendReach()
int steps = UI.spinBoxExtend->value();
-#if 0
- ManipulabilityPtr manipSpace = boost::dynamic_pointer_cast<Manipulability>(reachSpace);
- if (manipSpace && manipSpace->getManipulabilityMeasure())
- {
- manipSpace->getManipulabilityMeasure()->setVerbose(true);
- }
-#endif
//reachSpace->addRandomTCPPoses(steps, 1, true);
reachSpace->addRandomTCPPoses(steps, QThread::idealThreadCount() < 1 ? 1 : QThread::idealThreadCount(), true);
--
GitLab