Feature/grasping challenge

source/armarx/navigation/algorithms/astar/AStarPlanner.cpp

@@ -124,8 +124,8 @@ namespace armarx::navigation::algorithm::astar
         ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
         ARMARX_CHECK_NOT_NULL(obstacles);
 
-        const core::Pose globalPose = Eigen::Translation3f(conv::to3D(n->position)) *
-                                      Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
+        const core::Pose globalPose(Eigen::Translation3f(conv::to3D(n->position))); //*
+                                      //Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX());
 
         robotCollisionModel->setGlobalPose(globalPose.matrix());
 
@@ -186,6 +186,7 @@ namespace armarx::navigation::algorithm::astar
         // << "No collision model for robot node " << robotColModelName;
 
         // robotCollisionModel = robot->getRobotNode(robotColModelName)->getCollisionModel()->clone();
+        ARMARX_CHECK_NOT_NULL(robotCollisionModel);
         robotCollisionModel->setGlobalPose(robot->getGlobalPose());
 
         // static auto cylinder = VirtualRobot::ObjectIO::loadObstacle("/home/fabi/cylinder.xml");
@@ -194,7 +195,6 @@ namespace armarx::navigation::algorithm::astar
 
         // robotCollisionModel = cylinder->getCollisionModel();
 
-        ARMARX_CHECK_NOT_NULL(robotCollisionModel);
 
         createUniformGrid();
         ARMARX_DEBUG << "Setting start node for position " << start;