diff --git a/CMakeModules/SimoxMacros.cmake b/CMakeModules/SimoxMacros.cmake
index c5a1d42aab55f189f375e9839f435a3696b004a2..6833235eb666d33936b783b5d7c71fc86bb5dd87 100644
--- a/CMakeModules/SimoxMacros.cmake
+++ b/CMakeModules/SimoxMacros.cmake
@@ -54,15 +54,34 @@ function(SimoxQtLibrary name srcs incs mocFiles uiFiles)
TARGET_LINK_LIBRARIES(${name} PUBLIC GraspStudio Saba)
endfunction()
-macro(simox_subdirs result curdir)
- file(GLOB children ${curdir}/*)
- set(${result})
- foreach(child ${children})
- if(IS_DIRECTORY ${child})
- list(APPEND ${result} ${child})
+function(simox_subdirs RESULT DIRECTORY)
+ file(GLOB CHILDREN ${DIRECTORY}/*)
+ # message("## children of ${DIRECTORY}: ${CHILDREN}")
+
+ set(DIR_CHILDREN "")
+ foreach(CHILD ${CHILDREN})
+ if (IS_DIRECTORY "${CHILD}")
+ list(APPEND DIR_CHILDREN ${CHILD})
endif()
endforeach()
-endmacro()
+
+ set(${RESULT} "${DIR_CHILDREN}" PARENT_SCOPE)
+endfunction()
+
+
+function(simox_subdirs_recursive RESULT DIRECTORY)
+ simox_subdirs(TOP_CHILDREN "${DIRECTORY}")
+ set(ALL_CHILDREN ${TOP_CHILDREN})
+
+ foreach(TOP_CHILD ${TOP_CHILDREN})
+ simox_subdirs_recursive(REC_CHILDREN ${TOP_CHILD})
+ list(APPEND ALL_CHILDREN ${REC_CHILDREN})
+ endforeach()
+
+ set(${RESULT} "${ALL_CHILDREN}" PARENT_SCOPE)
+
+endfunction()
+
macro(simox_update_file file content) #macro since we want to provide simox_file_up_to_date
set(simox_file_up_to_date 0)
diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index 92b02612dd327bc9b80a09f5ec166e047139ccf4..d732c36477545bf42585ca586c074e9898f75058 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -124,22 +124,23 @@ namespace simox::geometric_planning
.toRotationMatrix());
// ARMARX_DEBUG << VAROUT(joint_T_joint_ref.linear());
- const Pose global_T_joint =
- global_T_joint_orig *
- joint_T_joint_ref; // * Pose(Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), revoluteJoint->getJointRotationAxis()).toRotationMatrix());
+ const Pose global_T_joint_ref = global_T_joint_orig * joint_T_joint_ref;
+ // * Pose(Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), revoluteJoint->getJointRotationAxis()).toRotationMatrix());
const Pose global_T_node(node->getGlobalPose());
// this is the node within the joint frame
- const Pose joint_T_node = global_T_joint.inverse() * global_T_node;
+ // joint_ref_T_node = joint_ref_T_global * global_T_node
+ const Pose joint_ref_T_node = global_T_joint_ref.inverse() * global_T_node;
+
// ARMARX_DEBUG << "relative position: " << joint_T_node.translation();
- // the plane in which the node moves is given by the following
+ // the plane in which the node moves is given by the following (x, y coordinates = 0 for movement plane)
const Pose global_T_joint_plane =
- global_T_joint * Eigen::Translation3f{0, 0, joint_T_node.translation().z()};
+ global_T_joint_ref * Eigen::Translation3f{0, 0, joint_ref_T_node.translation().z()};
// and the radius of the movement is given by the xy coordinate
- const float radius = joint_T_node.translation().head<2>().norm();
+ const float radius = joint_ref_T_node.translation().head<2>().norm();
// ARMARX_DEBUG << "Radius: " << radius;
REQUIRE_MESSAGE(joint->getJointLimitHigh() > joint->getJointLimitLow(),
@@ -158,7 +159,8 @@ namespace simox::geometric_planning
const std::string nodeJointReference = node->getName() + "_joint_reference";
- const Pose joint_T_joint_plane = global_T_joint.inverse() * global_T_joint_plane;
+ // joint_T_joint_plane = joint_ref_T_global * global_t_joint_plane
+ const Pose joint_ref_T_joint_plane = global_T_joint_ref.inverse() * global_T_joint_plane;
// now we make sure that the joint coordinate system is oriented such that the x axis points towards the initial node position
const Eigen::Vector3f global__joint_plane_P_node_initial =
@@ -178,9 +180,9 @@ namespace simox::geometric_planning
const Pose global_T_root(subpart->getGlobalPose());
const Pose root_T_global = global_T_root.inverse();
- const Pose root_T_joint = root_T_global * global_T_joint;
+ const Pose root_T_joint_ref = root_T_global * global_T_joint_ref;
- const Pose root_T_joint_reference(root_T_joint * joint_T_joint_plane *
+ const Pose root_T_joint_reference(root_T_joint_ref * joint_ref_T_joint_plane *
joint_plane_T_joint_reference);
const auto jointReferenceNode = std::make_shared<VirtualRobot::RobotNodeFixed>(
@@ -197,10 +199,18 @@ namespace simox::geometric_planning
// reset joint state
joint->setJointValue(initialJointValue);
+ // we need the transformation from root_T_joint_reference to node, so that the orientations match up.
+ // without the orientation adaption, this will output a coordinate system with x pointing in the
+ // direction from global_T_plane to node, and z pointing upward.
+ // therefore:
+ // global_T_root * root_T_joint_reference = global_T_joint_reference
+ // we just need the linear transformation though (i think), because the translation was already covered before
+ Pose joint_reference_T_node(
+ ((global_T_root * root_T_joint_reference).inverse() * global_T_node).linear());
return {jointReferenceNode,
std::make_unique<CircleSegment>(radius, parameterRange),
- Pose::Identity()};
+ joint_reference_T_node};
}
ParametricPath
@@ -266,11 +276,16 @@ namespace simox::geometric_planning
subpart->registerRobotNode(jointReferenceNode);
CHECK(jointReferenceNode->initialize(subpart->getRootNode()));
+
// ARMARX_DEBUG << "registered robot node `" << jointReferenceNode->getName() << "`";
+ const Eigen::Isometry3f global_T_node(node->getGlobalPose());
+ const Eigen::Isometry3f global_T_jointReference(jointReferenceNode->getGlobalPose());
+
+ const Eigen::Isometry3f jointReference_T_node = global_T_jointReference.inverse() * global_T_node;
+ // jointRef_T_node = jointRef_T_global * global_T_node
- return ParametricPath(jointReferenceNode,
- std::make_unique<Line>(parameterRange),
- Pose::Identity());
+ return ParametricPath(
+ jointReferenceNode, std::make_unique<Line>(parameterRange), jointReference_T_node);
}
ParametricPath
diff --git a/GeometricPlanning/CMakeLists.txt b/GeometricPlanning/CMakeLists.txt
index 77d85645ef611b144f628c8fd8649fcbbb9b27dc..fe29714155848bea33a1112148a7b8c007d2f224 100644
--- a/GeometricPlanning/CMakeLists.txt
+++ b/GeometricPlanning/CMakeLists.txt
@@ -116,9 +116,10 @@ endif()
if(BUILD_TESTING)
# include unit tests
- # ADD_SUBDIRECTORY(tests/)
+ ADD_SUBDIRECTORY(tests)
endif()
+
#######################################################################################
############################ Setup for installation ###################################
#######################################################################################
diff --git a/GeometricPlanning/tests/AOGPHelper.cpp b/GeometricPlanning/tests/AOGPHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5c92dc75cbd30b585c21db9f4c33634e28c446e1
--- /dev/null
+++ b/GeometricPlanning/tests/AOGPHelper.cpp
@@ -0,0 +1,176 @@
+
+#include <filesystem>
+
+#include "VirtualRobot/XML/RobotIO.h"
+#define BOOST_TEST_MODULE GeometricPlanning_AOGPHelper
+
+#include <boost/test/unit_test.hpp>
+
+#include "../ArticulatedObjectGeometricPlanningHelper.h"
+
+BOOST_AUTO_TEST_SUITE(ArticulatedObjectGeometricPlanningHelper)
+
+static constexpr const char* scDataDir =
+ "/common/homes/students/hoefer/workspace/base/simox-control/data/";
+
+static constexpr float precision = 0.00001;
+static constexpr size_t nSamples = 50;
+
+static std::filesystem::path
+getPath(const std::string& model)
+{
+ return std::filesystem::path(scDataDir) / "Model" / "mobile_kitchen" / model /
+ (model + "_articulated.xml");
+}
+
+VirtualRobot::RobotPtr
+load_robot(const std::string& model)
+{
+ // TODO(Hawo): somehow input a simple robot model
+ const auto path = getPath(model);
+ BOOST_REQUIRE(std::filesystem::exists(path));
+
+ auto robot = VirtualRobot::RobotIO::loadRobot(path);
+ BOOST_REQUIRE(robot != nullptr);
+
+ return robot;
+}
+
+using AOGPHelper = simox::geometric_planning::ArticulatedObjectGeometricPlanningHelper;
+
+static void
+printMat(const Eigen::MatrixXf& mat)
+{
+ for (int row = 0; row < mat.rows(); ++row)
+ {
+ for (int col = 0; col < mat.cols(); ++col)
+ {
+ float val = mat(row, col);
+ val = std::fabs(val) < precision ? 0 : val;
+ std::cout << val << ",";
+ }
+ std::cout << "\n";
+ }
+}
+BOOST_AUTO_TEST_CASE(AOGPHelper_circle_first_path_element)
+{
+ const std::string articulatedObjectName = "mobile-fridge";
+ const std::string handleName = "Fridge_handle";
+ auto robot = load_robot(articulatedObjectName);
+
+ auto helper = AOGPHelper(robot);
+ auto path = helper.getPathForNode(handleName);
+ auto range = path.parameterRange();
+
+ Eigen::Matrix4f pathPose = path.getPose(range.min).matrix();
+ Eigen::Matrix4f objectPose = robot->getRobotNode(handleName)->getGlobalPose();
+
+ Eigen::Matrix4f diff = objectPose.inverse() * pathPose - Eigen::Matrix4f::Identity();
+ float sse = diff.array().square().sum();
+ if (sse > precision)
+ {
+ std::cout << "Object Pose (target):\n";
+ std::cout << objectPose << "\n";
+ std::cout << "Path Pose:\n";
+ std::cout << pathPose << "\n";
+ printMat(diff);
+ }
+ BOOST_REQUIRE(sse < precision);
+}
+
+
+BOOST_AUTO_TEST_CASE(AOGPHelper_circle_path_whole_around_z)
+{
+ const std::string articulatedObjectName = "mobile-fridge";
+ const std::string handleName = "Fridge_handle";
+ const std::string jointName = "Fridge_joint";
+ auto robot = load_robot(articulatedObjectName);
+
+ auto helper = AOGPHelper(robot);
+ auto path = helper.getPathForNode(handleName);
+ auto range = path.parameterRange();
+
+
+ for (size_t i = 0; i < nSamples; ++i)
+ {
+ float t = range.min + (range.max - range.min) / nSamples * i;
+ Eigen::Matrix4f pathPose = path.getPose(t).matrix();
+
+ robot->setJointValue(jointName, t);
+ Eigen::Matrix4f objectPose = robot->getRobotNode(handleName)->getGlobalPose();
+
+ Eigen::Matrix4f diff = objectPose.inverse() * pathPose - Eigen::Matrix4f::Identity();
+ float sse = diff.array().square().sum();
+ if (sse > precision)
+ {
+ std::cout << "SSE at " << t << ": " << sse << "\n";
+ printMat(diff);
+ }
+ BOOST_REQUIRE(sse < precision);
+ }
+}
+
+BOOST_AUTO_TEST_CASE(AOGPHelper_circle_path_whole_around_y)
+{
+ const std::string articulatedObjectName = "mobile-dishwasher";
+ const std::string handleName = "Dishwasher->Dishwasher_handle";
+ const std::string jointName = "Dishwasher_joint";
+ auto robot = load_robot(articulatedObjectName);
+
+ auto helper = AOGPHelper(robot);
+ auto path = helper.getPathForNode(handleName);
+ auto range = path.parameterRange();
+
+
+ for (size_t i = 0; i < nSamples; ++i)
+ {
+ float t = range.min + (range.max - range.min) / nSamples * i;
+ Eigen::Matrix4f pathPose = path.getPose(t).matrix();
+
+ robot->setJointValue(jointName, t);
+ Eigen::Matrix4f objectPose = robot->getRobotNode(handleName)->getGlobalPose();
+
+ Eigen::Matrix4f diff = objectPose.inverse() * pathPose - Eigen::Matrix4f::Identity();
+ float sse = diff.array().square().sum();
+ if (sse > precision)
+ {
+ std::cout << "SSE at " << t << ": " << sse << "\n";
+ printMat(diff);
+ }
+ BOOST_REQUIRE(sse < precision);
+ }
+}
+
+BOOST_AUTO_TEST_CASE(AOGPHelper_linear_path_whole)
+{
+ const std::string articulatedObjectName = "mobile-kitchen-counter";
+ const std::string handleName = "DrawerMiddle_handle";
+ const std::string jointName = "DrawerMiddle_joint";
+ auto robot = load_robot(articulatedObjectName);
+
+ auto helper = AOGPHelper(robot);
+ auto path = helper.getPathForNode(handleName);
+ auto range = path.parameterRange();
+
+
+ for (size_t i = 0; i < nSamples; ++i)
+ {
+ float t = range.min + (range.max - range.min) / nSamples * i;
+ Eigen::Matrix4f pathPose = path.getPose(t).matrix();
+
+ robot->setJointValue(jointName, t);
+ Eigen::Matrix4f objectPose = robot->getRobotNode(handleName)->getGlobalPose();
+
+ Eigen::Matrix4f diff = objectPose.inverse() * pathPose - Eigen::Matrix4f::Identity();
+ float sse = diff.array().square().sum();
+ if (sse > precision)
+ {
+ std::cout << "SSE at " << t << ": " << sse << "\n";
+ printMat(diff);
+ }
+ BOOST_REQUIRE(sse < precision);
+ }
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/GeometricPlanning/tests/CMakeLists.txt b/GeometricPlanning/tests/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..59f9b89478a8eabe52426b20ca55ae9ebe6505a1
--- /dev/null
+++ b/GeometricPlanning/tests/CMakeLists.txt
@@ -0,0 +1,3 @@
+# FIXME the following test must be adapted. It requires a model that is not available in simox
+
+# ADD_GEOMETRIC_PLANNING_TEST(AOGPHelper)
diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.h b/GraspPlanning/ApproachMovementSurfaceNormal.h
index bc976fdf72a87ea07fb5071661b0f127d28af924..17dc6279f57cdf3b53ce9317310a54fbe5542305 100644
--- a/GraspPlanning/ApproachMovementSurfaceNormal.h
+++ b/GraspPlanning/ApproachMovementSurfaceNormal.h
@@ -55,10 +55,10 @@ namespace GraspStudio
* \param object The object.
* \param eef The end effector.
* \param graspPreshape An optional preshape that can be used in order to "open" the eef.
- * \param maxRandDist
+ * \param maxRetreatDist
* If >0, the resulting apporach pose is randomly moved in the approach direction
* (away from the object) in order to create different distances to the object.
- * \param usefaceAreaDistribution
+ * \param useFaceAreaDistribution
* If true, the probability of a face being selected is proportional to its area.
* If false, all faces are selected with equal probability.
*/
diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
index 0903780fe0eb6ec0aec7f7b27465b9dcda7ce41d..7be6aa603fef73249530d095d827d87eecaa1c5f 100644
--- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
@@ -62,7 +62,8 @@ namespace GraspStudio
/*!
Creates new grasps.
\param nrGrasps The number of grasps to be planned.
- \param timeOutMS The time out in milliseconds. Planning is stopped when this time is exceeded. Disabled when zero.
+ \param timeOutDuration The time out in milliseconds. Planning is stopped when this time is exceeded. Disabled when zero.
+ \param obstacles
\return Number of generated grasps.
*/
int plan(int nrGrasps, int timeOutDuration = 0, VirtualRobot::SceneObjectSetPtr obstacles = {}) override;
diff --git a/GraspPlanning/GraspPlanner/GraspPlanner.h b/GraspPlanning/GraspPlanner/GraspPlanner.h
index b38cc46520ac099f282c4d56a46f6db0e7d2bb46..5b7feb70257860c75d04fa6149407631f7d59b82 100644
--- a/GraspPlanning/GraspPlanner/GraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GraspPlanner.h
@@ -56,6 +56,7 @@ namespace GraspStudio
Creates new grasps.
\param nrGrasps The number of grasps to be planned.
\param timeOutMS The time out in milliseconds. Planning is stopped when this time is exceeded. Disabled when zero.
+ \param obstacles
\return Number of planned grasps.
*/
virtual int plan(int nrGrasps, int timeOutMS = 0, VirtualRobot::SceneObjectSetPtr obstacles = {}) = 0;
diff --git a/MotionPlanning/Planner/BiRrt.h b/MotionPlanning/Planner/BiRrt.h
index bbaaea6829e505db01c263edb24d9b976d185ce4..5d846881915a86c3c374403df29cd26d2c56c4af 100644
--- a/MotionPlanning/Planner/BiRrt.h
+++ b/MotionPlanning/Planner/BiRrt.h
@@ -46,6 +46,7 @@ namespace Saba
\param cspace An initialized cspace object.
\param modeA Specify the RRT method that should be used to build the first tree
\param modeB Specify the RRT method that should be used to build the second tree
+ \param samplingSize
*/
BiRrt(CSpacePtr cspace, RrtMethod modeA = eConnect, RrtMethod modeB = eConnect, float samplingSize = -1);
~BiRrt() override;
diff --git a/MotionPlanning/Planner/Rrt.h b/MotionPlanning/Planner/Rrt.h
index 953e1e340d27ddbf74453b9c9d5e649a8158c3ea..94aae148b75cee2adef2a6d3988cf737d0d4b4f8 100644
--- a/MotionPlanning/Planner/Rrt.h
+++ b/MotionPlanning/Planner/Rrt.h
@@ -68,6 +68,7 @@ namespace Saba
\param cspace A cspace, defining the used joints (the dimension/DoF) and the collision detection setup.
\param mode Specify the RRT method that should be used
\param probabilityExtendToGoal Specify how often the goal node should be used instead of a random config (value must be between 0 and 1)
+ \param samplingSize
*/
Rrt(CSpacePtr cspace, RrtMethod mode = eConnect, float probabilityExtendToGoal = 0.1f, float samplingSize = -1);
~Rrt() override;
diff --git a/SimoxUtility/math/SoftMinMax.cpp b/SimoxUtility/math/SoftMinMax.cpp
index 622c94ff3e1fbd7c32a78bd2c5b6dbf060a4e8c6..13272a937ee096217f7e87e5b7c1109dc4965a54 100644
--- a/SimoxUtility/math/SoftMinMax.cpp
+++ b/SimoxUtility/math/SoftMinMax.cpp
@@ -13,29 +13,29 @@ namespace simox::math
reset(0, 1);
}
- SoftMinMax::SoftMinMax(float percentile, std::size_t numValues)
+ SoftMinMax::SoftMinMax(float quantile, std::size_t numValues)
{
- reset(percentile, numValues);
+ reset(quantile, numValues);
}
- void SoftMinMax::reset(float percentile, std::size_t numValues)
+ void SoftMinMax::reset(float quantile, std::size_t numValues)
{
minQueue = MinQueue();
maxQueue = MaxQueue();
- if (percentile < 0 || percentile > 0.5f)
+ if (quantile < 0 || quantile > 0.5f)
{
std::stringstream msg;
- msg << "percentile must be in [0, 0.5], but was " << percentile << ".";
+ msg << "The quantile must be in [0, 0.5], but was " << quantile << ".";
throw std::invalid_argument(msg.str());
}
if (numValues == 0)
{
std::stringstream msg;
- msg << "numValues must be > 0, but was " << numValues;
+ msg << "The numValues must be > 0, but was " << numValues;
throw std::invalid_argument(msg.str());
}
- this->percentile = percentile;
+ this->quantile = quantile;
this->num_elements = numValues;
allowed_heap_size_cache = allowedHeapSize();
@@ -99,7 +99,7 @@ namespace simox::math
std::size_t SoftMinMax::numOutsideSoftMinMax() const
{
- return size_t(std::ceil(percentile * num_elements));
+ return size_t(std::ceil(quantile * num_elements));
}
std::size_t SoftMinMax::allowedHeapSize() const
diff --git a/SimoxUtility/math/SoftMinMax.h b/SimoxUtility/math/SoftMinMax.h
index 97122d50e3426e8f431baada71a4c68e76fc7943..c19097ec516c382dbe893816cdb7bdb8a4656de6 100644
--- a/SimoxUtility/math/SoftMinMax.h
+++ b/SimoxUtility/math/SoftMinMax.h
@@ -11,7 +11,7 @@ namespace simox::math
*
* Soft means that some values are allowed to be >= soft min / <= soft max
* (including the soft min/max), respectively.
- * A percentile argument in [0..0.5] specifies the percentage of values
+ * A quantile parameter in [0..0.5] specifies the proportion of values
* which are allowed to excess the soft min/max.
*/
class SoftMinMax
@@ -22,16 +22,16 @@ namespace simox::math
SoftMinMax();
/**
- * @brief Constructs a SoftMinMax for given percentile and number of values.
+ * @brief Constructs a SoftMinMax for given quantile and number of values.
*
- * @param percentile
- * The percentage of values that may excess the soft min/max. Must be in [0..0.5].
+ * @param quantile
+ * The proportion of values that may excess the soft min/max. Must be in [0..0.5].
* @param numValues the total number of values that will be added. Must be > 0.
*
* @throws `std::invalid_argument`
* If one of the parameters value does not meet the requirements
*/
- SoftMinMax(float percentile, std::size_t numValues);
+ SoftMinMax(float quantile, std::size_t numValues);
/**
* Reinitializes the SoftMinMax with the given arguments.
@@ -39,7 +39,7 @@ namespace simox::math
* @throws `std::invalid_argument`
* If one of the parameters value does not meet the requirements.
*/
- void reset(float percentile, std::size_t numValues);
+ void reset(float quantile, std::size_t numValues);
/// Add a value to the considered collection.
/// @note Only values excessing the current soft min/max are stored.
@@ -62,8 +62,8 @@ namespace simox::math
std::size_t allowedHeapSize() const;
- /// The percentile in [0, 0.5].
- float percentile = 0;
+ /// The quantile in [0, 0.5].
+ float quantile = 0;
/// The number of elements to be added.
std::size_t num_elements = 0;
diff --git a/VirtualRobot/Manipulability/AbstractManipulability.cpp b/VirtualRobot/Manipulability/AbstractManipulability.cpp
index 8e6725d0e62a1158ec840da3615a4f3382cdbab6..a2a83e05e617a11d3aa664739e1e95cf6a4c000d 100644
--- a/VirtualRobot/Manipulability/AbstractManipulability.cpp
+++ b/VirtualRobot/Manipulability/AbstractManipulability.cpp
@@ -108,26 +108,49 @@ VisualizationNodePtr AbstractManipulability::getManipulabilityVis(const Eigen::M
return vis;
}
-void AbstractManipulability::getEllipsoidOrientationAndScale(const Eigen::MatrixXd &manipulability, Eigen::Quaternionf &orientation, Eigen::Vector3d &scale) {
- Eigen::MatrixXd reduced_manipulability = (mode != Mode::Orientation || manipulability.rows() == 3) ? manipulability.block(0, 0, 3, 3) : manipulability.block(3, 3, 3, 3);
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(reduced_manipulability);
- Eigen::MatrixXd eigenVectors = eigenSolver.eigenvectors();
- Eigen::VectorXd eigenValues = eigenSolver.eigenvalues();
- Eigen::VectorXd v1 = eigenVectors.col(eigenVectors.cols() - 1);
- Eigen::VectorXd v2 = eigenVectors.col(eigenVectors.cols() - 2);
-
- orientation = Eigen::Quaterniond::FromTwoVectors(v1, v2).cast<float>();
+void AbstractManipulability::getEllipsoidOrientationAndScale(const Eigen::MatrixXd& manipulability, Eigen::Quaternionf& orientation, Eigen::Vector3d& scale) {
+
+ constexpr std::size_t POSITION_DIMS = 3;
+ constexpr std::size_t ORIENTATION_DIMS = 3;
+
+ const Eigen::MatrixXd reduced_manipulability =
+ (mode != Mode::Orientation || manipulability.rows() == 3)
+ ? manipulability.block(0, 0, POSITION_DIMS, POSITION_DIMS)
+ : manipulability.block(
+ POSITION_DIMS, POSITION_DIMS, ORIENTATION_DIMS, ORIENTATION_DIMS);
+ const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(reduced_manipulability);
+
+ // Eigenvectors are the columns of the matrix 'eigenVectors'
+ // they are already normalized to have length 1
+ const Eigen::MatrixXd& eigenVectors = eigenSolver.eigenvectors();
+
+ // Eigen values are sorted in increasing order
+ const Eigen::Vector3d& eigenValues = eigenSolver.eigenvalues();
+
+ // We create a ortho-normal basis of the eigen vectors.
+ // Here, we use the eigen vectors with the eigen values in decreasing order.
+ // To ensure a right-handed coordinate system, the third basis vector is computed
+ // by using the cross product.
+ Eigen::Matrix3d rotationMatrix;
+ rotationMatrix.col(0) = eigenVectors.col(2);
+ rotationMatrix.col(1) = eigenVectors.col(1);
+ rotationMatrix.col(2) = rotationMatrix.col(0).cross(rotationMatrix.col(1));
+
+ orientation = rotationMatrix.cast<float>();
+
+ scale = eigenValues.reverse();
// normalize singular values for scaling
- eigenValues /= eigenValues.sum();
- for (int i = 0; i < eigenValues.rows(); i++) {
- if (eigenValues(i) < 0.005) // 5mm
- eigenValues(i) = 0.005;
- }
+ scale /= scale.sum();
+ for (int i = 0; i < eigenValues.rows(); i++)
+ {
+ constexpr double minEigenVal = 0.005; // [mm]
- scale(0) = eigenValues(eigenValues.rows() - 1);
- scale(1) = eigenValues(eigenValues.rows() - 2);
- scale(2) = eigenValues(eigenValues.rows() - 3);
+ if (scale(i) < minEigenVal)
+ {
+ scale(i) = minEigenVal;
+ }
+ }
}
void AbstractManipulability::getEllipsoidOrientationAndScale(Eigen::Quaternionf &orientation, Eigen::Vector3d &scale) {
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index 74624441024d722bb024a4edeff6746fe3932db0..4297106c392480f0daba82bd741ca2a2bab2e73a 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -650,8 +650,8 @@ namespace VirtualRobot
double VIRTUAL_ROBOT_IMPORT_EXPORT getDamping(const Eigen::MatrixXf &matrix);
/*! @brief Calculates the damped least square inverse by J^T * (JJ^T + k^2 I)^{-1}
- * @jacobian Jacobian J
- * @squaredDampingFactor Damping factor k^2
+ * @param jacobian Jacobian J
+ * @param squaredDampingFactor Damping factor k^2
*/
template <typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> VIRTUAL_ROBOT_IMPORT_EXPORT getDampedLeastSquareInverse(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &jacobian, double squaredDampingFactor = 0.01f) {
diff --git a/VirtualRobot/Workspace/NaturalPosture.h b/VirtualRobot/Workspace/NaturalPosture.h
index ca1e335cfea2b593b52850625743428b1c5a8036..69af00341969f453df7969babf228ec95b2b9ae3 100644
--- a/VirtualRobot/Workspace/NaturalPosture.h
+++ b/VirtualRobot/Workspace/NaturalPosture.h
@@ -31,7 +31,8 @@ namespace VirtualRobot
{
- class VIRTUAL_ROBOT_IMPORT_EXPORT NaturalPosture : public WorkspaceRepresentation, public std::enable_shared_from_this<NaturalPosture>
+ class VIRTUAL_ROBOT_IMPORT_EXPORT NaturalPosture :
+ public WorkspaceRepresentation, public std::enable_shared_from_this<NaturalPosture>
{
public:
friend class CoinVisualizationFactory;
diff --git a/VirtualRobot/Workspace/WorkspaceGrid.h b/VirtualRobot/Workspace/WorkspaceGrid.h
index 0b55c26619d7090d95f8de6fef77d7726abeb1be..1d22d9aae3b5d8889db496b9991f1353b97d7a79 100644
--- a/VirtualRobot/Workspace/WorkspaceGrid.h
+++ b/VirtualRobot/Workspace/WorkspaceGrid.h
@@ -34,7 +34,7 @@ namespace VirtualRobot
* A 2D grid which represents a quality distribution (e.g. the reachability) at 2D positions w.r.t. one or multiple grasp(s).
* Internally the inverse workspace data (@see WorkspaceRepresentation), which encodes the
* transformation between robot's base and grasping position, is used.
- * This data is useful to quickly sample positions from where the probability that a grasp is reachable is high (see \func getRandomPos).
+ * This data is useful to quickly sample positions from where the probability that a grasp is reachable is high (see getRandomPos()).
*/
class VIRTUAL_ROBOT_IMPORT_EXPORT WorkspaceGrid
{
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.h b/VirtualRobot/Workspace/WorkspaceRepresentation.h
index 666aee275ce0294046cdcc52a0c8ef166839a725..de58e980884f79ad5db9197779db1ecae60e0243 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.h
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.h
@@ -273,7 +273,7 @@ namespace VirtualRobot
/*!
Estimate a parameter setup for the given RNS by randomly set configurations and check for achieved workspace extends. The results are slightly scaled.
- \param ndoeSet
+ \param nodeSet
\param steps How many loops should be performed to estimate the result. Chose a value >= 1000.
\param storeMinBounds Workspace extend from min
\param storeMaxBounds Workspace extend to max
@@ -321,6 +321,7 @@ namespace VirtualRobot
* \brief createCut Create a cut at a specific height (assuming z is upwards).
* \param heightPercent Value in [0,1]
* \param cellSize The discretization step size of the result
+ * \param sumAngles¸
* \return
*/
WorkspaceCut2DPtr createCut(float heightPercent, float cellSize, bool sumAngles) const;
diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt
index 75467f330a5e435b3012a5a823ddfee7f0237846..907d79aa830708da3f493f16605456937bf65615 100644
Binary files a/doc/CMakeLists.txt and b/doc/CMakeLists.txt differ