snapshot

VirtualRobot/Workspace/WorkspaceGrid.cpp

@@ -474,13 +474,15 @@ namespace VirtualRobot
         return fillGridData(ws, graspGlobal, g, baseRobotNode, baseOrientation, maxAngle);
     }
 
-    bool WorkspaceGrid::fillGridData(WorkspaceRepresentationPtr ws, const Eigen::Matrix4f &global_T_grasp, GraspPtr g, RobotNodePtr baseRobotNode, float baseOrientation, const float maxAngle)
+    bool WorkspaceGrid::fillGridData(WorkspaceRepresentationPtr ws, const Eigen::Matrix4f &global_T_grasp_orig, GraspPtr g, RobotNodePtr baseRobotNode, float baseOrientation, const float maxAngle)
     {
         if (!ws)
         {
             return false;
         }
 
+        // baseOrientation = 0;
+
         // ensure robot is at identity
         Eigen::Matrix4f gpOrig = Eigen::Matrix4f::Identity();
 
@@ -491,19 +493,24 @@ namespace VirtualRobot
             baseRobotNode->getRobot()->setGlobalPose(Eigen::Matrix4f::Identity());
         }
 
-        const Eigen::AngleAxisf global_R_robot_inv(-baseOrientation, Eigen::Vector3f::UnitZ());
-        const Eigen::AngleAxisf global_R_robot(baseOrientation, Eigen::Vector3f::UnitZ());
+        // const Eigen::AngleAxisf global_R_robot_inv(baseOrientation, Eigen::Vector3f::UnitZ());
+        // const Eigen::AngleAxisf global_R_robot(-baseOrientation, Eigen::Vector3f::UnitZ());
+
+        Eigen::Isometry3f global_T_grasp(global_T_grasp_orig);
+
+        global_T_grasp.linear() = Eigen::AngleAxisf(-baseOrientation, Eigen::Vector3f::UnitZ()) * global_T_grasp.linear();
+
 
-        Eigen::Isometry3f robot_T_grasp(global_T_grasp);
-        robot_T_grasp.linear() = global_R_robot_inv.toRotationMatrix() * robot_T_grasp.linear();
+        // Eigen::Isometry3f robot_T_grasp(global_T_grasp);
+        // robot_T_grasp.linear() = global_R_robot_inv.toRotationMatrix() * robot_T_grasp.linear();
 
-        Eigen::Isometry3f robot_T_grasp2(global_T_grasp);
-        robot_T_grasp2.linear() = global_R_robot.toRotationMatrix() * robot_T_grasp2.linear();
+        // Eigen::Isometry3f robot_T_grasp2(global_T_grasp);
+        // robot_T_grasp2.linear() = global_R_robot.toRotationMatrix() * robot_T_grasp2.linear();
 
-        WorkspaceRepresentation::WorkspaceCut2DPtr cutXY = ws->createCut(robot_T_grasp.matrix(), discretizeSize, false);
+        WorkspaceRepresentation::WorkspaceCut2DPtr cutXY = ws->createCut(global_T_grasp.matrix(), discretizeSize, false);
 
         // VR_INFO << "Reference global pose" << cutXY->referenceGlobalPose;
-        cutXY->referenceGlobalPose = (global_R_robot_inv * Eigen::Isometry3f(global_T_grasp)).matrix();
+        // cutXY->referenceGlobalPose = (global_R_robot * Eigen::Isometry3f(global_T_grasp)).matrix();
 
         std::vector<WorkspaceRepresentation::WorkspaceCut2DTransformationPtr> transformations = ws->createCutTransformations(cutXY, baseRobotNode, M_PI/*, isFlipped*/);
         setEntries(transformations, global_T_grasp.matrix(), g);

VirtualRobot/Workspace/WorkspaceRepresentation.cpp

@@ -1875,10 +1875,10 @@ namespace VirtualRobot
                     float yaw = std::atan2(pose.translation().y(), pose.translation().x()) - M_PI_2f32;
                     yaw = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
 
-                    if (yaw < maxAngle and yaw > -maxAngle)
-                    {
+                    // if (yaw < maxAngle and yaw > -maxAngle)
+                    // {
                         result.push_back(tp);
-                    }
+                    // }
                 }
             }
         }