From ef13f55fe45bdd7e7aa59f5b1afa64b7fb888357 Mon Sep 17 00:00:00 2001
From: Nikolaus Vahrenkamp <vahrenkamp@kit.edu>
Date: Mon, 2 May 2016 21:24:06 +0200
Subject: [PATCH] some minor fixes: * do not add nlopt define when nlopt has
 not been found * avoid some warnings due to wrong floating point and
 signed/unsigned formats

---
 VirtualRobot/CMakeLists.txt                                 | 4 +++-
 VirtualRobot/EndEffector/EndEffector.cpp                    | 5 +++++
 VirtualRobot/IK/ConstrainedIK.cpp                           | 2 +-
 .../IK/constraints/JointLimitAvoidanceConstraint.cpp        | 6 +++---
 VirtualRobot/IK/constraints/PoseConstraint.cpp              | 5 ++++-
 VirtualRobot/IK/constraints/TSRConstraint.cpp               | 2 +-
 6 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 67225e0d4..96ab8a56e 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -21,7 +21,9 @@ endif()
 
 if (Simox_USE_NLOPT)
     FIND_PACKAGE (NLOPT)
-    ADD_DEFINITIONS(-DUSE_NLOPT)
+    if (NLOPT_FOUND)
+        ADD_DEFINITIONS(-DUSE_NLOPT)
+    endif()
 endif()
 
 ########################### TESTING #####################################
diff --git a/VirtualRobot/EndEffector/EndEffector.cpp b/VirtualRobot/EndEffector/EndEffector.cpp
index b524cbedc..d653b9549 100644
--- a/VirtualRobot/EndEffector/EndEffector.cpp
+++ b/VirtualRobot/EndEffector/EndEffector.cpp
@@ -633,6 +633,8 @@ namespace VirtualRobot
         if (!obstacle)
             return 0;
 
+        int contactCount = 0;
+
         for (size_t i = 0; i < statics.size(); i++)
         {
             RobotNodePtr n = statics[i];
@@ -657,9 +659,12 @@ namespace VirtualRobot
                 ci.approachDirectionGlobal = -approachDirGlobal;
 
                 contacts.push_back(ci);
+                contactCount++;
             }
 
         }
+
+        return contactCount;
     }
 
 
diff --git a/VirtualRobot/IK/ConstrainedIK.cpp b/VirtualRobot/IK/ConstrainedIK.cpp
index c64c63929..5dea24738 100644
--- a/VirtualRobot/IK/ConstrainedIK.cpp
+++ b/VirtualRobot/IK/ConstrainedIK.cpp
@@ -198,7 +198,7 @@ void ConstrainedIK::getUnitableNodes(const RobotNodePtr &robotNode, const RobotN
 RobotPtr ConstrainedIK::buildReducedRobot(const RobotPtr &original)
 {
     std::vector<std::string> names;
-    for(int i = 0; i < nodeSet->getSize(); i++)
+    for(size_t i = 0; i < nodeSet->getSize(); i++)
     {
         names.push_back(nodeSet->getNode(i)->getName());
     }
diff --git a/VirtualRobot/IK/constraints/JointLimitAvoidanceConstraint.cpp b/VirtualRobot/IK/constraints/JointLimitAvoidanceConstraint.cpp
index 56465f7b0..fc815bfc5 100644
--- a/VirtualRobot/IK/constraints/JointLimitAvoidanceConstraint.cpp
+++ b/VirtualRobot/IK/constraints/JointLimitAvoidanceConstraint.cpp
@@ -29,7 +29,7 @@ JointLimitAvoidanceConstraint::JointLimitAvoidanceConstraint(const RobotPtr &rob
     Constraint(nodeSet),
     robot(robot),
     nodeSet(nodeSet),
-    factor(0.001)
+    factor(0.001f)
 {
     // Joint limit avoidance is considered a soft constraint
     addOptimizationFunction(0, true);
@@ -41,7 +41,7 @@ double JointLimitAvoidanceConstraint::optimizationFunction(unsigned int id)
 {
     double value = 0;
 
-    for(int i = 0; i < nodeSet->getSize(); i++)
+    for(size_t i = 0; i < nodeSet->getSize(); i++)
     {
         RobotNodePtr node = nodeSet->getNode(i);
         value += factor * node->getJointValue() * node->getJointValue();
@@ -54,7 +54,7 @@ Eigen::VectorXf JointLimitAvoidanceConstraint::optimizationGradient(unsigned int
 {
     Eigen::VectorXf gradient(nodeSet->getSize());
 
-    for(int i = 0; i < nodeSet->getSize(); i++)
+    for(size_t i = 0; i < nodeSet->getSize(); i++)
     {
         gradient(i) = 2 * factor * nodeSet->getNode(i)->getJointValue();
     }
diff --git a/VirtualRobot/IK/constraints/PoseConstraint.cpp b/VirtualRobot/IK/constraints/PoseConstraint.cpp
index 8a23b4e45..59dfde1f9 100644
--- a/VirtualRobot/IK/constraints/PoseConstraint.cpp
+++ b/VirtualRobot/IK/constraints/PoseConstraint.cpp
@@ -47,7 +47,7 @@ PoseConstraint::PoseConstraint(const RobotPtr& robot, const RobotNodeSetPtr& nod
     cartesianSelection(cartesianSelection),
     tolerancePosition(tolerancePosition),
     toleranceRotation(toleranceRotation),
-    posRotTradeoff(0.1)
+    posRotTradeoff(0.1f)
 {
     ik.reset(new DifferentialIK(nodeSet));
     ik->setGoal(target, eef, cartesianSelection, tolerancePosition, toleranceRotation);
@@ -164,6 +164,7 @@ double PoseConstraint::positionOptimizationFunction()
         case IKSolver::CartesianSelection::Orientation:
             return 0;
     }
+    return 0;
 }
 
 Eigen::VectorXf PoseConstraint::positionOptimizationGradient()
@@ -191,6 +192,7 @@ Eigen::VectorXf PoseConstraint::positionOptimizationGradient()
         case IKSolver::CartesianSelection::Orientation:
             return Eigen::VectorXf::Zero(size);
     }
+    return Eigen::VectorXf::Zero(size);
 }
 
 double PoseConstraint::orientationOptimizationFunction()
@@ -240,4 +242,5 @@ Eigen::VectorXf PoseConstraint::orientationOptimizationGradient()
         case IKSolver::CartesianSelection::All:
             return 2 * rpy.transpose() * J.block(3, 0, 3, size);
     }
+    return Eigen::VectorXf::Zero(size);
 }
diff --git a/VirtualRobot/IK/constraints/TSRConstraint.cpp b/VirtualRobot/IK/constraints/TSRConstraint.cpp
index bfa17da7e..5e8491634 100644
--- a/VirtualRobot/IK/constraints/TSRConstraint.cpp
+++ b/VirtualRobot/IK/constraints/TSRConstraint.cpp
@@ -42,7 +42,7 @@ TSRConstraint::TSRConstraint(const RobotPtr& robot, const RobotNodeSetPtr& nodeS
     bounds(bounds),
     toleranceTranslation(1.0f),
     toleranceRotation(0.1f),
-    posRotTradeoff(0.1)
+    posRotTradeoff(0.1f)
 {
     ik.reset(new DifferentialIK(nodeSet));
 
-- 
GitLab