From f3162f14dca630a6f4a40071ed3c010672b955e1 Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Tue, 12 Apr 2016 16:30:47 +0200
Subject: [PATCH 01/11] Implemented moveit based distance computation

---
 cob_obstacle_distance/CMakeLists.txt           | 18 +++++++++++++-----
 .../src/debug/debug_obstacle_distance_node.cpp |  1 +
 cob_obstacle_distance/src/distance_manager.cpp |  2 +-
 .../src/cob_twist_controller.cpp               |  6 +++---
 4 files changed, 18 insertions(+), 9 deletions(-)

diff --git a/cob_obstacle_distance/CMakeLists.txt b/cob_obstacle_distance/CMakeLists.txt
index fd3e22d5..d1e9ed40 100644
--- a/cob_obstacle_distance/CMakeLists.txt
+++ b/cob_obstacle_distance/CMakeLists.txt
@@ -21,30 +21,38 @@ catkin_package(
   CATKIN_DEPENDS cob_control_msgs cob_srvs dynamic_reconfigure eigen_conversions geometry_msgs kdl_conversions kdl_parser moveit_msgs roscpp roslib sensor_msgs shape_msgs std_msgs tf_conversions tf urdf visualization_msgs
   DEPENDS assimp Boost fcl
   INCLUDE_DIRS include
-  LIBRARIES parsers marker_shapes_management
+  LIBRARIES parsers marker_shapes_management distance_manager
 )
 
 ### BUILD ###
 include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS} ${FCL_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS} ${ASSIMP_INCLUDE_DIRS})
 
+add_library(helper_functions src/helpers/helper_functions.cpp)
+add_dependencies(helper_functions ${catkin_EXPORTED_TARGETS})
+target_link_libraries(helper_functions ${catkin_LIBRARIES})
+
 add_library(parsers src/parsers/stl_parser.cpp src/parsers/mesh_parser.cpp)
 add_dependencies(parsers ${catkin_EXPORTED_TARGETS})
-target_link_libraries(parsers assimp ${fcl_LIBRARIES} ${catkin_LIBRARIES})
+target_link_libraries(parsers helper_functions assimp ${fcl_LIBRARIES} ${catkin_LIBRARIES})
 
 add_library(marker_shapes_management src/marker_shapes/marker_shapes_impl.cpp src/marker_shapes/marker_shapes_interface.cpp src/shapes_manager.cpp src/link_to_collision.cpp)
 add_dependencies(marker_shapes_management ${catkin_EXPORTED_TARGETS})
 target_link_libraries(marker_shapes_management parsers ${fcl_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
 
-add_executable(cob_obstacle_distance src/cob_obstacle_distance.cpp src/helpers/helper_functions.cpp src/distance_manager.cpp src/chainfk_solvers/advanced_chainfksolver_recursive.cpp)
+add_library(distance_manager src/distance_manager.cpp src/chainfk_solvers/advanced_chainfksolver_recursive.cpp)
+add_dependencies(distance_manager ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(distance_manager marker_shapes_management ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
+
+add_executable(cob_obstacle_distance src/cob_obstacle_distance.cpp)
 add_dependencies(cob_obstacle_distance ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(cob_obstacle_distance parsers marker_shapes_management ${fcl_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
+target_link_libraries(cob_obstacle_distance helper_functions parsers marker_shapes_management distance_manager ${fcl_LIBRARIES} ${catkin_LIBRARIES})
 
 add_executable(debug_obstacle_distance_node src/debug/debug_obstacle_distance_node.cpp)
 add_dependencies(debug_obstacle_distance_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(debug_obstacle_distance_node ${catkin_LIBRARIES})
 
 ### Install ###
-install(TARGETS cob_obstacle_distance parsers marker_shapes_management debug_obstacle_distance_node
+install(TARGETS cob_obstacle_distance parsers marker_shapes_management distance_manager debug_obstacle_distance_node
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp b/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp
index bdaf44d3..2c4dcd7f 100644
--- a/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp
+++ b/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp
@@ -93,6 +93,7 @@ std::string chain_base_link_;
             marker_vector.id = 42;
             marker_vector.header.frame_id = chain_base_link_;
 
+
             marker_vector.scale.x = 0.01;
             marker_vector.scale.y = 0.05;
 
diff --git a/cob_obstacle_distance/src/distance_manager.cpp b/cob_obstacle_distance/src/distance_manager.cpp
index 8c3fbc5c..7140d700 100644
--- a/cob_obstacle_distance/src/distance_manager.cpp
+++ b/cob_obstacle_distance/src/distance_manager.cpp
@@ -90,7 +90,7 @@ int DistanceManager::init()
         return -1;
     }
 
-    if(!nh_.getParam("joint_names", this->joints_))
+    if(!nh_.getParam("/arm_left/joint_names", this->joints_))
     {
         ROS_ERROR("Failed to get parameter \"joint_names\".");
         return -2;
diff --git a/cob_twist_controller/src/cob_twist_controller.cpp b/cob_twist_controller/src/cob_twist_controller.cpp
index d4b8b5ea..75d4b10c 100644
--- a/cob_twist_controller/src/cob_twist_controller.cpp
+++ b/cob_twist_controller/src/cob_twist_controller.cpp
@@ -142,7 +142,7 @@ bool CobTwistController::initialize()
     {
         twist_controller_params_.frame_names.push_back(chain_.getSegment(i).getName());
     }
-    register_link_client_ = nh_.serviceClient<cob_srvs::SetString>("obstacle_distance/registerLinkOfInterest");
+    register_link_client_ = nh_.serviceClient<cob_srvs::SetString>("/register_links");
     register_link_client_.waitForExistence(ros::Duration(5.0));
     twist_controller_params_.constraint_ca = CA_OFF;
 
@@ -164,7 +164,7 @@ bool CobTwistController::initialize()
     ros::Duration(1.0).sleep();
 
     /// initialize ROS interfaces
-    obstacle_distance_sub_ = nh_.subscribe("obstacle_distance", 1, &CallbackDataMediator::distancesToObstaclesCallback, &callback_data_mediator_);
+    obstacle_distance_sub_ = nh_.subscribe("/obstacle_distances", 1, &CallbackDataMediator::distancesToObstaclesCallback, &callback_data_mediator_);
     jointstate_sub_ = nh_.subscribe("joint_states", 1, &CobTwistController::jointstateCallback, this);
     twist_sub_ = nh_twist.subscribe("command_twist", 1, &CobTwistController::twistCallback, this);
     twist_stamped_sub_ = nh_twist.subscribe("command_twist_stamped", 1, &CobTwistController::twistStampedCallback, this);
@@ -309,7 +309,7 @@ void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistCo
     {
         if (!register_link_client_.exists())
         {
-            ROS_ERROR("ServiceServer 'obstacle_distance/registerLinkOfInterest' does not exist. CA not possible");
+            ROS_ERROR("ServiceServer '/register_links' does not exist. CA not possible");
             twist_controller_params_.constraint_ca = CA_OFF;
             config.constraint_ca = static_cast<int>(twist_controller_params_.constraint_ca);
             warning = true;

From 61e56ae804de651719d6d810d8b167cc9bafc5ca Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Tue, 19 Apr 2016 17:22:11 +0200
Subject: [PATCH 02/11] Work in progress: verify if the
 nearest_point_frame_vector is equal to the old vector

---
 .../cob_twist_controller/constraints/constraint_ca_impl.h   | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
index 16eca092..a655d794 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
@@ -287,9 +287,13 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
         {
             if (params.frame_names.end() != str_it)
             {
-                Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector;
+//                Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector;
+                Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector;
+
                 Eigen::Vector3d distance_vec = it->nearest_point_frame_vector - it->nearest_point_obstacle_vector;
 
+                ROS_INFO_STREAM("Distance: " << sqrt(pow(distance_vec[0],2) + pow(distance_vec[1],2) + pow(distance_vec[1],2)));
+
                 // Create a skew-symm matrix as transformation between the segment root and the critical point.
                 Eigen::Matrix3d skew_symm;
                 skew_symm <<     0.0,                       collision_pnt_vector.z(), -collision_pnt_vector.y(),

From 992054cde1736d3f18e3f6832f121b4af6642299 Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Tue, 10 May 2016 18:04:29 +0200
Subject: [PATCH 03/11] First implementation of CA with base_active

---
 .../cob_twist_controller.h                    |  2 +
 .../constraints/constraint_ca_impl.h          | 45 +++++++++++++++----
 .../inverse_differential_kinematics_solver.h  |  2 +
 .../kinematic_extension_base.h                |  2 +-
 .../kinematic_extension_builder.h             |  2 +-
 .../kinematic_extension_dof.h                 |  4 +-
 .../kinematic_extension_lookat.h              |  2 +-
 .../kinematic_extension_urdf.h                |  2 +-
 .../src/cob_twist_controller.cpp              |  5 +++
 ...inverse_differential_kinematics_solver.cpp |  4 +-
 .../kinematic_extension_builder.cpp           |  2 +-
 .../kinematic_extension_dof.cpp               | 37 +++++++++++----
 .../kinematic_extension_lookat.cpp            |  2 +-
 .../kinematic_extension_urdf.cpp              |  2 +-
 14 files changed, 86 insertions(+), 27 deletions(-)

diff --git a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h
index d9974c3d..f5d5816f 100644
--- a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h
+++ b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h
@@ -78,6 +78,7 @@ class CobTwistController
     KDL::Chain chain_;
     JointStates joint_states_;
     KDL::Twist twist_odometry_cb_;
+    KDL::Twist twist_odometry_bl_;
 
     TwistControllerParams twist_controller_params_;
 
@@ -119,6 +120,7 @@ class CobTwistController
 
     boost::recursive_mutex reconfig_mutex_;
     boost::shared_ptr< dynamic_reconfigure::Server<cob_twist_controller::TwistControllerConfig> > reconfigure_server_;
+    geometry_msgs::Pose pose_;
 };
 
 #endif  // COB_TWIST_CONTROLLER_COB_TWIST_CONTROLLER_H
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
index a655d794..bf187ade 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
@@ -287,13 +287,10 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
         {
             if (params.frame_names.end() != str_it)
             {
-//                Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector;
                 Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector;
 
                 Eigen::Vector3d distance_vec = it->nearest_point_frame_vector - it->nearest_point_obstacle_vector;
 
-                ROS_INFO_STREAM("Distance: " << sqrt(pow(distance_vec[0],2) + pow(distance_vec[1],2) + pow(distance_vec[1],2)));
-
                 // Create a skew-symm matrix as transformation between the segment root and the critical point.
                 Eigen::Matrix3d skew_symm;
                 skew_symm <<     0.0,                       collision_pnt_vector.z(), -collision_pnt_vector.y(),
@@ -301,6 +298,8 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
                                  collision_pnt_vector.y(), -collision_pnt_vector.x(),  0.0;
 
                 Eigen::Matrix3d ident = Eigen::Matrix3d::Identity();
+
+                // ToDo: dynamic matrix size for Base Active mode.
                 Eigen::Matrix<double, 6, 6> T;
                 T.block(0, 0, 3, 3) << ident;
                 T.block(0, 3, 3, 3) << skew_symm;
@@ -312,7 +311,8 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
                 uint32_t frame_number = idx + 1;  // segment nr not index represents frame number
 
                 KDL::JntArray ja = this->joint_states_.current_q_;
-                KDL::Jacobian new_jac_chain(this->joint_states_.current_q_.rows());
+                ja.resize((unsigned int)params.dof);
+                KDL::Jacobian new_jac_chain(ja.rows());
 
                 // ROS_INFO_STREAM("frame_number: " << frame_number);
                 // ROS_INFO_STREAM("ja.rows: " << ja.rows());
@@ -393,22 +393,26 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
 
     if (params.frame_names.end() != str_it)
     {
+        unsigned int dof;
         if (this->constraint_params_.current_distances_.size() > 0)
         {
             uint32_t frame_number = (str_it - params.frame_names.begin()) + 1;  // segment nr not index represents frame number
             KDL::FrameVel frame_vel;
-
+            ROS_WARN_STREAM("frame_number" << frame_number);
             // ToDo: the fk_solver_vel_ is only initialized for the primary chain - kinematic extensions cannot be considered yet!
             KDL::JntArrayVel jnts_prediction_chain(params.dof);
+
             for (unsigned int i = 0; i < params.dof; i++)
             {
                 jnts_prediction_chain.q(i) = this->jnts_prediction_.q(i);
                 jnts_prediction_chain.qdot(i) = this->jnts_prediction_.qdot(i);
             }
-            // ROS_INFO_STREAM("jnts_prediction_chain.q.rows: " << jnts_prediction_chain.q.rows());
 
-            // Calculate prediction for pos and vel
-            int error = this->fk_solver_vel_.JntToCart(this->jnts_prediction_, frame_vel, frame_number);
+
+//             ROS_INFO_STREAM("jnts_prediction_chain.q.rows: " << jnts_prediction_chain.q.rows());
+
+            // Calculate prediction for the mainipulator
+            int error = this->fk_solver_vel_.JntToCart(jnts_prediction_chain, frame_vel, frame_number);
             if (error != 0)
             {
                 ROS_ERROR_STREAM("Could not calculate twist for frame: " << frame_number << ". Error Code: " << error << " (" << this->fk_solver_vel_.strError(error) << ")");
@@ -417,7 +421,30 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
             }
             // ROS_INFO_STREAM("Calculated twist for frame: " << frame_number);
 
-            KDL::Twist twist = frame_vel.GetTwist();  // predicted frame twist
+            KDL::Twist predicted_twist_odometry;
+            if(params.kinematic_extension == BASE_ACTIVE)   // jnts_prediction_ gives us the predicted joint_state for the plattform (calculated in stack_of_tasks_solver.cpp)
+            {
+                predicted_twist_odometry.vel.data[0] = this->jnts_prediction_.qdot(params.dof);
+                predicted_twist_odometry.vel.data[1] = this->jnts_prediction_.qdot(params.dof+1);
+                predicted_twist_odometry.vel.data[2] = this->jnts_prediction_.qdot(params.dof+2);
+
+                predicted_twist_odometry.rot.data[0] = this->jnts_prediction_.qdot(params.dof+3);
+                predicted_twist_odometry.rot.data[1] = this->jnts_prediction_.qdot(params.dof+4);
+                predicted_twist_odometry.rot.data[2] = this->jnts_prediction_.qdot(params.dof+5);
+            }
+            else
+            {
+                predicted_twist_odometry.vel.data[0] = 0;
+                predicted_twist_odometry.vel.data[1] = 0;
+                predicted_twist_odometry.vel.data[2] = 0;
+
+                predicted_twist_odometry.rot.data[0] = 0;
+                predicted_twist_odometry.rot.data[1] = 0;
+                predicted_twist_odometry.rot.data[2] = 0;
+            }
+
+
+            KDL::Twist twist = frame_vel.GetTwist() + predicted_twist_odometry;  // predicted frame twist
 
             Eigen::Vector3d pred_twist_vel;
             tf::vectorKDLToEigen(twist.vel, pred_twist_vel);
diff --git a/cob_twist_controller/include/cob_twist_controller/inverse_differential_kinematics_solver.h b/cob_twist_controller/include/cob_twist_controller/inverse_differential_kinematics_solver.h
index 170f6e6c..af46f642 100644
--- a/cob_twist_controller/include/cob_twist_controller/inverse_differential_kinematics_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/inverse_differential_kinematics_solver.h
@@ -85,6 +85,8 @@ class InverseDifferentialKinematicsSolver
 
     /** CartToJnt for chain using SVD considering KinematicExtensions and various DampingMethods **/
     virtual int CartToJnt(const JointStates& joint_states,
+                          const geometry_msgs::Pose pose,
+                          const KDL::Twist& twist,
                           const KDL::Twist& v_in,
                           KDL::JntArray& qdot_out);
 
diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_base.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_base.h
index 77da1feb..9eeade41 100644
--- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_base.h
+++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_base.h
@@ -51,7 +51,7 @@ class KinematicExtensionBase
 
         virtual bool initExtension() = 0;
         virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain) = 0;
-        virtual JointStates adjustJointStates(const JointStates& joint_states) = 0;
+        virtual JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist) = 0;
         virtual LimiterParams adjustLimiterParams(const LimiterParams& limiter_params) = 0;
         virtual void processResultExtension(const KDL::JntArray& q_dot_ik) = 0;
 
diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_builder.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_builder.h
index 20c571c8..b2428a8d 100644
--- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_builder.h
+++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_builder.h
@@ -66,7 +66,7 @@ class KinematicExtensionNone : public KinematicExtensionBase
 
         bool initExtension();
         KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain);
-        JointStates adjustJointStates(const JointStates& joint_states);
+        JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist);
         LimiterParams adjustLimiterParams(const LimiterParams& limiter_params);
         void processResultExtension(const KDL::JntArray& q_dot_ik);
 };
diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_dof.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_dof.h
index daa495c3..267ba676 100644
--- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_dof.h
+++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_dof.h
@@ -52,7 +52,7 @@ class KinematicExtensionDOF : public KinematicExtensionBase
 
         virtual bool initExtension() = 0;
         virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain) = 0;
-        virtual JointStates adjustJointStates(const JointStates& joint_states) = 0;
+        virtual JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist) = 0;
         virtual LimiterParams adjustLimiterParams(const LimiterParams& limiter_params) = 0;
         virtual void processResultExtension(const KDL::JntArray& q_dot_ik) = 0;
 
@@ -94,7 +94,7 @@ class KinematicExtensionBaseActive : public KinematicExtensionDOF
 
         bool initExtension();
         KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain);
-        JointStates adjustJointStates(const JointStates& joint_states);
+        JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist);
         LimiterParams adjustLimiterParams(const LimiterParams& limiter_params);
         void processResultExtension(const KDL::JntArray& q_dot_ik);
 
diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_lookat.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_lookat.h
index 3b3c8d13..8db3ea26 100644
--- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_lookat.h
+++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_lookat.h
@@ -65,7 +65,7 @@ class KinematicExtensionLookat : public KinematicExtensionBase
 
         bool initExtension();
         virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain);
-        virtual JointStates adjustJointStates(const JointStates& joint_states);
+        virtual JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist);
         virtual LimiterParams adjustLimiterParams(const LimiterParams& limiter_params);
         virtual void processResultExtension(const KDL::JntArray& q_dot_ik);
 
diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_urdf.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_urdf.h
index 03133139..87ae0aa5 100644
--- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_urdf.h
+++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_urdf.h
@@ -55,7 +55,7 @@ class KinematicExtensionURDF : public KinematicExtensionBase
 
         bool initExtension();
         virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain);
-        virtual JointStates adjustJointStates(const JointStates& joint_states);
+        virtual JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist);
         virtual LimiterParams adjustLimiterParams(const LimiterParams& limiter_params);
         virtual void processResultExtension(const KDL::JntArray& q_dot_ik);
 
diff --git a/cob_twist_controller/src/cob_twist_controller.cpp b/cob_twist_controller/src/cob_twist_controller.cpp
index 75d4b10c..a9f03b9c 100644
--- a/cob_twist_controller/src/cob_twist_controller.cpp
+++ b/cob_twist_controller/src/cob_twist_controller.cpp
@@ -142,6 +142,7 @@ bool CobTwistController::initialize()
     {
         twist_controller_params_.frame_names.push_back(chain_.getSegment(i).getName());
     }
+
     register_link_client_ = nh_.serviceClient<cob_srvs::SetString>("/register_links");
     register_link_client_.waitForExistence(ros::Duration(5.0));
     twist_controller_params_.constraint_ca = CA_OFF;
@@ -388,6 +389,8 @@ void CobTwistController::solveTwist(KDL::Twist twist)
     }
 
     int ret_ik = p_inv_diff_kin_solver_->CartToJnt(this->joint_states_,
+                                                   pose_,
+                                                   twist_odometry_bl_,
                                                    twist,
                                                    q_dot_ik);
 
@@ -565,5 +568,7 @@ void CobTwistController::odometryCallback(const nav_msgs::Odometry::ConstPtr& ms
     // transform into chain_base
     twist_odometry_transformed_cb = cb_frame_bl * (twist_odometry_bl + tangential_twist_bl);
 
+    twist_odometry_bl_ = twist_odometry_bl;
     twist_odometry_cb_ = twist_odometry_transformed_cb;
+    pose_ = msg->pose.pose; // Needed for selfcollision avoidance in stack_of_tasks_solver.cpp
 }
diff --git a/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp b/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp
index d83ae576..fed4f137 100644
--- a/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp
+++ b/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp
@@ -36,6 +36,8 @@
  * Solve the inverse kinematics problem at the first order differential level.
  */
 int InverseDifferentialKinematicsSolver::CartToJnt(const JointStates& joint_states,
+                                                   const geometry_msgs::Pose pose,
+                                                   const KDL::Twist& twist,
                                                    const KDL::Twist& v_in,
                                                    KDL::JntArray& qdot_out)
 {
@@ -47,7 +49,7 @@ int InverseDifferentialKinematicsSolver::CartToJnt(const JointStates& joint_stat
     jnt2jac_.JntToJac(joint_states.current_q_, jac_chain);
     // ROS_INFO_STREAM("jac_chain.rows: " << jac_chain.rows() << ", jac_chain.columns: " << jac_chain.columns());
 
-    JointStates joint_states_full = this->kinematic_extension_->adjustJointStates(joint_states);
+    JointStates joint_states_full = this->kinematic_extension_->adjustJointStates(joint_states, pose, twist);
     // ROS_INFO_STREAM("joint_states_full.current_q_: " << joint_states_full.current_q_.rows());
 
     /// append columns to Jacobian in order to reflect additional DoFs of kinematical extension
diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_builder.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_builder.cpp
index 715666b9..b30e92f3 100644
--- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_builder.cpp
+++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_builder.cpp
@@ -86,7 +86,7 @@ KDL::Jacobian KinematicExtensionNone::adjustJacobian(const KDL::Jacobian& jac_ch
 /**
  * Method adjusting the JointStates used in inverse differential computation and limiters. No changes applied.
  */
-JointStates KinematicExtensionNone::adjustJointStates(const JointStates& joint_states)
+JointStates KinematicExtensionNone::adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist)
 {
     return joint_states;
 }
diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
index 60f60df3..e407af90 100644
--- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
+++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
@@ -214,7 +214,7 @@ KDL::Jacobian KinematicExtensionBaseActive::adjustJacobian(const KDL::Jacobian&
 /**
  * Method adjusting the JointStates used in inverse differential computation and limiters. Fill neutrally.
  */
-JointStates KinematicExtensionBaseActive::adjustJointStates(const JointStates& joint_states)
+JointStates KinematicExtensionBaseActive::adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist)
 {
     JointStates js;
     unsigned int chain_dof = joint_states.current_q_.rows();
@@ -230,13 +230,34 @@ JointStates KinematicExtensionBaseActive::adjustJointStates(const JointStates& j
         js.current_q_dot_(i) = joint_states.current_q_dot_(i);
         js.last_q_dot_(i) = joint_states.last_q_dot_(i);
     }
-    for (unsigned int i = 0; i < ext_dof_; i++)
-    {
-        js.current_q_(chain_dof + i) = 0.0;
-        js.last_q_(chain_dof + i) = 0.0;
-        js.current_q_dot_(chain_dof + i) = 0.0;
-        js.last_q_dot_(chain_dof + i) = 0.0;
-    }
+//    for (unsigned int i = 0; i < ext_dof_; i++)
+//    {
+//        js.current_q_(chain_dof + i) = 0.0;
+//        js.last_q_(chain_dof + i) = 0.0;
+//        js.current_q_dot_(chain_dof + i) = 0.0;
+//        js.last_q_dot_(chain_dof + i) = 0.0;
+//    }
+    double roll, pitch, yaw;
+    tf::Quaternion q;
+    q.setW(pose.orientation.w);
+    q.setX(pose.orientation.x);
+    q.setY(pose.orientation.y);
+    q.setZ(pose.orientation.z);
+    tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
+
+    js.current_q_(chain_dof + 0) = pose.position.x;
+    js.current_q_(chain_dof + 1) = pose.position.y;
+    js.current_q_(chain_dof + 2) = 0;
+    js.current_q_(chain_dof + 3) = 0;
+    js.current_q_(chain_dof + 4) = 0;
+    js.current_q_(chain_dof + 5) = yaw;
+
+    js.current_q_dot_(chain_dof + 0) = twist.vel.data[0];
+    js.current_q_dot_(chain_dof + 1) = twist.vel.data[1];
+    js.current_q_dot_(chain_dof + 2) = 0;
+    js.current_q_dot_(chain_dof + 3) = 0;
+    js.current_q_dot_(chain_dof + 4) = 0;
+    js.current_q_dot_(chain_dof + 5) = twist.rot.data[2];
     return js;
 }
 
diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp
index 2ea2ec67..6aaa45f8 100644
--- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp
+++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp
@@ -164,7 +164,7 @@ KDL::Jacobian KinematicExtensionLookat::adjustJacobian(const KDL::Jacobian& jac_
     return jac_full;
 }
 
-JointStates KinematicExtensionLookat::adjustJointStates(const JointStates& joint_states)
+JointStates KinematicExtensionLookat::adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist)
 {
     boost::mutex::scoped_lock lock(mutex_);
     unsigned int chain_dof = joint_states.current_q_.rows();
diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp
index 0be86074..ad9b8d0d 100644
--- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp
+++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp
@@ -191,7 +191,7 @@ KDL::Jacobian KinematicExtensionURDF::adjustJacobian(const KDL::Jacobian& jac_ch
     return jac_full;
 }
 
-JointStates KinematicExtensionURDF::adjustJointStates(const JointStates& joint_states)
+JointStates KinematicExtensionURDF::adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist)
 {
     JointStates js;
     unsigned int chain_dof = joint_states.current_q_.rows();

From abafddb43a8991c3f48a6e521da44671dda659e1 Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Tue, 26 Jul 2016 14:19:11 +0200
Subject: [PATCH 04/11] Added a new constraint to avoid singular regions (Still
 work in progess).. we might change this one to maximize the manipulability
 rate to find a better value for the damping_gain which we introduced to the
 pseudo inverse calculations. The main change in the code is the additional
 constriant.

---
 cob_twist_controller/CMakeLists.txt           |   6 +-
 cob_twist_controller/cfg/TwistController.cfg  |  20 +-
 .../callback_data_mediator.h                  |   7 +
 .../cob_twist_controller_data_types.h         |  27 ++
 .../factories/solver_factory.h                |   7 +-
 .../solvers/constraint_solver_base.h          |   3 +-
 .../gradient_projection_method_solver.h       |   5 +-
 .../solvers/stack_of_tasks_solver.h           |   3 +-
 .../solvers/task_priority_solver.h            |   3 +-
 .../solvers/unconstraint_solver.h             |   3 +-
 .../solvers/weighted_least_norm_solver.h      |   3 +-
 .../constraints/constraint.h                  |  47 +++
 .../constraints/constraint_base.h             |   5 +-
 .../constraints/constraint_ca_impl.h          |   2 -
 .../constraints/constraint_impl.h             |  21 ++
 .../constraints/constraint_jla_impl.h         |   5 +
 .../constraints/constraint_jsa_impl.h         | 271 ++++++++++++++++++
 .../constraints/constraint_params.h           |  29 ++
 .../utils/chainjnttojacdotsolver.hpp          | 176 ++++++++++++
 .../src/callback_data_mediator.cpp            |   6 +
 .../src/cob_twist_controller.cpp              |  14 +
 .../gradient_projection_method_solver.cpp     |   4 +-
 .../solvers/stack_of_tasks_solver.cpp         |   9 +-
 .../solvers/task_priority_solver.cpp          |   4 +-
 .../solvers/unconstraint_solver.cpp           |   4 +-
 .../solvers/weighted_least_norm_solver.cpp    |   4 +-
 .../wln_joint_limit_avoidance_solver.cpp      |   1 +
 ...inverse_differential_kinematics_solver.cpp |   2 +-
 .../inverse_jacobian_calculation.cpp          |  23 +-
 .../kinematic_extension_dof.cpp               |  35 ++-
 .../twist_controller_config.py                |   9 +
 .../src/utils/chainjnttojacdotsolver.cpp      | 229 +++++++++++++++
 32 files changed, 962 insertions(+), 25 deletions(-)
 create mode 100644 cob_twist_controller/include/cob_twist_controller/constraints/constraint_jsa_impl.h
 create mode 100644 cob_twist_controller/include/cob_twist_controller/utils/chainjnttojacdotsolver.hpp
 create mode 100644 cob_twist_controller/src/utils/chainjnttojacdotsolver.cpp

diff --git a/cob_twist_controller/CMakeLists.txt b/cob_twist_controller/CMakeLists.txt
index 197b6341..f31c7f49 100644
--- a/cob_twist_controller/CMakeLists.txt
+++ b/cob_twist_controller/CMakeLists.txt
@@ -29,6 +29,10 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS} ${oroco
 set(SRC_C_DIR "src/constraint_solvers")
 set(SRC_CS_DIR "${SRC_C_DIR}/solvers")
 
+add_library(jacobian_derivative src/utils/chainjnttojacdotsolver.cpp)
+add_dependencies(jacobian_derivative ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
+target_link_libraries(jacobian_derivative ${catkin_LIBRARIES})
+
 add_library(damping_methods src/damping_methods/damping.cpp)
 target_link_libraries(damping_methods ${catkin_LIBRARIES})
 
@@ -37,7 +41,7 @@ target_link_libraries(inv_calculations ${catkin_LIBRARIES})
 
 add_library(constraint_solvers ${SRC_CS_DIR}/unconstraint_solver.cpp ${SRC_CS_DIR}/wln_joint_limit_avoidance_solver.cpp ${SRC_CS_DIR}/weighted_least_norm_solver.cpp ${SRC_CS_DIR}/gradient_projection_method_solver.cpp ${SRC_CS_DIR}/stack_of_tasks_solver.cpp ${SRC_CS_DIR}/task_priority_solver.cpp ${SRC_C_DIR}/constraint_solver_factory.cpp)
 add_dependencies(constraint_solvers ${catkin_EXPORTED_TARGETS})
-target_link_libraries(constraint_solvers damping_methods inv_calculations)
+target_link_libraries(constraint_solvers damping_methods inv_calculations jacobian_derivative)
 
 add_library(limiters src/limiters/limiter.cpp)
 target_link_libraries(limiters ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
diff --git a/cob_twist_controller/cfg/TwistController.cfg b/cob_twist_controller/cfg/TwistController.cfg
index de7cb34c..2b1e3f2a 100755
--- a/cob_twist_controller/cfg/TwistController.cfg
+++ b/cob_twist_controller/cfg/TwistController.cfg
@@ -27,7 +27,6 @@ solver_types_enum = gen.enum([
                        ],
                      "enum types for the solvers")
 
-
 jla_constraints_enum = gen.enum([
                        gen.const("JLA_OFF",     int_t, 0, "JLA inactive"),
                        gen.const("JLA",         int_t, 1, "JLA active"),
@@ -35,6 +34,12 @@ jla_constraints_enum = gen.enum([
                        gen.const("JLA_INEQ",    int_t, 3, "Inequality constraint for JLA."),
                        ],
                      "enum types for the joint limit avoidance constraints")
+                     
+jsa_constraints_enum = gen.enum([
+                       gen.const("JSA_OFF",     int_t, 0, "JSA inactive"),
+                       gen.const("JSA",         int_t, 1, "JSA active"),
+                       ],
+                     "enum types for the joint singularity avoidance constraints")
 
 ca_constraints_enum = gen.enum([
                        gen.const("CA_OFF",            int_t, 0, "CA inactive"),
@@ -58,15 +63,20 @@ ctrl_interface.add("integrator_smoothing",   double_t, 0, "The factor used for e
 # ==================================== Damping and truncation (singular value adaption) ====================================================
 damp_trunc = gen.add_group("Damping and Truncation", "damping_truncation")
 damp_trunc.add("numerical_filtering",         bool_t,   0, "Numerical Filtering yes/no",  False)
+damp_trunc.add("singular_value_damping",      bool_t,   0, "Singular value daming yes/no",  True)
 damp_trunc.add("damping_method",              int_t,    0, "The damping method to use.", 2, None, None, edit_method=damping_method_enum)
 damp_trunc.add("damping_factor",     double_t, 0, "The constant damping_factor (used in CONSTANT)",  0.01, 0, 1)
 damp_trunc.add("lambda_max",         double_t, 0, "Value for maximum damping_factor (used in MANIPULABILITY/LSV)",  0.1, 0, 10)
 damp_trunc.add("w_threshold",        double_t, 0, "Value for manipulability threshold (used in MANIPULABILITY)",  0.005, 0, 1)
 damp_trunc.add("beta",               double_t, 0, "Beta for Low Isotropic Damping",  0.005, 0, 1)
 damp_trunc.add("eps_damping",        double_t, 0, "Value for least singular value damping",  0.003, 0,   1)
-
 damp_trunc.add("eps_truncation",     double_t, 0, "Value for singular value threshold (used for truncation: sing. value < eps)",    0.001, 0,   1)
 
+damp_trunc.add("damping_delta",      double_t, 0, "Offset for singular value daming",  0.1, 0,   5)
+damp_trunc.add("damping_gain",       double_t, 0, "Gain for singular value daming",  0.02, 0,   5)
+damp_trunc.add("damping_slope",      double_t, 0, "Slope of singular value daming",  0.05, 0,   5)
+
+
 # ==================================== Parameters for the solver (e.g. WLN, CA and JLA) =====================================================
 solv_constr = gen.add_group("Solver and Constraints", "solver_constraints")
 solv_constr.add("solver",             int_t,    0, "The solver to use (edited via an enum)", 1, None, None, edit_method=solver_types_enum)
@@ -82,6 +92,12 @@ jla.add("activation_buffer_jla",    double_t, 0, "In [%]. For smooth transition
 jla.add("critical_threshold_jla",   double_t, 0, "In [%]. Tolerance when critical region becomes active. JLA becomes task. Should be less than activation threshold (best experienced: 1/2 of activation threshold).", 5.0, 0.0, 100.0)
 jla.add("damping_jla",              double_t, 0, "Const. damping factor for the inv. of the JLA task jacobian", 0.000001, 0.0, 1.0)
 
+jsa = solv_constr.add_group("Joint Singularity Avoidance", "jsa")
+jsa.add("constraint_jsa",           int_t,    0, "The JSA constraint to use (edited via an enum)", 1, None, None, edit_method=jsa_constraints_enum)
+jsa.add("priority_jsa",             int_t,    0, "Priority for the joint singularity avoidance constraint (important for task processing; 0 = highest prio)", 50, 0,   1000)
+jsa.add("k_H_jsa",                  double_t, 0, "Self-motion factor for GPM. Special weighting for JSA constraint", -1.0, -1000,   1000)
+jsa.add("damping_jsa",              double_t, 0, "Const. damping factor for the inv. of the JSA task jacobian", 0.000001, 0.0, 1.0)
+
 ca = solv_constr.add_group("Collision Avoidance", "ca")
 ca.add("constraint_ca",           int_t,    0, "The CA constraint to use (edited via an enum)", 0, None, None, edit_method=ca_constraints_enum)
 ca.add("priority_ca",             int_t,    0, "Priority for the collision avoidance constraint (important for task processing; 0 = highest prio).", 100, 0,   1000)
diff --git a/cob_twist_controller/include/cob_twist_controller/callback_data_mediator.h b/cob_twist_controller/include/cob_twist_controller/callback_data_mediator.h
index 6d1869f2..fc1b09f2 100644
--- a/cob_twist_controller/include/cob_twist_controller/callback_data_mediator.h
+++ b/cob_twist_controller/include/cob_twist_controller/callback_data_mediator.h
@@ -69,6 +69,13 @@ class CallbackDataMediator
          */
         bool fill(ConstraintParamsJLA& params_jla);
 
+        /**
+         * Special implementation for Joint Singularity Avoidance parameters.
+         * @param params_jsa Reference to JSA parameters.
+         * @return Success of filling parameters.
+         */
+        bool fill(ConstraintParamsJSA& params_jsa);
+
         /**
          * Callback method that can be used by a ROS subscriber to a obstacle distance topic.
          * @param msg The published message containting obstacle distances.
diff --git a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h
index 0b7894d6..b2422f20 100644
--- a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h
+++ b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h
@@ -93,6 +93,12 @@ enum ConstraintTypesJLA
     JLA_INEQ_ON = cob_twist_controller::TwistController_JLA_INEQ,
 };
 
+enum ConstraintTypesJSA
+{
+    JSA_OFF = cob_twist_controller::TwistController_JSA_OFF,
+    JSA_ON = cob_twist_controller::TwistController_JSA,
+};
+
 enum ConstraintTypes
 {
     None,
@@ -100,6 +106,7 @@ enum ConstraintTypes
     JLA,
     JLA_MID,
     JLA_INEQ,
+    JSA,
 };
 
 enum LookatAxisTypes
@@ -201,6 +208,7 @@ struct TwistControllerParams
         integrator_smoothing(0.2),
 
         numerical_filtering(false),
+        singular_value_damping(true),
         damping_method(MANIPULABILITY),
         damping_factor(0.2),
         lambda_max(0.1),
@@ -208,6 +216,9 @@ struct TwistControllerParams
         beta(0.005),
         eps_damping(0.003),
         eps_truncation(0.001),
+        damping_delta(0.1),
+        damping_gain(0.02),
+        damping_slope(0.05),
 
         solver(GPM),
         priority_main(500),
@@ -223,6 +234,11 @@ struct TwistControllerParams
         damping_ca(0.000001),
         k_H_ca(2.0),
 
+        constraint_jsa(JSA_ON),
+        priority_jsa(50),
+        k_H_jsa(-10.0),
+        damping_jsa(0.000001),
+
         kinematic_extension(NO_EXTENSION),
         extension_ratio(0.0)
     {
@@ -243,6 +259,7 @@ struct TwistControllerParams
     double integrator_smoothing;
 
     bool numerical_filtering;
+    bool singular_value_damping;
     DampingMethodTypes damping_method;
     double damping_factor;
     double lambda_max;
@@ -250,6 +267,9 @@ struct TwistControllerParams
     double beta;
     double eps_damping;
     double eps_truncation;
+    double damping_delta;
+    double damping_gain;
+    double damping_slope;
 
     SolverTypes solver;
     uint32_t priority_main;
@@ -267,6 +287,11 @@ struct TwistControllerParams
     double damping_jla;
     ConstraintThresholds thresholds_jla;
 
+    ConstraintTypesJSA constraint_jsa;
+    uint32_t priority_jsa;
+    double k_H_jsa;
+    double damping_jsa;
+
     LimiterParams limiter_params;
 
     KinematicExtensionTypes kinematic_extension;
@@ -278,6 +303,8 @@ struct TwistControllerParams
 
     // vector of links of the chain to be considered for collision avoidance
     std::vector<std::string> collision_check_links;
+
+    KDL::Chain chain;
 };
 
 enum EN_ConstraintStates
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h
index b2133a01..95b3fbcc 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h
@@ -33,6 +33,7 @@
 #include <Eigen/Core>
 #include <Eigen/SVD>
 #include <kdl/jntarray.hpp>
+#include <eigen_conversions/eigen_kdl.h>
 
 #include "cob_twist_controller/damping_methods/damping_base.h"
 #include "cob_twist_controller/constraints/constraint_base.h"
@@ -88,12 +89,16 @@ class SolverFactory : public ISolverFactory
             constraint_solver_->setJacobianData(jacobian_data);
             constraint_solver_->setConstraints(constraints);
             constraint_solver_->setDamping(damping_method);
-            Eigen::MatrixXd new_q_dot = constraint_solver_->solve(in_cart_velocities, joint_states);
+
+            bool active_constraint = false;
+            Eigen::MatrixXd new_q_dot = constraint_solver_->solve(in_cart_velocities, joint_states, active_constraint);
+
             return new_q_dot;
         }
 
     private:
         boost::shared_ptr<T> constraint_solver_;
+        JointStates joint_states_backup_;
 };
 
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINT_SOLVERS_FACTORIES_SOLVER_FACTORY_H
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h
index 5748bcd3..811aed11 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h
@@ -52,7 +52,8 @@ class ConstraintSolver
          * @return The calculated new joint velocities.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states) = 0;
+                                      const JointStates& joint_states,
+                                      bool &active_constraint) = 0;
 
         /**
          * Inline method to set the damping
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h
index 8ad8d3d4..0a90762f 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h
@@ -56,7 +56,10 @@ class GradientProjectionMethodSolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states);
+                                      const JointStates& joint_states,
+                                      bool &active_constraint);
+
 };
 
+
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINT_SOLVERS_SOLVERS_GRADIENT_PROJECTION_METHOD_SOLVER_H
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/stack_of_tasks_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/stack_of_tasks_solver.h
index cec0e4da..382d3fcd 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/stack_of_tasks_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/stack_of_tasks_solver.h
@@ -62,7 +62,8 @@ class StackOfTasksSolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states);
+                                      const JointStates& joint_states,
+                                      bool &active_constraint);
 
         /**
          * Process the state of the constraint and update the sum_of_gradient.
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h
index 2d0ce190..3f73327f 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h
@@ -59,7 +59,8 @@ class TaskPrioritySolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states);
+                                      const JointStates& joint_states,
+                                      bool &active_constraint);
 
     protected:
         ros::Time last_time_;
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unconstraint_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unconstraint_solver.h
index 4c1ca105..9e63e4fc 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unconstraint_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unconstraint_solver.h
@@ -50,7 +50,8 @@ class UnconstraintSolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states);
+                                      const JointStates& joint_states,
+                                      bool &active_constraint);
 };
 
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINT_SOLVERS_SOLVERS_UNCONSTRAINT_SOLVER_H
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/weighted_least_norm_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/weighted_least_norm_solver.h
index bbe7a117..aeb883ca 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/weighted_least_norm_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/weighted_least_norm_solver.h
@@ -52,7 +52,8 @@ class WeightedLeastNormSolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states);
+                                      const JointStates& joint_states,
+                                      bool &active_constraint);
 
     private:
         /**
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint.h
index 863ba9d0..0b6c1120 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint.h
@@ -250,6 +250,53 @@ class JointLimitAvoidanceIneq : public ConstraintBase<T_PARAMS, PRIO>
 };
 /* END JointLimitAvoidanceIneq **************************************************************************************/
 
+/* BEGIN JointSingularityAvoidance ************************************************************************************/
+/// Class providing methods that realize a JointSingularityAvoidance constraint.
+template <typename T_PARAMS, typename PRIO = uint32_t>
+class JointSingularityAvoidance : public ConstraintBase<T_PARAMS, PRIO>
+{
+    public:
+        JointSingularityAvoidance(PRIO prio,
+                            T_PARAMS constraint_params,
+                            CallbackDataMediator& cbdm,
+                            KDL::ChainFkSolverVel_recursive& fk_solver_vel) :
+              ConstraintBase<T_PARAMS, PRIO>(prio, constraint_params, cbdm),
+              abs_delta_max_(std::numeric_limits<double>::max()),
+              abs_delta_min_(std::numeric_limits<double>::max()),
+              rel_max_(1.0),
+              rel_min_(1.0),
+              fk_solver_vel_(fk_solver_vel)
+        {}
+
+        virtual ~JointSingularityAvoidance()
+        {}
+
+        virtual Task_t createTask();
+        virtual std::string getTaskId() const;
+
+        virtual void calculate();
+        virtual Eigen::MatrixXd getTaskJacobian() const;
+        virtual Eigen::VectorXd getTaskDerivatives() const;
+
+        virtual double getActivationGain() const;
+        virtual double getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const;
+
+    private:
+        virtual ConstraintTypes getType() const;
+
+        void calcValue();
+        void calcDerivativeValue();
+        void calcPartialValues();
+
+        double abs_delta_max_;
+        double abs_delta_min_;
+        double rel_max_;
+        double rel_min_;
+        KDL::ChainFkSolverVel_recursive& fk_solver_vel_;
+
+};
+/* END JointLimitAvoidance **************************************************************************************/
+
 typedef ConstraintsBuilder<uint32_t> ConstraintsBuilder_t;
 
 #include "cob_twist_controller/constraints/constraint_impl.h"   // implementation of templated class
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h
index fb9519b2..afc1e032 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h
@@ -140,7 +140,8 @@ class ConstraintBase : public PriorityBase<PRIO>
           prediction_value_(std::numeric_limits<double>::max()),
           last_value_(0.0),
           last_time_(ros::Time::now()),
-          last_pred_time_(ros::Time::now())
+          last_pred_time_(ros::Time::now()),
+          init_(false)
         {
             this->member_inst_cnt_ = instance_ctr_++;
         }
@@ -222,6 +223,8 @@ class ConstraintBase : public PriorityBase<PRIO>
         JointStates joint_states_;
         KDL::JntArrayVel jnts_prediction_;
         Matrix6Xd_t jacobian_data_;
+        Matrix6Xd_t jacobian_data_old_;
+        bool init_;
 
         double value_;
         double derivative_value_;
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
index bf187ade..524d06ff 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
@@ -408,7 +408,6 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
                 jnts_prediction_chain.qdot(i) = this->jnts_prediction_.qdot(i);
             }
 
-
 //             ROS_INFO_STREAM("jnts_prediction_chain.q.rows: " << jnts_prediction_chain.q.rows());
 
             // Calculate prediction for the mainipulator
@@ -443,7 +442,6 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
                 predicted_twist_odometry.rot.data[2] = 0;
             }
 
-
             KDL::Twist twist = frame_vel.GetTwist() + predicted_twist_odometry;  // predicted frame twist
 
             Eigen::Vector3d pred_twist_vel;
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h
index 876deca5..6548d841 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h
@@ -124,6 +124,24 @@ std::set<ConstraintBase_t> ConstraintsBuilder<PRIO>::createConstraints(const Twi
         // So the log would be filled unnecessarily.
     }
 
+    // Joint Singularity Avoidance
+    if (JSA_ON == tc_params.constraint_jsa)
+    {
+        typedef JointSingularityAvoidance<ConstraintParamsJSA, PRIO> Jsa_t;
+
+        ConstraintParamsJSA params = ConstraintParamFactory<ConstraintParamsJSA>::createConstraintParams(tc_params, limiter_params, data_mediator);
+        uint32_t startPrio = 20; // Todo
+
+
+        // TODO: take care PRIO could be of different type than UINT32
+        params.joint_ = tc_params.joints[tc_params.joints.size()-1];
+        params.joint_idx_ = static_cast<int32_t>(tc_params.joints.size()-1);
+        // copy of params will be created; priority increased with each joint.
+        boost::shared_ptr<Jsa_t > jsa(new Jsa_t(startPrio++, params, data_mediator, fk_solver_vel));
+        constraints.insert(boost::static_pointer_cast<PriorityBase<PRIO> >(jsa));
+
+    }
+
     return constraints;
 }
 /* END ConstraintsBuilder *******************************************************************************************/
@@ -134,4 +152,7 @@ std::set<ConstraintBase_t> ConstraintsBuilder<PRIO>::createConstraints(const Twi
 // Joint Limit Avoidance Constraint Implementation
 #include "cob_twist_controller/constraints/constraint_jla_impl.h"
 
+// Joint Limit Avoidance Constraint Implementation
+#include "cob_twist_controller/constraints/constraint_jsa_impl.h"
+
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_IMPL_H
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h
index 48104d5c..7ade5039 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h
@@ -224,9 +224,13 @@ void JointLimitAvoidance<T_PARAMS, PRIO>::calcPartialValues()
     const double max_delta = (limits_max - joint_pos);
     const double nominator = (2.0 * joint_pos - limits_min - limits_max) * (limits_max - limits_min) * (limits_max - limits_min);
     const double denom = 4.0 * min_delta * min_delta * max_delta * max_delta;
+//    const double denom = min_delta * min_delta * max_delta * max_delta;
+
+
 
     partial_values(this->constraint_params_.joint_idx_) = std::abs(denom) > ZERO_THRESHOLD ? nominator / denom : nominator / DIV0_SAFE;
     this->partial_values_ = partial_values;
+
 }
 /* END JointLimitAvoidance **************************************************************************************/
 
@@ -523,6 +527,7 @@ void JointLimitAvoidanceIneq<T_PARAMS, PRIO>::calcValue()
 
     this->last_value_ = this->value_;
     this->value_ = (limit_max - joint_pos) * (joint_pos - limit_min);
+
 }
 
 /// Simple first order differential equation for exponential increase (move away from limit!)
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jsa_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jsa_impl.h
new file mode 100644
index 00000000..1519292d
--- /dev/null
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jsa_impl.h
@@ -0,0 +1,271 @@
+/*!
+ *****************************************************************
+ * \file
+ *
+ * \note
+ *   Copyright (c) 2015 \n
+ *   Fraunhofer Institute for Manufacturing Engineering
+ *   and Automation (IPA) \n\n
+ *
+ *****************************************************************
+ *
+ * \note
+ *   Project name: care-o-bot
+ * \note
+ *   ROS stack name: cob_control
+ * \note
+ *   ROS package name: cob_twist_controller
+ *
+ * \author
+ *   Author: Marco Bezzon, email: Marco.Bezzon@ipa.fraunhofer.de
+ *
+ * \date Date of creation: May, 2015
+ *
+ * \brief
+ *   Implementation of several constraints
+ *
+ ****************************************************************/
+
+#ifndef COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_JSA_IMPL_H
+#define COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_JSA_IMPL_H
+
+#include <sstream>
+#include <string>
+#include <vector>
+#include <limits>
+#include <ros/ros.h>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/pointer_cast.hpp>
+
+#include <kdl/chain.hpp>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <kdl/jntarray.hpp>
+#include "cob_twist_controller/utils/chainjnttojacdotsolver.hpp"
+
+#include "cob_twist_controller/constraints/constraint.h"
+#include "cob_twist_controller/constraints/constraint_params.h"
+
+#include "cob_twist_controller/damping_methods/damping.h"
+#include "cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h"
+
+#include <Eigen/Eigenvalues>
+#include <unsupported/Eigen/NumericalDiff>
+#include <eigen_conversions/eigen_kdl.h>
+
+/* BEGIN JointLimitAvoidance ************************************************************************************/
+template <typename T_PARAMS, typename PRIO>
+Task_t JointSingularityAvoidance<T_PARAMS, PRIO>::createTask()
+{
+    Eigen::MatrixXd cost_func_jac = this->getTaskJacobian();
+    Eigen::VectorXd derivs = this->getTaskDerivatives();
+    Task_t task(this->getPriority(),
+                this->getTaskId(),
+                cost_func_jac,
+                derivs,
+                this->getType());
+
+    task.tcp_ = this->adaptDampingParamsForTask(this->constraint_params_.tc_params_.damping_jla);
+    task.db_ = boost::shared_ptr<DampingBase>(DampingBuilder::createDamping(task.tcp_));
+    return task;
+}
+
+template <typename T_PARAMS, typename PRIO>
+std::string JointSingularityAvoidance<T_PARAMS, PRIO>::getTaskId() const
+{
+    std::ostringstream oss;
+    oss << this->member_inst_cnt_;
+    oss << "_Joint#";
+    oss << this->constraint_params_.joint_idx_;
+    oss << "_";
+    oss << this->priority_;
+    std::string taskid = "JointSingularityAvoidance_" + oss.str();
+    return taskid;
+}
+
+template <typename T_PARAMS, typename PRIO>
+Eigen::MatrixXd JointSingularityAvoidance<T_PARAMS, PRIO>::getTaskJacobian() const
+{
+    return this->partial_values_.transpose();
+}
+
+template <typename T_PARAMS, typename PRIO>
+Eigen::VectorXd JointSingularityAvoidance<T_PARAMS, PRIO>::getTaskDerivatives() const
+{
+    return Eigen::VectorXd::Identity(1, 1) * this->derivative_value_;
+}
+
+template <typename T_PARAMS, typename PRIO>
+void JointSingularityAvoidance<T_PARAMS, PRIO>::calculate()
+{
+    const TwistControllerParams& params = this->constraint_params_.tc_params_;
+    const LimiterParams& limiter_params = this->constraint_params_.limiter_params_;
+    const int32_t joint_idx = this->constraint_params_.joint_idx_;
+    std::vector<double> limits_min = limiter_params.limits_min;
+    std::vector<double> limits_max = limiter_params.limits_max;
+    const double limit_min = limiter_params.limits_min[joint_idx];
+    const double limit_max = limiter_params.limits_max[joint_idx];
+    const double joint_pos = this->joint_states_.current_q_(joint_idx);
+
+    this->abs_delta_max_ = std::abs(limit_max - joint_pos);
+    this->rel_max_ = std::abs(this->abs_delta_max_ / limit_max);
+
+    this->abs_delta_min_ = std::abs(joint_pos - limit_min);
+    this->rel_min_ = std::abs(this->abs_delta_min_ / limit_min);
+
+    const double rel_val = this->rel_max_ < this->rel_min_ ? this->rel_max_ : this->rel_min_;
+
+    this->calcValue();
+    this->calcPartialValues();
+    this->calcDerivativeValue();
+    this->state_.setState(DANGER);  // always active task
+
+    // Compute prediction
+    const double pred_delta_max = std::abs(limit_max - this->jnts_prediction_.q(joint_idx));
+    const double pred_rel_max = std::abs(pred_delta_max / limit_max);
+    const double pred_delta_min = std::abs(this->jnts_prediction_.q(joint_idx) - limit_min);
+    const double pred_rel_min = std::abs(pred_delta_min / limit_min);
+    const double pred_rel_val = pred_rel_max < pred_rel_min ? pred_rel_max : pred_rel_min;
+
+//    if (this->state_.getCurrent() == CRITICAL && pred_rel_val < rel_val)
+//    {
+//        ROS_WARN_STREAM(this->getTaskId() << ": Current JSA state is CRITICAL but prediction is smaller than current rel_val -> Stay in CRIT.");
+//    }
+//    else if (rel_val < 0.1 || pred_rel_val < 0.1)
+//    {
+//        this->state_.setState(CRITICAL);  // always highest task -> avoid HW destruction.
+//    }
+//    else
+//    {
+//        this->state_.setState(DANGER);  // always active task -> avoid HW destruction.
+//    }
+
+}
+
+template <typename T_PARAMS, typename PRIO>
+double JointSingularityAvoidance<T_PARAMS, PRIO>::getActivationGain() const
+{
+    const TwistControllerParams& params = this->constraint_params_.tc_params_;
+    const int32_t joint_idx = this->constraint_params_.joint_idx_;
+    const double joint_pos = this->joint_states_.current_q_(joint_idx);
+
+    double activation_gain = 1.0;
+    return activation_gain;
+}
+
+/// Returns a value for k_H to weight the partial values for e.g. GPM
+template <typename T_PARAMS, typename PRIO>
+double JointSingularityAvoidance<T_PARAMS, PRIO>::getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const
+{
+    const TwistControllerParams& params = this->constraint_params_.tc_params_;
+    double k_H = params.k_H_jsa;
+    return k_H;
+}
+
+template <typename T_PARAMS, typename PRIO>
+ConstraintTypes JointSingularityAvoidance<T_PARAMS, PRIO>::getType() const
+{
+    return JSA;
+}
+
+/// Calculate values of the JSA cost function.
+template <typename T_PARAMS, typename PRIO>
+void JointSingularityAvoidance<T_PARAMS, PRIO>::calcValue()
+{
+    const TwistControllerParams& params = this->constraint_params_.tc_params_;
+    const LimiterParams& limiter_params = this->constraint_params_.limiter_params_;
+    const int32_t joint_idx = this->constraint_params_.joint_idx_;
+    std::vector<double> limits_min = limiter_params.limits_min;
+    std::vector<double> limits_max = limiter_params.limits_max;
+    const double joint_pos = this->joint_states_.current_q_(joint_idx);
+
+//    double nom = (joint_pos - (limits_max[joint_idx] + limits_min[joint_idx])/2);
+//    double denom = (limits_max[joint_idx] - limits_min[joint_idx]);
+    Eigen::MatrixXd jac = this->jacobian_data_;
+    Eigen::MatrixXd prod = jac * jac.transpose();
+    double d = prod.determinant();
+    double mom = 100*std::sqrt(std::abs(d));
+
+    this->last_value_ = this->value_;
+    this->value_ = mom;
+    ROS_WARN_STREAM("manipulability: " << mom);
+
+//    this->last_value_ = this->value_;
+//    this->value_ = std::abs(w) > ZERO_THRESHOLD ? (1/w) : 1 / DIV0_SAFE;
+}
+
+/// Calculate derivative of values.
+template <typename T_PARAMS, typename PRIO>
+void JointSingularityAvoidance<T_PARAMS, PRIO>::calcDerivativeValue()
+{
+    this->derivative_value_ = -0.1 * this->value_;
+}
+
+/// Calculates values of the gradient of the cost function
+template <typename T_PARAMS, typename PRIO>
+void JointSingularityAvoidance<T_PARAMS, PRIO>::calcPartialValues()
+{
+    const TwistControllerParams& params = this->constraint_params_.tc_params_;
+    const LimiterParams& limiter_params = this->constraint_params_.limiter_params_;
+    const double joint_pos = this->joint_states_.current_q_(this->constraint_params_.joint_idx_);
+    const double joint_vel = this->joint_states_.current_q_dot_(this->constraint_params_.joint_idx_);
+    const double limits_min = limiter_params.limits_min[this->constraint_params_.joint_idx_];
+    const double limits_max = limiter_params.limits_max[this->constraint_params_.joint_idx_];
+    const int32_t joint_idx = this->constraint_params_.joint_idx_;
+
+    Eigen::VectorXd partial_values = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
+
+
+    PInvBySVD pinv_calc;
+    KDL::ChainJntToJacDotSolver J_dot_solver(params.chain);
+
+    KDL::Jacobian J_dot(params.chain.getNrOfJoints());
+
+    // JointVelocity predictions
+    KDL::FrameVel frame_vel;
+
+    KDL::JntArrayVel jnts_prediction_chain(params.dof);
+
+    for (unsigned int i = 0; i < params.dof; i++)
+    {
+        jnts_prediction_chain.q(i) = this->jnts_prediction_.q(i);
+        jnts_prediction_chain.qdot(i) = this->jnts_prediction_.qdot(i);
+    }
+    // Calculate prediction for the mainipulator
+    int error = this->fk_solver_vel_.JntToCart(jnts_prediction_chain, frame_vel, joint_idx);
+    if (error != 0)
+    {
+        ROS_ERROR_STREAM("Could not calculate twist for frame: " << joint_idx  << ". Error Code: " << error << " (" << this->fk_solver_vel_.strError(error) << ")");
+        return;
+    }
+
+    J_dot_solver.JntToJacDot(jnts_prediction_chain,J_dot, -1);
+
+    Eigen::MatrixXd jac = this->jacobian_data_;
+
+    Eigen::MatrixXd prod = jac * jac.transpose();
+    double d = prod.determinant();
+    double mom = std::sqrt(std::abs(d));
+    double dmom_dq = 0;
+
+
+
+
+    Eigen::MatrixXd dJ_dq_J_inverse = J_dot.data * pinv_calc.calculate(jac);
+    dmom_dq = -100*mom * dJ_dq_J_inverse.trace();
+
+    ROS_INFO_STREAM("dmom_dq: " << dmom_dq);
+
+//    Eigen::JacobiSVD<Eigen::MatrixXd> svd(dJ_dq_J_inverse, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+
+    partial_values(this->constraint_params_.joint_idx_) = dmom_dq;
+    this->partial_values_ = partial_values;
+    this->jacobian_data_old_ = jac;
+    this->init_ = true;
+}
+/* END JointSingularityAvoidance **************************************************************************************/
+
+#endif  // COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_JSA_IMPL_H
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h
index 65838249..fe480300 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h
@@ -109,6 +109,35 @@ class ConstraintParamsJLA : public ConstraintParamsBase
 };
 /* END ConstraintParamsJLA **************************************************************************************/
 
+
+/* BEGIN ConstraintParamsJSA ************************************************************************************/
+/// Class that represents the parameters for the Singularity Avoidance constraint.
+class ConstraintParamsJSA : public ConstraintParamsBase
+{
+    public:
+        ConstraintParamsJSA(const TwistControllerParams& params,
+                            const LimiterParams& limiter_params,
+                            const std::string& id = std::string()) :
+                ConstraintParamsBase(params, limiter_params, id),
+                joint_idx_(-1)
+        {}
+
+        ConstraintParamsJSA(const ConstraintParamsJLA& cpjsa) :
+                ConstraintParamsBase(cpjsa.tc_params_, cpjsa.limiter_params_, cpjsa.id_),
+                joint_(cpjsa.joint_),
+                joint_idx_(cpjsa.joint_idx_)
+        {}
+
+        virtual ~ConstraintParamsJSA()
+        {}
+
+        std::string joint_;
+        int32_t joint_idx_;
+};
+/* END ConstraintParamsJLA **************************************************************************************/
+
+
+
 typedef boost::shared_ptr<ConstraintParamsBase> ConstraintParamsBase_t;
 
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_PARAMS_H
diff --git a/cob_twist_controller/include/cob_twist_controller/utils/chainjnttojacdotsolver.hpp b/cob_twist_controller/include/cob_twist_controller/utils/chainjnttojacdotsolver.hpp
new file mode 100644
index 00000000..c3c360a4
--- /dev/null
+++ b/cob_twist_controller/include/cob_twist_controller/utils/chainjnttojacdotsolver.hpp
@@ -0,0 +1,176 @@
+/*
+    Computes the Jacobian time derivative
+    Copyright (C) 2015  Antoine Hoarau <hoarau [at] isir.upmc.fr>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+
+#ifndef KDL_CHAINJNTTOJACDOTSOLVER_HPP
+#define KDL_CHAINJNTTOJACDOTSOLVER_HPP
+
+#include <kdl/solveri.hpp>
+#include <kdl/frames.hpp>
+#include <kdl/jntarrayvel.hpp>
+#include <kdl/jacobian.hpp>
+#include <kdl/chain.hpp>
+#include <kdl/framevel.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+
+namespace KDL
+{
+
+/**
+ * @brief Computes the Jacobian time derivative (Jdot) by calculating the
+ * partial derivatives regarding to a joint angle, in the Hybrid, Body-fixed
+ * or Inertial representation.
+ *
+ * This work is based on :
+ * Symbolic differentiation of the velocity mapping for a serial kinematic chain
+ * H. Bruyninckx, J. De Schutter
+ * doi:10.1016/0094-114X(95)00069-B
+ *
+ * url : http://www.sciencedirect.com/science/article/pii/0094114X9500069B
+ */
+class ChainJntToJacDotSolver : public SolverI
+{
+public:
+    static const int E_JAC_DOT_FAILED = -100;
+
+    // Hybrid representation ref Frame: base, ref Point: end-effector
+    static const int HYBRID = 0;
+    // Body-fixed representation ref Frame: end-effector, ref Point: end-effector
+    static const int BODYFIXED = 1;
+    // Intertial representation ref Frame: base, ref Point: base
+    static const int INTERTIAL = 2;
+
+    explicit ChainJntToJacDotSolver(const Chain& chain);
+    virtual ~ChainJntToJacDotSolver();
+    /**
+     * @brief Computes \f$ {}_{bs}\dot{J}^{ee}.\dot{q} \f$
+     *
+     * @param q_in Current joint positions and velocities
+     * @param jac_dot_q_dot The twist representing Jdot*qdot
+     * @param seg_nr The final segment to compute
+     * @return int 0 if no errors happened
+     */
+    virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1);
+    /**
+     * @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$
+     *
+     * @param q_in Current joint positions and velocities
+     * @param jdot The jacobian time derivative in the configured representation
+     *        (HYBRID, BODYFIXED or INERTIAL)
+     * @param seg_nr The final segment to compute
+     * @return int 0 if no errors happened
+     */
+    virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1);
+    int setLockedJoints(const std::vector<bool> locked_joints);
+
+    /**
+     * @brief JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
+     *
+     *
+     * @return void
+     */
+    void setHybridRepresentation(){setRepresentation(HYBRID);}
+    /**
+     * @brief JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector)
+     *
+     * @return void
+     */
+    void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);}
+    /**
+     * @brief JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
+     *
+     * @return void
+     */
+    void setInternialRepresentation(){setRepresentation(INTERTIAL);}
+    /**
+     * @brief Sets the internal variable for the representation (with a check on the value)
+     *
+     * @param representation The representation for Jdot : HYBRID,BODYFIXED or INTERTIAL
+     * @return void
+     */
+    void setRepresentation(const int& representation);
+
+    /// @copydoc KDL::SolverI::strError()
+    virtual const char* strError(const int error) const;
+protected:
+     /**
+     * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
+     *
+     * @param bs_J_ee The Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver)
+     * @param joint_idx The index of the current joint (j in the formula)
+     * @param column_idx The index of the current column (i in the formula)
+     * @return Twist The twist representing dJi/dqj .qdotj
+     */
+    const Twist& getPartialDerivativeHybrid(const Jacobian& bs_J_ee,
+                                     const unsigned int& joint_idx,
+                                     const unsigned int& column_idx);
+     /**
+     * @brief Computes \f$ \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
+     *
+     * @param bs_J_ee The Jacobian expressed in the end effector frame with the end effector as the reference point
+     * @param joint_idx The indice of the current joint (j in the formula)
+     * @param column_idx The indice of the current column (i in the formula)
+     * @return Twist The twist representing dJi/dqj .qdotj
+     */
+    const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
+                                        const unsigned int& joint_idx,
+                                        const unsigned int& column_idx);
+     /**
+     * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \f$
+     *
+     * @param ee_J_ee The Jacobian expressed in the base frame with the base as the reference point
+     * @param joint_idx The indice of the current joint (j in the formula)
+     * @param column_idx The indice of the current column (i in the formula)
+     * @return Twist The twist representing dJi/dqj .qdotj
+     */
+    const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs,
+                                       const unsigned int& joint_idx,
+                                       const unsigned int& column_idx);
+     /**
+     * @brief Computes \f$ \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
+     *
+     * @param bs_J_bs The Jacobian expressed in the base frame with the end effector as the reference point
+     * @param joint_idx The indice of the current joint (j in the formula)
+     * @param column_idx The indice of the current column (i in the formula)
+     * @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj
+     * @return Twist The twist representing dJi/dqj .qdotj
+     */
+    const Twist& getPartialDerivative(const Jacobian& J,
+                               const unsigned int& joint_idx,
+                               const unsigned int& column_idx,
+                               const int& representation);
+private:
+
+    const Chain chain;
+    std::vector<bool> locked_joints_;
+    unsigned int nr_of_unlocked_joints_;
+    ChainJntToJacSolver jac_solver_;
+    Jacobian jac_;
+    Jacobian jac_dot_;
+    int representation_;
+    ChainFkSolverPos_recursive fk_solver_;
+    Frame F_bs_ee_;
+    Twist jac_dot_k_;
+    Twist jac_j_, jac_i_;
+    Twist t_djdq_;
+};
+
+}
+#endif
diff --git a/cob_twist_controller/src/callback_data_mediator.cpp b/cob_twist_controller/src/callback_data_mediator.cpp
index 9ed0ad73..7366012f 100644
--- a/cob_twist_controller/src/callback_data_mediator.cpp
+++ b/cob_twist_controller/src/callback_data_mediator.cpp
@@ -66,6 +66,12 @@ bool CallbackDataMediator::fill(ConstraintParamsJLA& params_jla)
     return true;
 }
 
+/// Can be used to fill parameters for joint singularity avoidance.
+bool CallbackDataMediator::fill(ConstraintParamsJSA& params_jsa)
+{
+    return true;
+}
+
 /// Producer: Fills obstacle distances but only when they are empty
 void CallbackDataMediator::distancesToObstaclesCallback(const cob_control_msgs::ObstacleDistances::ConstPtr& msg)
 {
diff --git a/cob_twist_controller/src/cob_twist_controller.cpp b/cob_twist_controller/src/cob_twist_controller.cpp
index a9f03b9c..c84150f7 100644
--- a/cob_twist_controller/src/cob_twist_controller.cpp
+++ b/cob_twist_controller/src/cob_twist_controller.cpp
@@ -88,6 +88,8 @@ bool CobTwistController::initialize()
         return false;
     }
 
+    twist_controller_params_.chain = chain_;
+
     /// parse robot_description and set velocity limits
     urdf::Model model;
     if (!model.initParam("/robot_description"))
@@ -193,6 +195,7 @@ bool CobTwistController::registerCollisionLinks()
         ROS_INFO_STREAM("Trying to register for " << *it);
         cob_srvs::SetString r;
         r.request.data = *it;
+
         if (register_link_client_.call(r))
         {
             ROS_INFO_STREAM("Called registration service with success: " << int(r.response.success) << ". Got message: " << r.response.message);
@@ -219,12 +222,16 @@ void CobTwistController::reconfigureCallback(cob_twist_controller::TwistControll
     twist_controller_params_.integrator_smoothing = config.integrator_smoothing;
 
     twist_controller_params_.numerical_filtering = config.numerical_filtering;
+    twist_controller_params_.singular_value_damping = config.singular_value_damping;
     twist_controller_params_.damping_method = static_cast<DampingMethodTypes>(config.damping_method);
     twist_controller_params_.damping_factor = config.damping_factor;
     twist_controller_params_.lambda_max = config.lambda_max;
     twist_controller_params_.w_threshold = config.w_threshold;
     twist_controller_params_.beta = config.beta;
     twist_controller_params_.eps_damping = config.eps_damping;
+    twist_controller_params_.damping_delta = config.damping_delta;
+    twist_controller_params_.damping_gain = config.damping_gain;
+    twist_controller_params_.damping_slope = config.damping_slope;
 
     twist_controller_params_.solver = static_cast<SolverTypes>(config.solver);
     twist_controller_params_.priority_main = config.priority;
@@ -240,6 +247,13 @@ void CobTwistController::reconfigureCallback(cob_twist_controller::TwistControll
     twist_controller_params_.thresholds_jla.critical =  critical_jla_in_percent / 100.0;
     twist_controller_params_.damping_jla = config.damping_jla;
 
+    twist_controller_params_.constraint_jsa = static_cast<ConstraintTypesJSA>(config.constraint_jsa);
+    twist_controller_params_.priority_jsa = config.priority_jsa;
+    twist_controller_params_.k_H_jsa = config.k_H_jsa;
+    twist_controller_params_.damping_jsa = config.damping_jsa;
+
+
+
     twist_controller_params_.constraint_ca = static_cast<ConstraintTypesCA>(config.constraint_ca);
     twist_controller_params_.priority_ca = config.priority_ca;
     twist_controller_params_.k_H_ca = config.k_H_ca;
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
index 6f3e8f69..a7e36a26 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
@@ -38,7 +38,8 @@
  * The q_dot_0 results from the sum of the constraint cost function gradients. The terms of the sum are weighted with a factor k_H separately.
  */
 Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_velocities,
-                                                      const JointStates& joint_states)
+                                                      const JointStates& joint_states,
+                                                      bool &active_constraint)
 {
     Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
     Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);
@@ -86,3 +87,4 @@ Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_
 
     return qdots_out;
 }
+
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
index 69b5fce1..f1ba6c39 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
@@ -33,7 +33,8 @@
 #include "cob_twist_controller/task_stack/task_stack_controller.h"
 
 Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
-                                          const JointStates& joint_states)
+                                          const JointStates& joint_states,
+                                          bool &active_constraint)
 {
     this->global_constraint_state_ = NORMAL;
     ros::Time now = ros::Time::now();
@@ -86,10 +87,12 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
     if (CRITICAL == this->global_constraint_state_)
     {
         this->in_cart_vel_damping_ = START_CNT;
+        active_constraint = true;
     }
     else
     {
         this->in_cart_vel_damping_ = this->in_cart_vel_damping_ > 1.0 ? (this->in_cart_vel_damping_ - 1.0) : 1.0;
+        active_constraint = false;
     }
 
     const Vector6d_t scaled_in_cart_velocities = (1.0 / pow(this->in_cart_vel_damping_, 2.0)) * in_cart_velocities;
@@ -104,13 +107,15 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
     {
         Eigen::MatrixXd J_task = it->task_jacobian_;
         Eigen::MatrixXd J_temp = J_task * projector_i;
-        Eigen::VectorXd v_task = it->task_;
+        Eigen::VectorXd v_task = it->task_; // Gradient
+
         Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp);
         q_i = q_i + J_temp_inv * (v_task - J_task * q_i);
         projector_i = projector_i - J_temp_inv * J_temp;
     }
 
     qdots_out.col(0) = q_i + projector_i * sum_of_gradient;
+
     return qdots_out;
 }
 
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
index c0e24509..778d4b9c 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
@@ -35,7 +35,8 @@
  * Maciejewski A., Obstacle Avoidance for Kinematically Redundant Manipulators in Dyn Varying Environments.
  */
 Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,
-                                          const JointStates& joint_states)
+                                          const JointStates& joint_states,
+                                          bool &active_constraint)
 {
     ros::Time now = ros::Time::now();
     double cycle = (now - this->last_time_).toSec();
@@ -99,4 +100,3 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,
     // Eigen::MatrixXd qdots_out = particular_solution + homogeneousSolution; // weighting with k_H is done in loop
     return qdots_out;
 }
-
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
index 4c23edde..a04a431f 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
@@ -36,10 +36,10 @@
  * With the pseudo-inverse the joint velocity vector is calculated.
  */
 Eigen::MatrixXd UnconstraintSolver::solve(const Vector6d_t& in_cart_velocities,
-                                          const JointStates& joint_states)
+                                          const JointStates& joint_states,
+                                          bool &active_constraint)
 {
     Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
     Eigen::MatrixXd qdots_out = pinv * in_cart_velocities;
     return qdots_out;
 }
-
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
index 120b961d..344143e2 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
@@ -35,7 +35,8 @@
  * Uses the base implementation of calculatePinvJacobianBySVD to calculate the pseudo-inverse (weighted) Jacobian.
  */
 Eigen::MatrixXd WeightedLeastNormSolver::solve(const Vector6d_t& in_cart_velocities,
-                                               const JointStates& joint_states)
+                                               const JointStates& joint_states,
+                                               bool &active_constraint)
 {
     Eigen::MatrixXd W_WLN = this->calculateWeighting(joint_states);
     // for the following formulas see Chan paper ISSN 1042-296X [Page 288]
@@ -60,3 +61,4 @@ Eigen::MatrixXd WeightedLeastNormSolver::calculateWeighting(const JointStates& j
     Eigen::VectorXd weighting = Eigen::VectorXd::Ones(cols);
     return weighting.asDiagonal();
 }
+
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp
index f997f063..6434ac22 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp
@@ -69,3 +69,4 @@ Eigen::MatrixXd WLN_JointLimitAvoidanceSolver::calculateWeighting(const JointSta
 
     return weighting.asDiagonal();
 }
+
diff --git a/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp b/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp
index fed4f137..46d75966 100644
--- a/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp
+++ b/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp
@@ -86,7 +86,7 @@ int InverseDifferentialKinematicsSolver::CartToJnt(const JointStates& joint_stat
     /// process result for kinematical extension
     this->kinematic_extension_->processResultExtension(qdot_out_full);
 
-    /// then qdot_out shut be resized to contain only the chain_qdot_out's again
+    /// then qdot_out should be resized to contain only the chain_qdot_out's again
     for (int i = 0; i < jac_chain.columns(); i++)
     {
         qdot_out(i) = qdot_out_full(i);
diff --git a/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp b/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
index 78de5894..d0c6b390 100644
--- a/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
+++ b/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
@@ -68,6 +68,7 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
     Eigen::VectorXd singularValues = svd.singularValues();
     Eigen::VectorXd singularValuesInv = Eigen::VectorXd::Zero(singularValues.rows());
     double lambda = db->getDampingFactor(singularValues, jacobian);
+    Eigen::MatrixXd result;
 
     if (params.numerical_filtering)
     {
@@ -81,6 +82,24 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
         // Formula 20 - additional part - numerical filtering for least singular value m
         uint32_t m = singularValues.rows();
         singularValuesInv(m) = singularValues(m) / (pow(singularValues(m), 2) + pow(params.beta, 2) + pow(lambda, 2));
+
+        result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
+    }
+    else if(params.singular_value_damping)
+    {
+        Eigen::VectorXd lambda_vec = Eigen::VectorXd::Zero(singularValues.size());
+
+        Eigen::MatrixXd S = Eigen::MatrixXd::Zero(singularValues.size(),singularValues.size());
+
+        for(unsigned i = 0; i < singularValues.size(); i++)
+        {
+            lambda_vec(i) = params.damping_gain /( 1+ exp(static_cast <double>(singularValues(i)) + params.damping_delta) / params.damping_slope);
+            S(i,i) = singularValues(i)/(pow(static_cast <double>(singularValues(i)),2) + lambda_vec(i));
+        }
+
+        ROS_INFO_STREAM("S: \n" << S);
+        ROS_INFO_STREAM("lambda: \n" << lambda_vec);
+        result = svd.matrixV() * S * svd.matrixU().transpose();
     }
     else
     {
@@ -98,9 +117,11 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
         //       // damping is disabled due to damping factor lower than a const. limit
         //       singularValues(i) = (singularValues(i) < eps_truncation) ? 0.0 : 1.0 / singularValues(i);
         // }
+
+        result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
     }
 
-    Eigen::MatrixXd result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
+//    Eigen::MatrixXd result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
 
     return result;
 }
diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
index e407af90..808770a5 100644
--- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
+++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
@@ -128,15 +128,44 @@ KDL::Jacobian KinematicExtensionDOF::adjustJacobianDof(const KDL::Jacobian& jac_
     jac_ext(5, 5) = w_z_cb(2) * active_dim.rot_z;
 
     // scale with extension_ratio
-    jac_ext *= params_.extension_ratio;
+    double ext_ratio = params_.extension_ratio;
+//    double ext_ratio;
+//
+//    Eigen::JacobiSVD<Eigen::MatrixXd> svd(jac_chain.data, Eigen::ComputeThinU | Eigen::ComputeThinV);
+//    Eigen::VectorXd least_singular_value_manipulator = svd.singularValues();
+//
+//    // Formula 15 Singularity-robust Task-priority Redundandancy Resolution
+//    double least_singular_value = least_singular_value_manipulator(least_singular_value_manipulator.rows() - 1);
+//
+//    double min_least_singular_value = 0.005;// Find a good value
+//    if (least_singular_value < min_least_singular_value)
+//    {
+//        double lambda_quad = pow(params_.extension_ratio, 2.0);
+//        ext_ratio = sqrt( (1.0 - pow(least_singular_value / min_least_singular_value, 2.0)) * lambda_quad);
+//    }
+//    else
+//    {
+//        ext_ratio = 0.0;
+//    }
+
+    Eigen::MatrixXd prod = jac_chain.data * jac_chain.data.transpose();
+    double d = prod.determinant();
+    double w = std::sqrt(std::abs(d));
+
+    ROS_WARN_STREAM("w: " << w);
+    jac_ext *= ext_ratio;
 
     // combine Jacobian of primary chain and extension
     Matrix6Xd_t jac_full_matrix;
     jac_full_matrix.resize(6, jac_chain.data.cols() + jac_ext.cols());
-    jac_full_matrix << jac_chain.data, jac_ext;
+//    jac_full_matrix << jac_chain.data, jac_ext;
+    jac_full_matrix << jac_chain.data*(1-ext_ratio), jac_ext;
+
     jac_full.resize(jac_chain.data.cols() + jac_ext.cols());
     jac_full.data << jac_full_matrix;
 
+    ROS_INFO_STREAM("\n" << jac_full.data);
+
     return jac_full;
 }
 /* END KinematicExtensionDOF **********************************************************************************************/
@@ -298,6 +327,8 @@ void KinematicExtensionBaseActive::processResultExtension(const KDL::JntArray& q
     base_vel_msg.angular.y = (std::fabs(q_dot_ik(params_.dof+4)) < min_vel_rot_base_) ? 0.0 : q_dot_ik(params_.dof+4);
     base_vel_msg.angular.z = (std::fabs(q_dot_ik(params_.dof+5)) < min_vel_rot_base_) ? 0.0 : q_dot_ik(params_.dof+5);
 
+
+
     base_vel_pub_.publish(base_vel_msg);
 }
 /* END KinematicExtensionBaseActive **********************************************************************************************/
diff --git a/cob_twist_controller/src/twist_controller_config/twist_controller_config.py b/cob_twist_controller/src/twist_controller_config/twist_controller_config.py
index 93131a3d..ba4a61fb 100644
--- a/cob_twist_controller/src/twist_controller_config/twist_controller_config.py
+++ b/cob_twist_controller/src/twist_controller_config/twist_controller_config.py
@@ -35,6 +35,7 @@
 CTRL_IF = 'controller_interface'
 
 NUM_FILT = 'numerical_filtering'
+SING_FILT = 'singular_value_damping'
 DAMP_METHOD = 'damping_method'
 DAMP_FACT = 'damping_factor'
 LAMBDA_MAX = 'lambda_max'
@@ -42,6 +43,9 @@
 BETA = 'beta'
 EPS_DAMP = 'eps_damping'
 EPS_TRUNC = 'eps_truncation'
+DAMP_DELTA = 'damping_delta'
+DAMP_GAIN = 'damping_gain'
+DAMP_SLOPE = 'damping_slope'
 
 SOLVER = 'solver'
 PRIO = 'priority'
@@ -55,6 +59,11 @@
 CRIT_THRESH_JLA = 'critical_threshold_jla'
 DAMP_JLA = 'damping_jla'
 
+CONSTR_JSA = 'constraint_jsa'
+PRIO_JSA = 'priority_jsa'
+K_H_JSA = 'k_H_jsa'
+DAMP_JSA = 'damping_jsa'
+
 CONSTR_CA = 'constraint_ca'
 PRIO_CA = 'priority_ca'
 K_H_CA = 'k_H_ca'
diff --git a/cob_twist_controller/src/utils/chainjnttojacdotsolver.cpp b/cob_twist_controller/src/utils/chainjnttojacdotsolver.cpp
new file mode 100644
index 00000000..85ec19b3
--- /dev/null
+++ b/cob_twist_controller/src/utils/chainjnttojacdotsolver.cpp
@@ -0,0 +1,229 @@
+/*
+    Computes the Jacobian time derivative
+    Copyright (C) 2015  Antoine Hoarau <hoarau [at] isir.upmc.fr>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+
+#include "cob_twist_controller/utils/chainjnttojacdotsolver.hpp"
+
+namespace KDL
+{
+
+ChainJntToJacDotSolver::ChainJntToJacDotSolver(const Chain& _chain):
+    chain(_chain),
+    locked_joints_(chain.getNrOfJoints(),false),
+    nr_of_unlocked_joints_(chain.getNrOfJoints()),
+    jac_solver_(chain),
+    jac_(chain.getNrOfJoints()),
+    jac_dot_(chain.getNrOfJoints()),
+    representation_(HYBRID),
+    fk_solver_(chain)
+{
+}
+
+int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Twist& jac_dot_q_dot, int seg_nr)
+{
+    JntToJacDot(q_in,jac_dot_,seg_nr);
+    MultiplyJacobian(jac_dot_,q_in.qdot,jac_dot_q_dot);
+    return (error = E_NOERROR);
+}
+
+int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, int seg_nr)
+{
+    unsigned int segmentNr;
+    if(seg_nr<0)
+        segmentNr=chain.getNrOfSegments();
+    else
+        segmentNr = seg_nr;
+
+    //Initialize Jacobian to zero since only segmentNr columns are computed
+    SetToZero(jdot) ;
+
+    if(q_in.q.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jdot.columns())
+        return (error = E_JAC_DOT_FAILED);
+    else if(segmentNr>chain.getNrOfSegments())
+        return (error = E_JAC_DOT_FAILED);
+
+    // First compute the jacobian in the Hybrid representation
+    jac_solver_.JntToJac(q_in.q,jac_,segmentNr);
+
+    // Change the reference frame and/or the reference point
+    switch(representation_)
+    {
+        case HYBRID:
+            // Do Nothing as it is the default in KDL;
+            break;
+        case BODYFIXED:
+            // Ref Frame {ee}, Ref Point {ee}
+            fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr);
+            jac_.changeBase(F_bs_ee_.M.Inverse());
+            break;
+        case INTERTIAL:
+            // Ref Frame {bs}, Ref Point {bs}
+            fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr);
+            jac_.changeRefPoint(-F_bs_ee_.p);
+            break;
+        default:
+            return (error = E_JAC_DOT_FAILED);
+    }
+
+    // Let's compute Jdot in the corresponding representation
+    int k=0;
+    for(unsigned int i=0;i<segmentNr;++i)
+    {
+        //Only increase joint nr if the segment has a joint
+        if(chain.getSegment(i).getJoint().getType()!=Joint::None){
+
+            for(unsigned int j=0;j<chain.getNrOfJoints();++j)
+            {
+                // Column J is the sum of all partial derivatives  ref (41)
+                if(!locked_joints_[j])
+                    jac_dot_k_ += getPartialDerivative(jac_,j,k,representation_) * q_in.qdot(j);
+            }
+            jdot.setColumn(k++,jac_dot_k_);
+            SetToZero(jac_dot_k_);
+        }
+    }
+
+    return (error = E_NOERROR);
+}
+
+const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J,
+                                                          const unsigned int& joint_idx,
+                                                          const unsigned int& column_idx,
+                                                          const int& representation)
+{
+    switch(representation)
+    {
+        case HYBRID:
+            return getPartialDerivativeHybrid(J,joint_idx,column_idx);
+        case BODYFIXED:
+            return getPartialDerivativeBodyFixed(J,joint_idx,column_idx);
+        case INTERTIAL:
+            return getPartialDerivativeInertial(J,joint_idx,column_idx);
+        default:
+            SetToZero(this->t_djdq_);
+            return t_djdq_;
+    }
+}
+
+const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacobian& bs_J_ee,
+                                                                const unsigned int& joint_idx,
+                                                                const unsigned int& column_idx)
+{
+    int j=joint_idx;
+    int i=column_idx;
+
+    jac_j_ = bs_J_ee.getColumn(j);
+    jac_i_ = bs_J_ee.getColumn(i);
+
+    SetToZero(t_djdq_);
+
+    if(j < i)
+    {
+        // P_{\Delta}({}_{bs}J^{j})  ref (20)
+        t_djdq_.vel = jac_j_.rot * jac_i_.vel;
+        t_djdq_.rot = jac_j_.rot * jac_i_.rot;
+    }else if(j > i)
+    {
+        // M_{\Delta}({}_{bs}J^{j})  ref (23)
+        SetToZero(t_djdq_.rot);
+        t_djdq_.vel = -jac_j_.vel * jac_i_.rot;
+    }else if(j == i)
+    {
+         // ref (40)
+         SetToZero(t_djdq_.rot);
+         t_djdq_.vel = jac_i_.rot * jac_i_.vel;
+    }
+    return t_djdq_;
+}
+
+const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
+                                                            const unsigned int& joint_idx,
+                                                            const unsigned int& column_idx)
+{
+    int j=joint_idx;
+    int i=column_idx;
+
+    SetToZero(t_djdq_);
+
+    if(j > i)
+    {
+        jac_j_ = ee_J_ee.getColumn(j);
+        jac_i_ = ee_J_ee.getColumn(i);
+
+        // - S_d_(ee_J^j) * ee_J^ee  ref (23)
+        t_djdq_.vel = jac_j_.rot * jac_i_.vel + jac_j_.vel * jac_i_.rot;
+        t_djdq_.rot = jac_j_.rot * jac_i_.rot;
+        t_djdq_ = -t_djdq_;
+    }
+
+    return t_djdq_;
+}
+const Twist& ChainJntToJacDotSolver::getPartialDerivativeInertial(const KDL::Jacobian& bs_J_bs,
+                                                                  const unsigned int& joint_idx,
+                                                                  const unsigned int& column_idx)
+{
+    int j=joint_idx;
+    int i=column_idx;
+
+    SetToZero(t_djdq_);
+
+    if(j < i)
+    {
+        jac_j_ = bs_J_bs.getColumn(j);
+        jac_i_ = bs_J_bs.getColumn(i);
+
+        // S_d_(bs_J^j) * bs_J^bs  ref (23)
+        t_djdq_.vel = jac_j_.rot * jac_i_.vel + jac_j_.vel * jac_i_.rot;
+        t_djdq_.rot = jac_j_.rot * jac_i_.rot;
+    }
+
+    return t_djdq_;
+}
+void ChainJntToJacDotSolver::setRepresentation(const int& representation)
+{
+    if(representation == HYBRID ||
+        representation == BODYFIXED ||
+        representation == INTERTIAL)
+    this->representation_ = representation;
+}
+
+
+int ChainJntToJacDotSolver::setLockedJoints(const std::vector< bool > locked_joints)
+{
+    if(locked_joints.size()!=locked_joints_.size())
+        return -1;
+    locked_joints_=locked_joints;
+    nr_of_unlocked_joints_=0;
+    for(unsigned int i=0;i<locked_joints_.size();i++){
+        if(!locked_joints_[i])
+            nr_of_unlocked_joints_++;
+    }
+
+    return 0;
+}
+const char* ChainJntToJacDotSolver::strError(const int error) const
+{
+    if (E_JAC_DOT_FAILED == error) return "Jac Dot Failed";
+    else return SolverI::strError(error);
+}
+
+ChainJntToJacDotSolver::~ChainJntToJacDotSolver()
+{
+}
+}  // namespace KDL

From b6df2ff4330dba230bf9d3a5bfe21f1f82773043 Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Tue, 26 Jul 2016 14:36:22 +0200
Subject: [PATCH 05/11] Removed obsolete arguments

---
 .../constraint_solvers/factories/solver_factory.h            | 4 +---
 .../constraint_solvers/solvers/constraint_solver_base.h      | 3 +--
 .../solvers/gradient_projection_method_solver.h              | 3 +--
 .../constraint_solvers/solvers/stack_of_tasks_solver.h       | 3 +--
 .../constraint_solvers/solvers/task_priority_solver.h        | 3 +--
 .../constraint_solvers/solvers/unconstraint_solver.h         | 3 +--
 .../constraint_solvers/solvers/weighted_least_norm_solver.h  | 3 +--
 .../solvers/gradient_projection_method_solver.cpp            | 3 +--
 .../src/constraint_solvers/solvers/stack_of_tasks_solver.cpp | 5 +----
 .../src/constraint_solvers/solvers/task_priority_solver.cpp  | 3 +--
 .../src/constraint_solvers/solvers/unconstraint_solver.cpp   | 3 +--
 .../solvers/weighted_least_norm_solver.cpp                   | 3 +--
 12 files changed, 12 insertions(+), 27 deletions(-)

diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h
index 95b3fbcc..6f25ae38 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h
@@ -90,15 +90,13 @@ class SolverFactory : public ISolverFactory
             constraint_solver_->setConstraints(constraints);
             constraint_solver_->setDamping(damping_method);
 
-            bool active_constraint = false;
-            Eigen::MatrixXd new_q_dot = constraint_solver_->solve(in_cart_velocities, joint_states, active_constraint);
+            Eigen::MatrixXd new_q_dot = constraint_solver_->solve(in_cart_velocities, joint_states);
 
             return new_q_dot;
         }
 
     private:
         boost::shared_ptr<T> constraint_solver_;
-        JointStates joint_states_backup_;
 };
 
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINT_SOLVERS_FACTORIES_SOLVER_FACTORY_H
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h
index 811aed11..5748bcd3 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h
@@ -52,8 +52,7 @@ class ConstraintSolver
          * @return The calculated new joint velocities.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states,
-                                      bool &active_constraint) = 0;
+                                      const JointStates& joint_states) = 0;
 
         /**
          * Inline method to set the damping
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h
index 0a90762f..f965ef96 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h
@@ -56,8 +56,7 @@ class GradientProjectionMethodSolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states,
-                                      bool &active_constraint);
+                                      const JointStates& joint_states);
 
 };
 
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/stack_of_tasks_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/stack_of_tasks_solver.h
index 382d3fcd..cec0e4da 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/stack_of_tasks_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/stack_of_tasks_solver.h
@@ -62,8 +62,7 @@ class StackOfTasksSolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states,
-                                      bool &active_constraint);
+                                      const JointStates& joint_states);
 
         /**
          * Process the state of the constraint and update the sum_of_gradient.
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h
index 3f73327f..2d0ce190 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h
@@ -59,8 +59,7 @@ class TaskPrioritySolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states,
-                                      bool &active_constraint);
+                                      const JointStates& joint_states);
 
     protected:
         ros::Time last_time_;
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unconstraint_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unconstraint_solver.h
index 9e63e4fc..4c1ca105 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unconstraint_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/unconstraint_solver.h
@@ -50,8 +50,7 @@ class UnconstraintSolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states,
-                                      bool &active_constraint);
+                                      const JointStates& joint_states);
 };
 
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINT_SOLVERS_SOLVERS_UNCONSTRAINT_SOLVER_H
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/weighted_least_norm_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/weighted_least_norm_solver.h
index aeb883ca..bbe7a117 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/weighted_least_norm_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/weighted_least_norm_solver.h
@@ -52,8 +52,7 @@ class WeightedLeastNormSolver : public ConstraintSolver<>
          * See base class ConstraintSolver for more details on params and returns.
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
-                                      const JointStates& joint_states,
-                                      bool &active_constraint);
+                                      const JointStates& joint_states);
 
     private:
         /**
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
index a7e36a26..b10c1d2b 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
@@ -38,8 +38,7 @@
  * The q_dot_0 results from the sum of the constraint cost function gradients. The terms of the sum are weighted with a factor k_H separately.
  */
 Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_velocities,
-                                                      const JointStates& joint_states,
-                                                      bool &active_constraint)
+                                                      const JointStates& joint_states)
 {
     Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
     Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
index f1ba6c39..76183050 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
@@ -33,8 +33,7 @@
 #include "cob_twist_controller/task_stack/task_stack_controller.h"
 
 Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
-                                          const JointStates& joint_states,
-                                          bool &active_constraint)
+                                          const JointStates& joint_states)
 {
     this->global_constraint_state_ = NORMAL;
     ros::Time now = ros::Time::now();
@@ -87,12 +86,10 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
     if (CRITICAL == this->global_constraint_state_)
     {
         this->in_cart_vel_damping_ = START_CNT;
-        active_constraint = true;
     }
     else
     {
         this->in_cart_vel_damping_ = this->in_cart_vel_damping_ > 1.0 ? (this->in_cart_vel_damping_ - 1.0) : 1.0;
-        active_constraint = false;
     }
 
     const Vector6d_t scaled_in_cart_velocities = (1.0 / pow(this->in_cart_vel_damping_, 2.0)) * in_cart_velocities;
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
index 778d4b9c..fa991633 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
@@ -35,8 +35,7 @@
  * Maciejewski A., Obstacle Avoidance for Kinematically Redundant Manipulators in Dyn Varying Environments.
  */
 Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,
-                                          const JointStates& joint_states,
-                                          bool &active_constraint)
+                                          const JointStates& joint_states)
 {
     ros::Time now = ros::Time::now();
     double cycle = (now - this->last_time_).toSec();
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
index a04a431f..b7c9f064 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
@@ -36,8 +36,7 @@
  * With the pseudo-inverse the joint velocity vector is calculated.
  */
 Eigen::MatrixXd UnconstraintSolver::solve(const Vector6d_t& in_cart_velocities,
-                                          const JointStates& joint_states,
-                                          bool &active_constraint)
+                                          const JointStates& joint_states)
 {
     Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
     Eigen::MatrixXd qdots_out = pinv * in_cart_velocities;
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
index 344143e2..560dd01a 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
@@ -35,8 +35,7 @@
  * Uses the base implementation of calculatePinvJacobianBySVD to calculate the pseudo-inverse (weighted) Jacobian.
  */
 Eigen::MatrixXd WeightedLeastNormSolver::solve(const Vector6d_t& in_cart_velocities,
-                                               const JointStates& joint_states,
-                                               bool &active_constraint)
+                                               const JointStates& joint_states)
 {
     Eigen::MatrixXd W_WLN = this->calculateWeighting(joint_states);
     // for the following formulas see Chan paper ISSN 1042-296X [Page 288]

From 3e6257a7b4526876356db429a2d9f6e57559eb4e Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Tue, 26 Jul 2016 14:58:28 +0200
Subject: [PATCH 06/11] Deleted obsolete blank spaces and variables.

---
 .../factories/solver_factory.h                |  3 --
 .../gradient_projection_method_solver.h       |  2 --
 .../constraints/constraint_ca_impl.h          |  2 ++
 .../constraints/constraint_jla_impl.h         |  5 ---
 .../src/cob_twist_controller.cpp              |  2 --
 .../gradient_projection_method_solver.cpp     |  1 -
 .../solvers/stack_of_tasks_solver.cpp         |  4 +--
 .../solvers/task_priority_solver.cpp          |  1 +
 .../solvers/unconstraint_solver.cpp           |  1 +
 .../solvers/weighted_least_norm_solver.cpp    |  1 -
 .../wln_joint_limit_avoidance_solver.cpp      |  1 -
 .../kinematic_extension_dof.cpp               | 35 ++-----------------
 12 files changed, 7 insertions(+), 51 deletions(-)

diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h
index 6f25ae38..b2133a01 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/factories/solver_factory.h
@@ -33,7 +33,6 @@
 #include <Eigen/Core>
 #include <Eigen/SVD>
 #include <kdl/jntarray.hpp>
-#include <eigen_conversions/eigen_kdl.h>
 
 #include "cob_twist_controller/damping_methods/damping_base.h"
 #include "cob_twist_controller/constraints/constraint_base.h"
@@ -89,9 +88,7 @@ class SolverFactory : public ISolverFactory
             constraint_solver_->setJacobianData(jacobian_data);
             constraint_solver_->setConstraints(constraints);
             constraint_solver_->setDamping(damping_method);
-
             Eigen::MatrixXd new_q_dot = constraint_solver_->solve(in_cart_velocities, joint_states);
-
             return new_q_dot;
         }
 
diff --git a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h
index f965ef96..8ad8d3d4 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h
@@ -57,8 +57,6 @@ class GradientProjectionMethodSolver : public ConstraintSolver<>
          */
         virtual Eigen::MatrixXd solve(const Vector6d_t& in_cart_velocities,
                                       const JointStates& joint_states);
-
 };
 
-
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINT_SOLVERS_SOLVERS_GRADIENT_PROJECTION_METHOD_SOLVER_H
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
index 524d06ff..bf187ade 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
@@ -408,6 +408,7 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
                 jnts_prediction_chain.qdot(i) = this->jnts_prediction_.qdot(i);
             }
 
+
 //             ROS_INFO_STREAM("jnts_prediction_chain.q.rows: " << jnts_prediction_chain.q.rows());
 
             // Calculate prediction for the mainipulator
@@ -442,6 +443,7 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
                 predicted_twist_odometry.rot.data[2] = 0;
             }
 
+
             KDL::Twist twist = frame_vel.GetTwist() + predicted_twist_odometry;  // predicted frame twist
 
             Eigen::Vector3d pred_twist_vel;
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h
index 7ade5039..48104d5c 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jla_impl.h
@@ -224,13 +224,9 @@ void JointLimitAvoidance<T_PARAMS, PRIO>::calcPartialValues()
     const double max_delta = (limits_max - joint_pos);
     const double nominator = (2.0 * joint_pos - limits_min - limits_max) * (limits_max - limits_min) * (limits_max - limits_min);
     const double denom = 4.0 * min_delta * min_delta * max_delta * max_delta;
-//    const double denom = min_delta * min_delta * max_delta * max_delta;
-
-
 
     partial_values(this->constraint_params_.joint_idx_) = std::abs(denom) > ZERO_THRESHOLD ? nominator / denom : nominator / DIV0_SAFE;
     this->partial_values_ = partial_values;
-
 }
 /* END JointLimitAvoidance **************************************************************************************/
 
@@ -527,7 +523,6 @@ void JointLimitAvoidanceIneq<T_PARAMS, PRIO>::calcValue()
 
     this->last_value_ = this->value_;
     this->value_ = (limit_max - joint_pos) * (joint_pos - limit_min);
-
 }
 
 /// Simple first order differential equation for exponential increase (move away from limit!)
diff --git a/cob_twist_controller/src/cob_twist_controller.cpp b/cob_twist_controller/src/cob_twist_controller.cpp
index c84150f7..e0f31d49 100644
--- a/cob_twist_controller/src/cob_twist_controller.cpp
+++ b/cob_twist_controller/src/cob_twist_controller.cpp
@@ -195,7 +195,6 @@ bool CobTwistController::registerCollisionLinks()
         ROS_INFO_STREAM("Trying to register for " << *it);
         cob_srvs::SetString r;
         r.request.data = *it;
-
         if (register_link_client_.call(r))
         {
             ROS_INFO_STREAM("Called registration service with success: " << int(r.response.success) << ". Got message: " << r.response.message);
@@ -253,7 +252,6 @@ void CobTwistController::reconfigureCallback(cob_twist_controller::TwistControll
     twist_controller_params_.damping_jsa = config.damping_jsa;
 
 
-
     twist_controller_params_.constraint_ca = static_cast<ConstraintTypesCA>(config.constraint_ca);
     twist_controller_params_.priority_ca = config.priority_ca;
     twist_controller_params_.k_H_ca = config.k_H_ca;
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
index b10c1d2b..6f3e8f69 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
@@ -86,4 +86,3 @@ Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_
 
     return qdots_out;
 }
-
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
index 76183050..69b5fce1 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
@@ -104,15 +104,13 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
     {
         Eigen::MatrixXd J_task = it->task_jacobian_;
         Eigen::MatrixXd J_temp = J_task * projector_i;
-        Eigen::VectorXd v_task = it->task_; // Gradient
-
+        Eigen::VectorXd v_task = it->task_;
         Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp);
         q_i = q_i + J_temp_inv * (v_task - J_task * q_i);
         projector_i = projector_i - J_temp_inv * J_temp;
     }
 
     qdots_out.col(0) = q_i + projector_i * sum_of_gradient;
-
     return qdots_out;
 }
 
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
index fa991633..c0e24509 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
@@ -99,3 +99,4 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,
     // Eigen::MatrixXd qdots_out = particular_solution + homogeneousSolution; // weighting with k_H is done in loop
     return qdots_out;
 }
+
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
index b7c9f064..4c23edde 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
@@ -42,3 +42,4 @@ Eigen::MatrixXd UnconstraintSolver::solve(const Vector6d_t& in_cart_velocities,
     Eigen::MatrixXd qdots_out = pinv * in_cart_velocities;
     return qdots_out;
 }
+
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
index 560dd01a..120b961d 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
@@ -60,4 +60,3 @@ Eigen::MatrixXd WeightedLeastNormSolver::calculateWeighting(const JointStates& j
     Eigen::VectorXd weighting = Eigen::VectorXd::Ones(cols);
     return weighting.asDiagonal();
 }
-
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp
index 6434ac22..f997f063 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.cpp
@@ -69,4 +69,3 @@ Eigen::MatrixXd WLN_JointLimitAvoidanceSolver::calculateWeighting(const JointSta
 
     return weighting.asDiagonal();
 }
-
diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
index 808770a5..e407af90 100644
--- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
+++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
@@ -128,44 +128,15 @@ KDL::Jacobian KinematicExtensionDOF::adjustJacobianDof(const KDL::Jacobian& jac_
     jac_ext(5, 5) = w_z_cb(2) * active_dim.rot_z;
 
     // scale with extension_ratio
-    double ext_ratio = params_.extension_ratio;
-//    double ext_ratio;
-//
-//    Eigen::JacobiSVD<Eigen::MatrixXd> svd(jac_chain.data, Eigen::ComputeThinU | Eigen::ComputeThinV);
-//    Eigen::VectorXd least_singular_value_manipulator = svd.singularValues();
-//
-//    // Formula 15 Singularity-robust Task-priority Redundandancy Resolution
-//    double least_singular_value = least_singular_value_manipulator(least_singular_value_manipulator.rows() - 1);
-//
-//    double min_least_singular_value = 0.005;// Find a good value
-//    if (least_singular_value < min_least_singular_value)
-//    {
-//        double lambda_quad = pow(params_.extension_ratio, 2.0);
-//        ext_ratio = sqrt( (1.0 - pow(least_singular_value / min_least_singular_value, 2.0)) * lambda_quad);
-//    }
-//    else
-//    {
-//        ext_ratio = 0.0;
-//    }
-
-    Eigen::MatrixXd prod = jac_chain.data * jac_chain.data.transpose();
-    double d = prod.determinant();
-    double w = std::sqrt(std::abs(d));
-
-    ROS_WARN_STREAM("w: " << w);
-    jac_ext *= ext_ratio;
+    jac_ext *= params_.extension_ratio;
 
     // combine Jacobian of primary chain and extension
     Matrix6Xd_t jac_full_matrix;
     jac_full_matrix.resize(6, jac_chain.data.cols() + jac_ext.cols());
-//    jac_full_matrix << jac_chain.data, jac_ext;
-    jac_full_matrix << jac_chain.data*(1-ext_ratio), jac_ext;
-
+    jac_full_matrix << jac_chain.data, jac_ext;
     jac_full.resize(jac_chain.data.cols() + jac_ext.cols());
     jac_full.data << jac_full_matrix;
 
-    ROS_INFO_STREAM("\n" << jac_full.data);
-
     return jac_full;
 }
 /* END KinematicExtensionDOF **********************************************************************************************/
@@ -327,8 +298,6 @@ void KinematicExtensionBaseActive::processResultExtension(const KDL::JntArray& q
     base_vel_msg.angular.y = (std::fabs(q_dot_ik(params_.dof+4)) < min_vel_rot_base_) ? 0.0 : q_dot_ik(params_.dof+4);
     base_vel_msg.angular.z = (std::fabs(q_dot_ik(params_.dof+5)) < min_vel_rot_base_) ? 0.0 : q_dot_ik(params_.dof+5);
 
-
-
     base_vel_pub_.publish(base_vel_msg);
 }
 /* END KinematicExtensionBaseActive **********************************************************************************************/

From 44979e360acc81b8900fdc2a643828f8e03513b0 Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Thu, 28 Jul 2016 13:53:49 +0200
Subject: [PATCH 07/11] removed unreleased orocos files

---
 .../utils/chainjnttojacdotsolver.hpp          | 176 --------------
 .../src/utils/chainjnttojacdotsolver.cpp      | 229 ------------------
 2 files changed, 405 deletions(-)
 delete mode 100644 cob_twist_controller/include/cob_twist_controller/utils/chainjnttojacdotsolver.hpp
 delete mode 100644 cob_twist_controller/src/utils/chainjnttojacdotsolver.cpp

diff --git a/cob_twist_controller/include/cob_twist_controller/utils/chainjnttojacdotsolver.hpp b/cob_twist_controller/include/cob_twist_controller/utils/chainjnttojacdotsolver.hpp
deleted file mode 100644
index c3c360a4..00000000
--- a/cob_twist_controller/include/cob_twist_controller/utils/chainjnttojacdotsolver.hpp
+++ /dev/null
@@ -1,176 +0,0 @@
-/*
-    Computes the Jacobian time derivative
-    Copyright (C) 2015  Antoine Hoarau <hoarau [at] isir.upmc.fr>
-
-    This library is free software; you can redistribute it and/or
-    modify it under the terms of the GNU Lesser General Public
-    License as published by the Free Software Foundation; either
-    version 2.1 of the License, or (at your option) any later version.
-
-    This library is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-    Lesser General Public License for more details.
-
-    You should have received a copy of the GNU Lesser General Public
-    License along with this library; if not, write to the Free Software
-    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-*/
-
-
-#ifndef KDL_CHAINJNTTOJACDOTSOLVER_HPP
-#define KDL_CHAINJNTTOJACDOTSOLVER_HPP
-
-#include <kdl/solveri.hpp>
-#include <kdl/frames.hpp>
-#include <kdl/jntarrayvel.hpp>
-#include <kdl/jacobian.hpp>
-#include <kdl/chain.hpp>
-#include <kdl/framevel.hpp>
-#include <kdl/chainjnttojacsolver.hpp>
-#include <kdl/chainfksolverpos_recursive.hpp>
-
-namespace KDL
-{
-
-/**
- * @brief Computes the Jacobian time derivative (Jdot) by calculating the
- * partial derivatives regarding to a joint angle, in the Hybrid, Body-fixed
- * or Inertial representation.
- *
- * This work is based on :
- * Symbolic differentiation of the velocity mapping for a serial kinematic chain
- * H. Bruyninckx, J. De Schutter
- * doi:10.1016/0094-114X(95)00069-B
- *
- * url : http://www.sciencedirect.com/science/article/pii/0094114X9500069B
- */
-class ChainJntToJacDotSolver : public SolverI
-{
-public:
-    static const int E_JAC_DOT_FAILED = -100;
-
-    // Hybrid representation ref Frame: base, ref Point: end-effector
-    static const int HYBRID = 0;
-    // Body-fixed representation ref Frame: end-effector, ref Point: end-effector
-    static const int BODYFIXED = 1;
-    // Intertial representation ref Frame: base, ref Point: base
-    static const int INTERTIAL = 2;
-
-    explicit ChainJntToJacDotSolver(const Chain& chain);
-    virtual ~ChainJntToJacDotSolver();
-    /**
-     * @brief Computes \f$ {}_{bs}\dot{J}^{ee}.\dot{q} \f$
-     *
-     * @param q_in Current joint positions and velocities
-     * @param jac_dot_q_dot The twist representing Jdot*qdot
-     * @param seg_nr The final segment to compute
-     * @return int 0 if no errors happened
-     */
-    virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1);
-    /**
-     * @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$
-     *
-     * @param q_in Current joint positions and velocities
-     * @param jdot The jacobian time derivative in the configured representation
-     *        (HYBRID, BODYFIXED or INERTIAL)
-     * @param seg_nr The final segment to compute
-     * @return int 0 if no errors happened
-     */
-    virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1);
-    int setLockedJoints(const std::vector<bool> locked_joints);
-
-    /**
-     * @brief JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
-     *
-     *
-     * @return void
-     */
-    void setHybridRepresentation(){setRepresentation(HYBRID);}
-    /**
-     * @brief JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector)
-     *
-     * @return void
-     */
-    void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);}
-    /**
-     * @brief JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
-     *
-     * @return void
-     */
-    void setInternialRepresentation(){setRepresentation(INTERTIAL);}
-    /**
-     * @brief Sets the internal variable for the representation (with a check on the value)
-     *
-     * @param representation The representation for Jdot : HYBRID,BODYFIXED or INTERTIAL
-     * @return void
-     */
-    void setRepresentation(const int& representation);
-
-    /// @copydoc KDL::SolverI::strError()
-    virtual const char* strError(const int error) const;
-protected:
-     /**
-     * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
-     *
-     * @param bs_J_ee The Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver)
-     * @param joint_idx The index of the current joint (j in the formula)
-     * @param column_idx The index of the current column (i in the formula)
-     * @return Twist The twist representing dJi/dqj .qdotj
-     */
-    const Twist& getPartialDerivativeHybrid(const Jacobian& bs_J_ee,
-                                     const unsigned int& joint_idx,
-                                     const unsigned int& column_idx);
-     /**
-     * @brief Computes \f$ \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
-     *
-     * @param bs_J_ee The Jacobian expressed in the end effector frame with the end effector as the reference point
-     * @param joint_idx The indice of the current joint (j in the formula)
-     * @param column_idx The indice of the current column (i in the formula)
-     * @return Twist The twist representing dJi/dqj .qdotj
-     */
-    const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
-                                        const unsigned int& joint_idx,
-                                        const unsigned int& column_idx);
-     /**
-     * @brief Computes \f$ \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \f$
-     *
-     * @param ee_J_ee The Jacobian expressed in the base frame with the base as the reference point
-     * @param joint_idx The indice of the current joint (j in the formula)
-     * @param column_idx The indice of the current column (i in the formula)
-     * @return Twist The twist representing dJi/dqj .qdotj
-     */
-    const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs,
-                                       const unsigned int& joint_idx,
-                                       const unsigned int& column_idx);
-     /**
-     * @brief Computes \f$ \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
-     *
-     * @param bs_J_bs The Jacobian expressed in the base frame with the end effector as the reference point
-     * @param joint_idx The indice of the current joint (j in the formula)
-     * @param column_idx The indice of the current column (i in the formula)
-     * @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj
-     * @return Twist The twist representing dJi/dqj .qdotj
-     */
-    const Twist& getPartialDerivative(const Jacobian& J,
-                               const unsigned int& joint_idx,
-                               const unsigned int& column_idx,
-                               const int& representation);
-private:
-
-    const Chain chain;
-    std::vector<bool> locked_joints_;
-    unsigned int nr_of_unlocked_joints_;
-    ChainJntToJacSolver jac_solver_;
-    Jacobian jac_;
-    Jacobian jac_dot_;
-    int representation_;
-    ChainFkSolverPos_recursive fk_solver_;
-    Frame F_bs_ee_;
-    Twist jac_dot_k_;
-    Twist jac_j_, jac_i_;
-    Twist t_djdq_;
-};
-
-}
-#endif
diff --git a/cob_twist_controller/src/utils/chainjnttojacdotsolver.cpp b/cob_twist_controller/src/utils/chainjnttojacdotsolver.cpp
deleted file mode 100644
index 85ec19b3..00000000
--- a/cob_twist_controller/src/utils/chainjnttojacdotsolver.cpp
+++ /dev/null
@@ -1,229 +0,0 @@
-/*
-    Computes the Jacobian time derivative
-    Copyright (C) 2015  Antoine Hoarau <hoarau [at] isir.upmc.fr>
-
-    This library is free software; you can redistribute it and/or
-    modify it under the terms of the GNU Lesser General Public
-    License as published by the Free Software Foundation; either
-    version 2.1 of the License, or (at your option) any later version.
-
-    This library is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-    Lesser General Public License for more details.
-
-    You should have received a copy of the GNU Lesser General Public
-    License along with this library; if not, write to the Free Software
-    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-*/
-
-
-#include "cob_twist_controller/utils/chainjnttojacdotsolver.hpp"
-
-namespace KDL
-{
-
-ChainJntToJacDotSolver::ChainJntToJacDotSolver(const Chain& _chain):
-    chain(_chain),
-    locked_joints_(chain.getNrOfJoints(),false),
-    nr_of_unlocked_joints_(chain.getNrOfJoints()),
-    jac_solver_(chain),
-    jac_(chain.getNrOfJoints()),
-    jac_dot_(chain.getNrOfJoints()),
-    representation_(HYBRID),
-    fk_solver_(chain)
-{
-}
-
-int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Twist& jac_dot_q_dot, int seg_nr)
-{
-    JntToJacDot(q_in,jac_dot_,seg_nr);
-    MultiplyJacobian(jac_dot_,q_in.qdot,jac_dot_q_dot);
-    return (error = E_NOERROR);
-}
-
-int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, int seg_nr)
-{
-    unsigned int segmentNr;
-    if(seg_nr<0)
-        segmentNr=chain.getNrOfSegments();
-    else
-        segmentNr = seg_nr;
-
-    //Initialize Jacobian to zero since only segmentNr columns are computed
-    SetToZero(jdot) ;
-
-    if(q_in.q.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jdot.columns())
-        return (error = E_JAC_DOT_FAILED);
-    else if(segmentNr>chain.getNrOfSegments())
-        return (error = E_JAC_DOT_FAILED);
-
-    // First compute the jacobian in the Hybrid representation
-    jac_solver_.JntToJac(q_in.q,jac_,segmentNr);
-
-    // Change the reference frame and/or the reference point
-    switch(representation_)
-    {
-        case HYBRID:
-            // Do Nothing as it is the default in KDL;
-            break;
-        case BODYFIXED:
-            // Ref Frame {ee}, Ref Point {ee}
-            fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr);
-            jac_.changeBase(F_bs_ee_.M.Inverse());
-            break;
-        case INTERTIAL:
-            // Ref Frame {bs}, Ref Point {bs}
-            fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr);
-            jac_.changeRefPoint(-F_bs_ee_.p);
-            break;
-        default:
-            return (error = E_JAC_DOT_FAILED);
-    }
-
-    // Let's compute Jdot in the corresponding representation
-    int k=0;
-    for(unsigned int i=0;i<segmentNr;++i)
-    {
-        //Only increase joint nr if the segment has a joint
-        if(chain.getSegment(i).getJoint().getType()!=Joint::None){
-
-            for(unsigned int j=0;j<chain.getNrOfJoints();++j)
-            {
-                // Column J is the sum of all partial derivatives  ref (41)
-                if(!locked_joints_[j])
-                    jac_dot_k_ += getPartialDerivative(jac_,j,k,representation_) * q_in.qdot(j);
-            }
-            jdot.setColumn(k++,jac_dot_k_);
-            SetToZero(jac_dot_k_);
-        }
-    }
-
-    return (error = E_NOERROR);
-}
-
-const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J,
-                                                          const unsigned int& joint_idx,
-                                                          const unsigned int& column_idx,
-                                                          const int& representation)
-{
-    switch(representation)
-    {
-        case HYBRID:
-            return getPartialDerivativeHybrid(J,joint_idx,column_idx);
-        case BODYFIXED:
-            return getPartialDerivativeBodyFixed(J,joint_idx,column_idx);
-        case INTERTIAL:
-            return getPartialDerivativeInertial(J,joint_idx,column_idx);
-        default:
-            SetToZero(this->t_djdq_);
-            return t_djdq_;
-    }
-}
-
-const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacobian& bs_J_ee,
-                                                                const unsigned int& joint_idx,
-                                                                const unsigned int& column_idx)
-{
-    int j=joint_idx;
-    int i=column_idx;
-
-    jac_j_ = bs_J_ee.getColumn(j);
-    jac_i_ = bs_J_ee.getColumn(i);
-
-    SetToZero(t_djdq_);
-
-    if(j < i)
-    {
-        // P_{\Delta}({}_{bs}J^{j})  ref (20)
-        t_djdq_.vel = jac_j_.rot * jac_i_.vel;
-        t_djdq_.rot = jac_j_.rot * jac_i_.rot;
-    }else if(j > i)
-    {
-        // M_{\Delta}({}_{bs}J^{j})  ref (23)
-        SetToZero(t_djdq_.rot);
-        t_djdq_.vel = -jac_j_.vel * jac_i_.rot;
-    }else if(j == i)
-    {
-         // ref (40)
-         SetToZero(t_djdq_.rot);
-         t_djdq_.vel = jac_i_.rot * jac_i_.vel;
-    }
-    return t_djdq_;
-}
-
-const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
-                                                            const unsigned int& joint_idx,
-                                                            const unsigned int& column_idx)
-{
-    int j=joint_idx;
-    int i=column_idx;
-
-    SetToZero(t_djdq_);
-
-    if(j > i)
-    {
-        jac_j_ = ee_J_ee.getColumn(j);
-        jac_i_ = ee_J_ee.getColumn(i);
-
-        // - S_d_(ee_J^j) * ee_J^ee  ref (23)
-        t_djdq_.vel = jac_j_.rot * jac_i_.vel + jac_j_.vel * jac_i_.rot;
-        t_djdq_.rot = jac_j_.rot * jac_i_.rot;
-        t_djdq_ = -t_djdq_;
-    }
-
-    return t_djdq_;
-}
-const Twist& ChainJntToJacDotSolver::getPartialDerivativeInertial(const KDL::Jacobian& bs_J_bs,
-                                                                  const unsigned int& joint_idx,
-                                                                  const unsigned int& column_idx)
-{
-    int j=joint_idx;
-    int i=column_idx;
-
-    SetToZero(t_djdq_);
-
-    if(j < i)
-    {
-        jac_j_ = bs_J_bs.getColumn(j);
-        jac_i_ = bs_J_bs.getColumn(i);
-
-        // S_d_(bs_J^j) * bs_J^bs  ref (23)
-        t_djdq_.vel = jac_j_.rot * jac_i_.vel + jac_j_.vel * jac_i_.rot;
-        t_djdq_.rot = jac_j_.rot * jac_i_.rot;
-    }
-
-    return t_djdq_;
-}
-void ChainJntToJacDotSolver::setRepresentation(const int& representation)
-{
-    if(representation == HYBRID ||
-        representation == BODYFIXED ||
-        representation == INTERTIAL)
-    this->representation_ = representation;
-}
-
-
-int ChainJntToJacDotSolver::setLockedJoints(const std::vector< bool > locked_joints)
-{
-    if(locked_joints.size()!=locked_joints_.size())
-        return -1;
-    locked_joints_=locked_joints;
-    nr_of_unlocked_joints_=0;
-    for(unsigned int i=0;i<locked_joints_.size();i++){
-        if(!locked_joints_[i])
-            nr_of_unlocked_joints_++;
-    }
-
-    return 0;
-}
-const char* ChainJntToJacDotSolver::strError(const int error) const
-{
-    if (E_JAC_DOT_FAILED == error) return "Jac Dot Failed";
-    else return SolverI::strError(error);
-}
-
-ChainJntToJacDotSolver::~ChainJntToJacDotSolver()
-{
-}
-}  // namespace KDL

From 3b82a04af74c39b98ec3f96851bdde285eb10078 Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Thu, 28 Jul 2016 14:03:29 +0200
Subject: [PATCH 08/11] remove unused constriant

---
 .../constraints/constraint_jsa_impl.h         | 271 ------------------
 1 file changed, 271 deletions(-)
 delete mode 100644 cob_twist_controller/include/cob_twist_controller/constraints/constraint_jsa_impl.h

diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jsa_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jsa_impl.h
deleted file mode 100644
index 1519292d..00000000
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_jsa_impl.h
+++ /dev/null
@@ -1,271 +0,0 @@
-/*!
- *****************************************************************
- * \file
- *
- * \note
- *   Copyright (c) 2015 \n
- *   Fraunhofer Institute for Manufacturing Engineering
- *   and Automation (IPA) \n\n
- *
- *****************************************************************
- *
- * \note
- *   Project name: care-o-bot
- * \note
- *   ROS stack name: cob_control
- * \note
- *   ROS package name: cob_twist_controller
- *
- * \author
- *   Author: Marco Bezzon, email: Marco.Bezzon@ipa.fraunhofer.de
- *
- * \date Date of creation: May, 2015
- *
- * \brief
- *   Implementation of several constraints
- *
- ****************************************************************/
-
-#ifndef COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_JSA_IMPL_H
-#define COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_JSA_IMPL_H
-
-#include <sstream>
-#include <string>
-#include <vector>
-#include <limits>
-#include <ros/ros.h>
-
-#include <boost/shared_ptr.hpp>
-#include <boost/pointer_cast.hpp>
-
-#include <kdl/chain.hpp>
-#include <kdl/chainfksolver.hpp>
-#include <kdl/chainfksolverpos_recursive.hpp>
-#include <kdl/chainjnttojacsolver.hpp>
-#include <kdl/jntarray.hpp>
-#include "cob_twist_controller/utils/chainjnttojacdotsolver.hpp"
-
-#include "cob_twist_controller/constraints/constraint.h"
-#include "cob_twist_controller/constraints/constraint_params.h"
-
-#include "cob_twist_controller/damping_methods/damping.h"
-#include "cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h"
-
-#include <Eigen/Eigenvalues>
-#include <unsupported/Eigen/NumericalDiff>
-#include <eigen_conversions/eigen_kdl.h>
-
-/* BEGIN JointLimitAvoidance ************************************************************************************/
-template <typename T_PARAMS, typename PRIO>
-Task_t JointSingularityAvoidance<T_PARAMS, PRIO>::createTask()
-{
-    Eigen::MatrixXd cost_func_jac = this->getTaskJacobian();
-    Eigen::VectorXd derivs = this->getTaskDerivatives();
-    Task_t task(this->getPriority(),
-                this->getTaskId(),
-                cost_func_jac,
-                derivs,
-                this->getType());
-
-    task.tcp_ = this->adaptDampingParamsForTask(this->constraint_params_.tc_params_.damping_jla);
-    task.db_ = boost::shared_ptr<DampingBase>(DampingBuilder::createDamping(task.tcp_));
-    return task;
-}
-
-template <typename T_PARAMS, typename PRIO>
-std::string JointSingularityAvoidance<T_PARAMS, PRIO>::getTaskId() const
-{
-    std::ostringstream oss;
-    oss << this->member_inst_cnt_;
-    oss << "_Joint#";
-    oss << this->constraint_params_.joint_idx_;
-    oss << "_";
-    oss << this->priority_;
-    std::string taskid = "JointSingularityAvoidance_" + oss.str();
-    return taskid;
-}
-
-template <typename T_PARAMS, typename PRIO>
-Eigen::MatrixXd JointSingularityAvoidance<T_PARAMS, PRIO>::getTaskJacobian() const
-{
-    return this->partial_values_.transpose();
-}
-
-template <typename T_PARAMS, typename PRIO>
-Eigen::VectorXd JointSingularityAvoidance<T_PARAMS, PRIO>::getTaskDerivatives() const
-{
-    return Eigen::VectorXd::Identity(1, 1) * this->derivative_value_;
-}
-
-template <typename T_PARAMS, typename PRIO>
-void JointSingularityAvoidance<T_PARAMS, PRIO>::calculate()
-{
-    const TwistControllerParams& params = this->constraint_params_.tc_params_;
-    const LimiterParams& limiter_params = this->constraint_params_.limiter_params_;
-    const int32_t joint_idx = this->constraint_params_.joint_idx_;
-    std::vector<double> limits_min = limiter_params.limits_min;
-    std::vector<double> limits_max = limiter_params.limits_max;
-    const double limit_min = limiter_params.limits_min[joint_idx];
-    const double limit_max = limiter_params.limits_max[joint_idx];
-    const double joint_pos = this->joint_states_.current_q_(joint_idx);
-
-    this->abs_delta_max_ = std::abs(limit_max - joint_pos);
-    this->rel_max_ = std::abs(this->abs_delta_max_ / limit_max);
-
-    this->abs_delta_min_ = std::abs(joint_pos - limit_min);
-    this->rel_min_ = std::abs(this->abs_delta_min_ / limit_min);
-
-    const double rel_val = this->rel_max_ < this->rel_min_ ? this->rel_max_ : this->rel_min_;
-
-    this->calcValue();
-    this->calcPartialValues();
-    this->calcDerivativeValue();
-    this->state_.setState(DANGER);  // always active task
-
-    // Compute prediction
-    const double pred_delta_max = std::abs(limit_max - this->jnts_prediction_.q(joint_idx));
-    const double pred_rel_max = std::abs(pred_delta_max / limit_max);
-    const double pred_delta_min = std::abs(this->jnts_prediction_.q(joint_idx) - limit_min);
-    const double pred_rel_min = std::abs(pred_delta_min / limit_min);
-    const double pred_rel_val = pred_rel_max < pred_rel_min ? pred_rel_max : pred_rel_min;
-
-//    if (this->state_.getCurrent() == CRITICAL && pred_rel_val < rel_val)
-//    {
-//        ROS_WARN_STREAM(this->getTaskId() << ": Current JSA state is CRITICAL but prediction is smaller than current rel_val -> Stay in CRIT.");
-//    }
-//    else if (rel_val < 0.1 || pred_rel_val < 0.1)
-//    {
-//        this->state_.setState(CRITICAL);  // always highest task -> avoid HW destruction.
-//    }
-//    else
-//    {
-//        this->state_.setState(DANGER);  // always active task -> avoid HW destruction.
-//    }
-
-}
-
-template <typename T_PARAMS, typename PRIO>
-double JointSingularityAvoidance<T_PARAMS, PRIO>::getActivationGain() const
-{
-    const TwistControllerParams& params = this->constraint_params_.tc_params_;
-    const int32_t joint_idx = this->constraint_params_.joint_idx_;
-    const double joint_pos = this->joint_states_.current_q_(joint_idx);
-
-    double activation_gain = 1.0;
-    return activation_gain;
-}
-
-/// Returns a value for k_H to weight the partial values for e.g. GPM
-template <typename T_PARAMS, typename PRIO>
-double JointSingularityAvoidance<T_PARAMS, PRIO>::getSelfMotionMagnitude(const Eigen::MatrixXd& particular_solution, const Eigen::MatrixXd& homogeneous_solution) const
-{
-    const TwistControllerParams& params = this->constraint_params_.tc_params_;
-    double k_H = params.k_H_jsa;
-    return k_H;
-}
-
-template <typename T_PARAMS, typename PRIO>
-ConstraintTypes JointSingularityAvoidance<T_PARAMS, PRIO>::getType() const
-{
-    return JSA;
-}
-
-/// Calculate values of the JSA cost function.
-template <typename T_PARAMS, typename PRIO>
-void JointSingularityAvoidance<T_PARAMS, PRIO>::calcValue()
-{
-    const TwistControllerParams& params = this->constraint_params_.tc_params_;
-    const LimiterParams& limiter_params = this->constraint_params_.limiter_params_;
-    const int32_t joint_idx = this->constraint_params_.joint_idx_;
-    std::vector<double> limits_min = limiter_params.limits_min;
-    std::vector<double> limits_max = limiter_params.limits_max;
-    const double joint_pos = this->joint_states_.current_q_(joint_idx);
-
-//    double nom = (joint_pos - (limits_max[joint_idx] + limits_min[joint_idx])/2);
-//    double denom = (limits_max[joint_idx] - limits_min[joint_idx]);
-    Eigen::MatrixXd jac = this->jacobian_data_;
-    Eigen::MatrixXd prod = jac * jac.transpose();
-    double d = prod.determinant();
-    double mom = 100*std::sqrt(std::abs(d));
-
-    this->last_value_ = this->value_;
-    this->value_ = mom;
-    ROS_WARN_STREAM("manipulability: " << mom);
-
-//    this->last_value_ = this->value_;
-//    this->value_ = std::abs(w) > ZERO_THRESHOLD ? (1/w) : 1 / DIV0_SAFE;
-}
-
-/// Calculate derivative of values.
-template <typename T_PARAMS, typename PRIO>
-void JointSingularityAvoidance<T_PARAMS, PRIO>::calcDerivativeValue()
-{
-    this->derivative_value_ = -0.1 * this->value_;
-}
-
-/// Calculates values of the gradient of the cost function
-template <typename T_PARAMS, typename PRIO>
-void JointSingularityAvoidance<T_PARAMS, PRIO>::calcPartialValues()
-{
-    const TwistControllerParams& params = this->constraint_params_.tc_params_;
-    const LimiterParams& limiter_params = this->constraint_params_.limiter_params_;
-    const double joint_pos = this->joint_states_.current_q_(this->constraint_params_.joint_idx_);
-    const double joint_vel = this->joint_states_.current_q_dot_(this->constraint_params_.joint_idx_);
-    const double limits_min = limiter_params.limits_min[this->constraint_params_.joint_idx_];
-    const double limits_max = limiter_params.limits_max[this->constraint_params_.joint_idx_];
-    const int32_t joint_idx = this->constraint_params_.joint_idx_;
-
-    Eigen::VectorXd partial_values = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
-
-
-    PInvBySVD pinv_calc;
-    KDL::ChainJntToJacDotSolver J_dot_solver(params.chain);
-
-    KDL::Jacobian J_dot(params.chain.getNrOfJoints());
-
-    // JointVelocity predictions
-    KDL::FrameVel frame_vel;
-
-    KDL::JntArrayVel jnts_prediction_chain(params.dof);
-
-    for (unsigned int i = 0; i < params.dof; i++)
-    {
-        jnts_prediction_chain.q(i) = this->jnts_prediction_.q(i);
-        jnts_prediction_chain.qdot(i) = this->jnts_prediction_.qdot(i);
-    }
-    // Calculate prediction for the mainipulator
-    int error = this->fk_solver_vel_.JntToCart(jnts_prediction_chain, frame_vel, joint_idx);
-    if (error != 0)
-    {
-        ROS_ERROR_STREAM("Could not calculate twist for frame: " << joint_idx  << ". Error Code: " << error << " (" << this->fk_solver_vel_.strError(error) << ")");
-        return;
-    }
-
-    J_dot_solver.JntToJacDot(jnts_prediction_chain,J_dot, -1);
-
-    Eigen::MatrixXd jac = this->jacobian_data_;
-
-    Eigen::MatrixXd prod = jac * jac.transpose();
-    double d = prod.determinant();
-    double mom = std::sqrt(std::abs(d));
-    double dmom_dq = 0;
-
-
-
-
-    Eigen::MatrixXd dJ_dq_J_inverse = J_dot.data * pinv_calc.calculate(jac);
-    dmom_dq = -100*mom * dJ_dq_J_inverse.trace();
-
-    ROS_INFO_STREAM("dmom_dq: " << dmom_dq);
-
-//    Eigen::JacobiSVD<Eigen::MatrixXd> svd(dJ_dq_J_inverse, Eigen::ComputeThinU | Eigen::ComputeThinV);
-
-
-    partial_values(this->constraint_params_.joint_idx_) = dmom_dq;
-    this->partial_values_ = partial_values;
-    this->jacobian_data_old_ = jac;
-    this->init_ = true;
-}
-/* END JointSingularityAvoidance **************************************************************************************/
-
-#endif  // COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_JSA_IMPL_H

From 7f732e2e4539a216c5ea3480fe3884d9ee42cb77 Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Thu, 28 Jul 2016 14:27:04 +0200
Subject: [PATCH 09/11] few changes from the pull request

---
 .../debug/debug_obstacle_distance_node.cpp    |  1 -
 .../src/distance_manager.cpp                  |  2 +-
 cob_twist_controller/CMakeLists.txt           |  6 +-
 cob_twist_controller/cfg/TwistController.cfg  | 28 +++---
 .../cob_twist_controller_data_types.h         | 31 +++----
 .../constraints/constraint_base.h             |  2 +-
 .../constraints/constraint_ca_impl.h          |  7 --
 .../constraints/constraint_impl.h             | 21 -----
 .../inverse_jacobian_calculation.h            |  6 ++
 .../src/cob_twist_controller.cpp              |  9 +-
 .../solvers/stack_of_tasks_solver.cpp         |  2 +-
 .../inverse_jacobian_calculation.cpp          | 89 ++++++++++---------
 .../kinematic_extension_dof.cpp               |  9 +-
 .../twist_controller_config.py                |  3 +-
 14 files changed, 83 insertions(+), 133 deletions(-)

diff --git a/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp b/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp
index 2c4dcd7f..bdaf44d3 100644
--- a/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp
+++ b/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp
@@ -93,7 +93,6 @@ std::string chain_base_link_;
             marker_vector.id = 42;
             marker_vector.header.frame_id = chain_base_link_;
 
-
             marker_vector.scale.x = 0.01;
             marker_vector.scale.y = 0.05;
 
diff --git a/cob_obstacle_distance/src/distance_manager.cpp b/cob_obstacle_distance/src/distance_manager.cpp
index 7140d700..8c3fbc5c 100644
--- a/cob_obstacle_distance/src/distance_manager.cpp
+++ b/cob_obstacle_distance/src/distance_manager.cpp
@@ -90,7 +90,7 @@ int DistanceManager::init()
         return -1;
     }
 
-    if(!nh_.getParam("/arm_left/joint_names", this->joints_))
+    if(!nh_.getParam("joint_names", this->joints_))
     {
         ROS_ERROR("Failed to get parameter \"joint_names\".");
         return -2;
diff --git a/cob_twist_controller/CMakeLists.txt b/cob_twist_controller/CMakeLists.txt
index f28e31e3..8c854028 100644
--- a/cob_twist_controller/CMakeLists.txt
+++ b/cob_twist_controller/CMakeLists.txt
@@ -29,10 +29,6 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS} ${oroco
 set(SRC_C_DIR "src/constraint_solvers")
 set(SRC_CS_DIR "${SRC_C_DIR}/solvers")
 
-add_library(jacobian_derivative src/utils/chainjnttojacdotsolver.cpp)
-add_dependencies(jacobian_derivative ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
-target_link_libraries(jacobian_derivative ${catkin_LIBRARIES})
-
 add_library(damping_methods src/damping_methods/damping.cpp)
 add_dependencies(damping_methods ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(damping_methods ${catkin_LIBRARIES})
@@ -43,7 +39,7 @@ target_link_libraries(inv_calculations ${catkin_LIBRARIES})
 
 add_library(constraint_solvers ${SRC_CS_DIR}/unconstraint_solver.cpp ${SRC_CS_DIR}/wln_joint_limit_avoidance_solver.cpp ${SRC_CS_DIR}/weighted_least_norm_solver.cpp ${SRC_CS_DIR}/gradient_projection_method_solver.cpp ${SRC_CS_DIR}/stack_of_tasks_solver.cpp ${SRC_CS_DIR}/task_priority_solver.cpp ${SRC_C_DIR}/constraint_solver_factory.cpp)
 add_dependencies(constraint_solvers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(constraint_solvers damping_methods inv_calculations jacobian_derivative)
+target_link_libraries(constraint_solvers damping_methods inv_calculations)
 
 add_library(limiters src/limiters/limiter.cpp)
 add_dependencies(limiters ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
diff --git a/cob_twist_controller/cfg/TwistController.cfg b/cob_twist_controller/cfg/TwistController.cfg
index 2b1e3f2a..4e574a2b 100755
--- a/cob_twist_controller/cfg/TwistController.cfg
+++ b/cob_twist_controller/cfg/TwistController.cfg
@@ -34,12 +34,6 @@ jla_constraints_enum = gen.enum([
                        gen.const("JLA_INEQ",    int_t, 3, "Inequality constraint for JLA."),
                        ],
                      "enum types for the joint limit avoidance constraints")
-                     
-jsa_constraints_enum = gen.enum([
-                       gen.const("JSA_OFF",     int_t, 0, "JSA inactive"),
-                       gen.const("JSA",         int_t, 1, "JSA active"),
-                       ],
-                     "enum types for the joint singularity avoidance constraints")
 
 ca_constraints_enum = gen.enum([
                        gen.const("CA_OFF",            int_t, 0, "CA inactive"),
@@ -52,9 +46,18 @@ kinematic_extension_enum = gen.enum([
                        gen.const("BASE_COMPENSATION", int_t, 1, "Compensate base motion"),
                        gen.const("BASE_ACTIVE",       int_t, 2, "Consider additional DoF of a mobile base (2d)"),
                        gen.const("COB_TORSO",         int_t, 3, "KinematicExtension for COB_TORSO (any DoF)"),
-                       gen.const("LOOKAT",            int_t, 4, "Consider additional DoF of a virtual Lookat component")],
+                       gen.const("LOOKAT",            int_t, 4, "Consider additional DoF of a virtual Lookat component"),
+                       ],
                      "enum types for the kinematic extensions")
 
+singularity_avoidance_enum = gen.enum([
+                       gen.const("SA_QD_EXTENDED",   int_t, 0, "Quadratic Damping with addidtional damping factor"),
+                       gen.const("SA_NF",   int_t, 1, "Numerical filtering (Damping value based on least singular value"),
+                       gen.const("SA_SR",   int_t, 2, "Robustness algorithm (Damping values for each singular value"),
+                       gen.const("SA_QD",   int_t, 3, "Quadratic Damping without additional damping factor"),
+                       ],
+                     "enum types for the different singular avoidance algorithms")
+                     
 # ==================================== Controller interfaces =================================================================================
 ctrl_interface = gen.add_group("Controller Interfaces", "ctrl_interface")
 ctrl_interface.add("controller_interface",   int_t,    0, "The controller interface to use", 0, None, None, edit_method=controller_interface_enum)
@@ -62,9 +65,8 @@ ctrl_interface.add("integrator_smoothing",   double_t, 0, "The factor used for e
 
 # ==================================== Damping and truncation (singular value adaption) ====================================================
 damp_trunc = gen.add_group("Damping and Truncation", "damping_truncation")
-damp_trunc.add("numerical_filtering",         bool_t,   0, "Numerical Filtering yes/no",  False)
-damp_trunc.add("singular_value_damping",      bool_t,   0, "Singular value daming yes/no",  True)
-damp_trunc.add("damping_method",              int_t,    0, "The damping method to use.", 2, None, None, edit_method=damping_method_enum)
+damp_trunc.add("singularity_avoidance", int_t,    0, "Singularity avoidance method", 0, None, None, edit_method=singularity_avoidance_enum)
+damp_trunc.add("damping_method",     int_t,    0, "The damping method to use.", 2, None, None, edit_method=damping_method_enum)
 damp_trunc.add("damping_factor",     double_t, 0, "The constant damping_factor (used in CONSTANT)",  0.01, 0, 1)
 damp_trunc.add("lambda_max",         double_t, 0, "Value for maximum damping_factor (used in MANIPULABILITY/LSV)",  0.1, 0, 10)
 damp_trunc.add("w_threshold",        double_t, 0, "Value for manipulability threshold (used in MANIPULABILITY)",  0.005, 0, 1)
@@ -92,12 +94,6 @@ jla.add("activation_buffer_jla",    double_t, 0, "In [%]. For smooth transition
 jla.add("critical_threshold_jla",   double_t, 0, "In [%]. Tolerance when critical region becomes active. JLA becomes task. Should be less than activation threshold (best experienced: 1/2 of activation threshold).", 5.0, 0.0, 100.0)
 jla.add("damping_jla",              double_t, 0, "Const. damping factor for the inv. of the JLA task jacobian", 0.000001, 0.0, 1.0)
 
-jsa = solv_constr.add_group("Joint Singularity Avoidance", "jsa")
-jsa.add("constraint_jsa",           int_t,    0, "The JSA constraint to use (edited via an enum)", 1, None, None, edit_method=jsa_constraints_enum)
-jsa.add("priority_jsa",             int_t,    0, "Priority for the joint singularity avoidance constraint (important for task processing; 0 = highest prio)", 50, 0,   1000)
-jsa.add("k_H_jsa",                  double_t, 0, "Self-motion factor for GPM. Special weighting for JSA constraint", -1.0, -1000,   1000)
-jsa.add("damping_jsa",              double_t, 0, "Const. damping factor for the inv. of the JSA task jacobian", 0.000001, 0.0, 1.0)
-
 ca = solv_constr.add_group("Collision Avoidance", "ca")
 ca.add("constraint_ca",           int_t,    0, "The CA constraint to use (edited via an enum)", 0, None, None, edit_method=ca_constraints_enum)
 ca.add("priority_ca",             int_t,    0, "Priority for the collision avoidance constraint (important for task processing; 0 = highest prio).", 100, 0,   1000)
diff --git a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h
index b2422f20..d894cbaa 100644
--- a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h
+++ b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h
@@ -70,6 +70,14 @@ enum KinematicExtensionTypes
     LOOKAT = cob_twist_controller::TwistController_LOOKAT
 };
 
+enum SingularityAvoidanceTypes
+{
+    SA_QD_EXTENDED = cob_twist_controller::TwistController_SA_QD_EXTENDED,
+    SA_NF = cob_twist_controller::TwistController_SA_NF,
+    SA_SR = cob_twist_controller::TwistController_SA_SR,
+    SA_QD = cob_twist_controller::TwistController_SA_QD,
+};
+
 enum SolverTypes
 {
     DEFAULT_SOLVER = cob_twist_controller::TwistController_DEFAULT_SOLVER,
@@ -93,12 +101,6 @@ enum ConstraintTypesJLA
     JLA_INEQ_ON = cob_twist_controller::TwistController_JLA_INEQ,
 };
 
-enum ConstraintTypesJSA
-{
-    JSA_OFF = cob_twist_controller::TwistController_JSA_OFF,
-    JSA_ON = cob_twist_controller::TwistController_JSA,
-};
-
 enum ConstraintTypes
 {
     None,
@@ -106,7 +108,6 @@ enum ConstraintTypes
     JLA,
     JLA_MID,
     JLA_INEQ,
-    JSA,
 };
 
 enum LookatAxisTypes
@@ -207,8 +208,7 @@ struct TwistControllerParams
         controller_interface(VELOCITY_INTERFACE),
         integrator_smoothing(0.2),
 
-        numerical_filtering(false),
-        singular_value_damping(true),
+        singularity_avoidance(SA_SR),
         damping_method(MANIPULABILITY),
         damping_factor(0.2),
         lambda_max(0.1),
@@ -234,11 +234,6 @@ struct TwistControllerParams
         damping_ca(0.000001),
         k_H_ca(2.0),
 
-        constraint_jsa(JSA_ON),
-        priority_jsa(50),
-        k_H_jsa(-10.0),
-        damping_jsa(0.000001),
-
         kinematic_extension(NO_EXTENSION),
         extension_ratio(0.0)
     {
@@ -258,8 +253,7 @@ struct TwistControllerParams
     ControllerInterfaceTypes controller_interface;
     double integrator_smoothing;
 
-    bool numerical_filtering;
-    bool singular_value_damping;
+    SingularityAvoidanceTypes singularity_avoidance;
     DampingMethodTypes damping_method;
     double damping_factor;
     double lambda_max;
@@ -287,11 +281,6 @@ struct TwistControllerParams
     double damping_jla;
     ConstraintThresholds thresholds_jla;
 
-    ConstraintTypesJSA constraint_jsa;
-    uint32_t priority_jsa;
-    double k_H_jsa;
-    double damping_jsa;
-
     LimiterParams limiter_params;
 
     KinematicExtensionTypes kinematic_extension;
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h
index afc1e032..6973dc0a 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_base.h
@@ -259,7 +259,7 @@ class ConstraintBase : public PriorityBase<PRIO>
             adapted_params.damping_method = CONSTANT;
             adapted_params.damping_factor = const_damping_factor;
             adapted_params.eps_truncation = 0.0;
-            adapted_params.numerical_filtering = false;
+//            adapted_params.numerical_filtering = false;
             return adapted_params;
         }
 };
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
index bf187ade..485d7a9a 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h
@@ -298,8 +298,6 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPartialValues()
                                  collision_pnt_vector.y(), -collision_pnt_vector.x(),  0.0;
 
                 Eigen::Matrix3d ident = Eigen::Matrix3d::Identity();
-
-                // ToDo: dynamic matrix size for Base Active mode.
                 Eigen::Matrix<double, 6, 6> T;
                 T.block(0, 0, 3, 3) << ident;
                 T.block(0, 3, 3, 3) << skew_symm;
@@ -398,8 +396,6 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
         {
             uint32_t frame_number = (str_it - params.frame_names.begin()) + 1;  // segment nr not index represents frame number
             KDL::FrameVel frame_vel;
-            ROS_WARN_STREAM("frame_number" << frame_number);
-            // ToDo: the fk_solver_vel_ is only initialized for the primary chain - kinematic extensions cannot be considered yet!
             KDL::JntArrayVel jnts_prediction_chain(params.dof);
 
             for (unsigned int i = 0; i < params.dof; i++)
@@ -408,9 +404,6 @@ void CollisionAvoidance<T_PARAMS, PRIO>::calcPredictionValue()
                 jnts_prediction_chain.qdot(i) = this->jnts_prediction_.qdot(i);
             }
 
-
-//             ROS_INFO_STREAM("jnts_prediction_chain.q.rows: " << jnts_prediction_chain.q.rows());
-
             // Calculate prediction for the mainipulator
             int error = this->fk_solver_vel_.JntToCart(jnts_prediction_chain, frame_vel, frame_number);
             if (error != 0)
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h
index 6548d841..876deca5 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_impl.h
@@ -124,24 +124,6 @@ std::set<ConstraintBase_t> ConstraintsBuilder<PRIO>::createConstraints(const Twi
         // So the log would be filled unnecessarily.
     }
 
-    // Joint Singularity Avoidance
-    if (JSA_ON == tc_params.constraint_jsa)
-    {
-        typedef JointSingularityAvoidance<ConstraintParamsJSA, PRIO> Jsa_t;
-
-        ConstraintParamsJSA params = ConstraintParamFactory<ConstraintParamsJSA>::createConstraintParams(tc_params, limiter_params, data_mediator);
-        uint32_t startPrio = 20; // Todo
-
-
-        // TODO: take care PRIO could be of different type than UINT32
-        params.joint_ = tc_params.joints[tc_params.joints.size()-1];
-        params.joint_idx_ = static_cast<int32_t>(tc_params.joints.size()-1);
-        // copy of params will be created; priority increased with each joint.
-        boost::shared_ptr<Jsa_t > jsa(new Jsa_t(startPrio++, params, data_mediator, fk_solver_vel));
-        constraints.insert(boost::static_pointer_cast<PriorityBase<PRIO> >(jsa));
-
-    }
-
     return constraints;
 }
 /* END ConstraintsBuilder *******************************************************************************************/
@@ -152,7 +134,4 @@ std::set<ConstraintBase_t> ConstraintsBuilder<PRIO>::createConstraints(const Twi
 // Joint Limit Avoidance Constraint Implementation
 #include "cob_twist_controller/constraints/constraint_jla_impl.h"
 
-// Joint Limit Avoidance Constraint Implementation
-#include "cob_twist_controller/constraints/constraint_jsa_impl.h"
-
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_IMPL_H
diff --git a/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h b/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h
index c96a3537..d47478b2 100644
--- a/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h
+++ b/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h
@@ -48,6 +48,12 @@ class PInvBySVD : public IPseudoinverseCalculator
                                           const Eigen::MatrixXd& jacobian) const;
 
         virtual ~PInvBySVD() {}
+
+        enum InverseDampingMethods
+        {
+            numerical_filtering = 1 ,
+            singular_damping_value = 2
+        };
 };
 /* END PInvBySVD ************************************************************************************************/
 
diff --git a/cob_twist_controller/src/cob_twist_controller.cpp b/cob_twist_controller/src/cob_twist_controller.cpp
index e0f31d49..1e18e4b7 100644
--- a/cob_twist_controller/src/cob_twist_controller.cpp
+++ b/cob_twist_controller/src/cob_twist_controller.cpp
@@ -220,8 +220,7 @@ void CobTwistController::reconfigureCallback(cob_twist_controller::TwistControll
     twist_controller_params_.controller_interface = static_cast<ControllerInterfaceTypes>(config.controller_interface);
     twist_controller_params_.integrator_smoothing = config.integrator_smoothing;
 
-    twist_controller_params_.numerical_filtering = config.numerical_filtering;
-    twist_controller_params_.singular_value_damping = config.singular_value_damping;
+    twist_controller_params_.singularity_avoidance = static_cast<SingularityAvoidanceTypes>(config.singularity_avoidance);
     twist_controller_params_.damping_method = static_cast<DampingMethodTypes>(config.damping_method);
     twist_controller_params_.damping_factor = config.damping_factor;
     twist_controller_params_.lambda_max = config.lambda_max;
@@ -246,12 +245,6 @@ void CobTwistController::reconfigureCallback(cob_twist_controller::TwistControll
     twist_controller_params_.thresholds_jla.critical =  critical_jla_in_percent / 100.0;
     twist_controller_params_.damping_jla = config.damping_jla;
 
-    twist_controller_params_.constraint_jsa = static_cast<ConstraintTypesJSA>(config.constraint_jsa);
-    twist_controller_params_.priority_jsa = config.priority_jsa;
-    twist_controller_params_.k_H_jsa = config.k_H_jsa;
-    twist_controller_params_.damping_jsa = config.damping_jsa;
-
-
     twist_controller_params_.constraint_ca = static_cast<ConstraintTypesCA>(config.constraint_ca);
     twist_controller_params_.priority_ca = config.priority_ca;
     twist_controller_params_.k_H_ca = config.k_H_ca;
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
index 69b5fce1..57205c15 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
@@ -105,7 +105,7 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
         Eigen::MatrixXd J_task = it->task_jacobian_;
         Eigen::MatrixXd J_temp = J_task * projector_i;
         Eigen::VectorXd v_task = it->task_;
-        Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(J_temp);
+        Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(params_,t.db_, J_temp);
         q_i = q_i + J_temp_inv * (v_task - J_task * q_i);
         projector_i = projector_i - J_temp_inv * J_temp;
     }
diff --git a/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp b/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
index d0c6b390..89ab2a67 100644
--- a/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
+++ b/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
@@ -43,6 +43,8 @@ Eigen::MatrixXd PInvBySVD::calculate(const Eigen::MatrixXd& jacobian) const
     Eigen::VectorXd singularValuesInv = Eigen::VectorXd::Zero(singularValues.rows());
 
     // small change to ref: here quadratic damping due to Control of Redundant Robot Manipulators : R.V. Patel, 2005, Springer [Page 13-14]
+
+    //ToDo: Test this
     for (uint32_t i = 0; i < singularValues.rows(); ++i)
     {
         double denominator = singularValues(i) * singularValues(i);
@@ -70,59 +72,64 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
     double lambda = db->getDampingFactor(singularValues, jacobian);
     Eigen::MatrixXd result;
 
-    if (params.numerical_filtering)
+    switch(params.singularity_avoidance)
     {
-        // Formula 20 Singularity-robust Task-priority Redundandancy Resolution
-        // Sum part
-        for (uint32_t i = 0; i < singularValues.rows()-1; ++i)
+        case(SA_NF):
         {
-            // pow(beta, 2) << pow(lambda, 2)
-            singularValuesInv(i) = singularValues(i) / (pow(singularValues(i), 2) + pow(params.beta, 2));
+            // Formula 20 Singularity-robust Task-priority Redundandancy Resolution
+            // Sum part
+            for (uint32_t i = 0; i < singularValues.rows()-1; ++i)
+            {
+                // pow(beta, 2) << pow(lambda, 2)
+                singularValuesInv(i) = singularValues(i) / (pow(singularValues(i), 2) + pow(params.beta, 2));
+            }
+            // Formula 20 - additional part - numerical filtering for least singular value m
+            uint32_t m = singularValues.rows()-1;
+            singularValuesInv(m) = singularValues(m) / (pow(singularValues(m), 2) + pow(params.beta, 2) + pow(lambda, 2));
+
+            result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
+            break;
         }
-        // Formula 20 - additional part - numerical filtering for least singular value m
-        uint32_t m = singularValues.rows();
-        singularValuesInv(m) = singularValues(m) / (pow(singularValues(m), 2) + pow(params.beta, 2) + pow(lambda, 2));
+        case(SA_SR):
+        {
+            Eigen::VectorXd lambda_vec = Eigen::VectorXd::Zero(singularValues.size());
 
-        result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
-    }
-    else if(params.singular_value_damping)
-    {
-        Eigen::VectorXd lambda_vec = Eigen::VectorXd::Zero(singularValues.size());
+            Eigen::MatrixXd S = Eigen::MatrixXd::Zero(singularValues.size(),singularValues.size());
 
-        Eigen::MatrixXd S = Eigen::MatrixXd::Zero(singularValues.size(),singularValues.size());
+            for(unsigned i = 0; i < singularValues.size(); i++)
+            {
+                lambda_vec(i) = params.damping_gain /( 1+ exp(static_cast <double>(singularValues(i)) + params.damping_delta) / params.damping_slope);
+                S(i,i) = singularValues(i)/(pow(static_cast <double>(singularValues(i)),2) + lambda_vec(i));
+            }
 
-        for(unsigned i = 0; i < singularValues.size(); i++)
+            result = svd.matrixV() * S * svd.matrixU().transpose();
+            break;
+        }
+        case(SA_QD):
         {
-            lambda_vec(i) = params.damping_gain /( 1+ exp(static_cast <double>(singularValues(i)) + params.damping_delta) / params.damping_slope);
-            S(i,i) = singularValues(i)/(pow(static_cast <double>(singularValues(i)),2) + lambda_vec(i));
+            for (uint32_t i = 0; i < singularValues.rows(); ++i)
+            {
+                double denominator = singularValues(i) * singularValues(i);
+                singularValuesInv(i) = (singularValues(i) < eps_truncation) ? 0.0 : singularValues(i) / denominator;
+            }
+
+            result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
+            break;
         }
-
-        ROS_INFO_STREAM("S: \n" << S);
-        ROS_INFO_STREAM("lambda: \n" << lambda_vec);
-        result = svd.matrixV() * S * svd.matrixU().transpose();
-    }
-    else
-    {
-        // small change to ref: here quadratic damping due to Control of Redundant Robot Manipulators : R.V. Patel, 2005, Springer [Page 13-14]
-        for (uint32_t i = 0; i < singularValues.rows(); ++i)
+        case(SA_QD_EXTENDED):
         {
-            double denominator = (singularValues(i) * singularValues(i) + pow(lambda, 2) );
-            // singularValuesInv(i) = (denominator < eps_truncation) ? 0.0 : singularValues(i) / denominator;
-            singularValuesInv(i) = (singularValues(i) < eps_truncation) ? 0.0 : singularValues(i) / denominator;
+            // small change to ref: here quadratic damping due to Control of Redundant Robot Manipulators : R.V. Patel, 2005, Springer [Page 13-14]
+            for (uint32_t i = 0; i < singularValues.rows(); ++i)
+            {
+                double denominator = (singularValues(i) * singularValues(i) + pow(lambda, 2) );
+                // singularValuesInv(i) = (denominator < eps_truncation) ? 0.0 : singularValues(i) / denominator;
+                singularValuesInv(i) = (singularValues(i) < eps_truncation) ? 0.0 : singularValues(i) / denominator;
+            }
+
+            result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
         }
-
-        //// Formula from Advanced Robotics : Redundancy and Optimization : Nakamura, Yoshihiko, 1991, Addison-Wesley Pub. Co [Page 258-260]
-        // for(uint32_t i = 0; i < singularValues.rows(); ++i)
-        // {
-        //       // damping is disabled due to damping factor lower than a const. limit
-        //       singularValues(i) = (singularValues(i) < eps_truncation) ? 0.0 : 1.0 / singularValues(i);
-        // }
-
-        result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
     }
 
-//    Eigen::MatrixXd result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
-
     return result;
 }
 
diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
index e407af90..166e228c 100644
--- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
+++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp
@@ -230,13 +230,7 @@ JointStates KinematicExtensionBaseActive::adjustJointStates(const JointStates& j
         js.current_q_dot_(i) = joint_states.current_q_dot_(i);
         js.last_q_dot_(i) = joint_states.last_q_dot_(i);
     }
-//    for (unsigned int i = 0; i < ext_dof_; i++)
-//    {
-//        js.current_q_(chain_dof + i) = 0.0;
-//        js.last_q_(chain_dof + i) = 0.0;
-//        js.current_q_dot_(chain_dof + i) = 0.0;
-//        js.last_q_dot_(chain_dof + i) = 0.0;
-//    }
+
     double roll, pitch, yaw;
     tf::Quaternion q;
     q.setW(pose.orientation.w);
@@ -290,7 +284,6 @@ LimiterParams KinematicExtensionBaseActive::adjustLimiterParams(const LimiterPar
 void KinematicExtensionBaseActive::processResultExtension(const KDL::JntArray& q_dot_ik)
 {
     geometry_msgs::Twist base_vel_msg;
-
     base_vel_msg.linear.x = (std::fabs(q_dot_ik(params_.dof)) < min_vel_lin_base_) ? 0.0 : q_dot_ik(params_.dof);
     base_vel_msg.linear.y = (std::fabs(q_dot_ik(params_.dof+1)) < min_vel_lin_base_) ? 0.0 : q_dot_ik(params_.dof+1);
     base_vel_msg.linear.z = (std::fabs(q_dot_ik(params_.dof+2)) < min_vel_lin_base_) ? 0.0 : q_dot_ik(params_.dof+2);
diff --git a/cob_twist_controller/src/twist_controller_config/twist_controller_config.py b/cob_twist_controller/src/twist_controller_config/twist_controller_config.py
index ba4a61fb..f88b7713 100644
--- a/cob_twist_controller/src/twist_controller_config/twist_controller_config.py
+++ b/cob_twist_controller/src/twist_controller_config/twist_controller_config.py
@@ -34,8 +34,6 @@
 '''
 CTRL_IF = 'controller_interface'
 
-NUM_FILT = 'numerical_filtering'
-SING_FILT = 'singular_value_damping'
 DAMP_METHOD = 'damping_method'
 DAMP_FACT = 'damping_factor'
 LAMBDA_MAX = 'lambda_max'
@@ -81,6 +79,7 @@
 MAX_VEL_ROT_BASE = 'max_vel_rot_base'
 
 KIN_EXT = 'kinematic_extension'
+SA = 'singularity_avoidance'
 EXT_RATIO = 'extension_ratio'
 
 '''

From 864f5e06a999ba310c4a15fd001853c821e5d29d Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Thu, 28 Jul 2016 14:31:06 +0200
Subject: [PATCH 10/11] removed JSA leftovers

---
 .../callback_data_mediator.h                  |  7 -----
 .../constraints/constraint_params.h           | 29 -------------------
 .../src/callback_data_mediator.cpp            |  6 ----
 .../twist_controller_config.py                |  5 ----
 4 files changed, 47 deletions(-)

diff --git a/cob_twist_controller/include/cob_twist_controller/callback_data_mediator.h b/cob_twist_controller/include/cob_twist_controller/callback_data_mediator.h
index fc1b09f2..6d1869f2 100644
--- a/cob_twist_controller/include/cob_twist_controller/callback_data_mediator.h
+++ b/cob_twist_controller/include/cob_twist_controller/callback_data_mediator.h
@@ -69,13 +69,6 @@ class CallbackDataMediator
          */
         bool fill(ConstraintParamsJLA& params_jla);
 
-        /**
-         * Special implementation for Joint Singularity Avoidance parameters.
-         * @param params_jsa Reference to JSA parameters.
-         * @return Success of filling parameters.
-         */
-        bool fill(ConstraintParamsJSA& params_jsa);
-
         /**
          * Callback method that can be used by a ROS subscriber to a obstacle distance topic.
          * @param msg The published message containting obstacle distances.
diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h
index fe480300..65838249 100644
--- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h
+++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_params.h
@@ -109,35 +109,6 @@ class ConstraintParamsJLA : public ConstraintParamsBase
 };
 /* END ConstraintParamsJLA **************************************************************************************/
 
-
-/* BEGIN ConstraintParamsJSA ************************************************************************************/
-/// Class that represents the parameters for the Singularity Avoidance constraint.
-class ConstraintParamsJSA : public ConstraintParamsBase
-{
-    public:
-        ConstraintParamsJSA(const TwistControllerParams& params,
-                            const LimiterParams& limiter_params,
-                            const std::string& id = std::string()) :
-                ConstraintParamsBase(params, limiter_params, id),
-                joint_idx_(-1)
-        {}
-
-        ConstraintParamsJSA(const ConstraintParamsJLA& cpjsa) :
-                ConstraintParamsBase(cpjsa.tc_params_, cpjsa.limiter_params_, cpjsa.id_),
-                joint_(cpjsa.joint_),
-                joint_idx_(cpjsa.joint_idx_)
-        {}
-
-        virtual ~ConstraintParamsJSA()
-        {}
-
-        std::string joint_;
-        int32_t joint_idx_;
-};
-/* END ConstraintParamsJLA **************************************************************************************/
-
-
-
 typedef boost::shared_ptr<ConstraintParamsBase> ConstraintParamsBase_t;
 
 #endif  // COB_TWIST_CONTROLLER_CONSTRAINTS_CONSTRAINT_PARAMS_H
diff --git a/cob_twist_controller/src/callback_data_mediator.cpp b/cob_twist_controller/src/callback_data_mediator.cpp
index 7366012f..9ed0ad73 100644
--- a/cob_twist_controller/src/callback_data_mediator.cpp
+++ b/cob_twist_controller/src/callback_data_mediator.cpp
@@ -66,12 +66,6 @@ bool CallbackDataMediator::fill(ConstraintParamsJLA& params_jla)
     return true;
 }
 
-/// Can be used to fill parameters for joint singularity avoidance.
-bool CallbackDataMediator::fill(ConstraintParamsJSA& params_jsa)
-{
-    return true;
-}
-
 /// Producer: Fills obstacle distances but only when they are empty
 void CallbackDataMediator::distancesToObstaclesCallback(const cob_control_msgs::ObstacleDistances::ConstPtr& msg)
 {
diff --git a/cob_twist_controller/src/twist_controller_config/twist_controller_config.py b/cob_twist_controller/src/twist_controller_config/twist_controller_config.py
index f88b7713..d951f29a 100644
--- a/cob_twist_controller/src/twist_controller_config/twist_controller_config.py
+++ b/cob_twist_controller/src/twist_controller_config/twist_controller_config.py
@@ -57,11 +57,6 @@
 CRIT_THRESH_JLA = 'critical_threshold_jla'
 DAMP_JLA = 'damping_jla'
 
-CONSTR_JSA = 'constraint_jsa'
-PRIO_JSA = 'priority_jsa'
-K_H_JSA = 'k_H_jsa'
-DAMP_JSA = 'damping_jsa'
-
 CONSTR_CA = 'constraint_ca'
 PRIO_CA = 'priority_ca'
 K_H_CA = 'k_H_ca'

From 1cd9dc97c8c188bd3fd6319d528e8d1d5237f1ec Mon Sep 17 00:00:00 2001
From: ipa-fxm-cm <christoph.mark@ipa.fraunhofer.de>
Date: Fri, 29 Jul 2016 14:37:15 +0200
Subject: [PATCH 11/11] Added new concept for jointlimit avoidance

---
 .../inverse_jacobian_calculation.h            |  3 ++-
 .../inverse_jacobian_calculation_base.h       |  3 ++-
 .../gradient_projection_method_solver.cpp     |  2 +-
 .../solvers/stack_of_tasks_solver.cpp         |  4 ++--
 .../solvers/task_priority_solver.cpp          |  2 +-
 .../solvers/unconstraint_solver.cpp           |  2 +-
 .../solvers/weighted_least_norm_solver.cpp    |  2 +-
 .../inverse_jacobian_calculation.cpp          | 23 +++++++++++++++----
 8 files changed, 29 insertions(+), 12 deletions(-)

diff --git a/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h b/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h
index d47478b2..551d9b1c 100644
--- a/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h
+++ b/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h
@@ -45,7 +45,8 @@ class PInvBySVD : public IPseudoinverseCalculator
          */
         virtual Eigen::MatrixXd calculate(const TwistControllerParams& params,
                                           boost::shared_ptr<DampingBase> db,
-                                          const Eigen::MatrixXd& jacobian) const;
+                                          const Eigen::MatrixXd& jacobian,
+                                          const JointStates& joint_states) const;
 
         virtual ~PInvBySVD() {}
 
diff --git a/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation_base.h b/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation_base.h
index f8a96a18..1617fb1c 100644
--- a/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation_base.h
+++ b/cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation_base.h
@@ -54,7 +54,8 @@ class IPseudoinverseCalculator
          */
         virtual Eigen::MatrixXd calculate(const TwistControllerParams& params,
                                           boost::shared_ptr<DampingBase> db,
-                                          const Eigen::MatrixXd& jacobian) const = 0;
+                                          const Eigen::MatrixXd& jacobian,
+                                          const JointStates& joint_states) const = 0;
 
         /**
          * Class has no members so implementing an empty destructor.
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
index 6f3e8f69..5ac23d6e 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/gradient_projection_method_solver.cpp
@@ -40,7 +40,7 @@
 Eigen::MatrixXd GradientProjectionMethodSolver::solve(const Vector6d_t& in_cart_velocities,
                                                       const JointStates& joint_states)
 {
-    Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
+    Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_,joint_states);
     Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);
 
     Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
index 57205c15..486783b8 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/stack_of_tasks_solver.cpp
@@ -40,7 +40,7 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
     double cycle = (now - this->last_time_).toSec();
     this->last_time_ = now;
 
-    Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
+    Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_, joint_states);
     Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);
 
     Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;
@@ -105,7 +105,7 @@ Eigen::MatrixXd StackOfTasksSolver::solve(const Vector6d_t& in_cart_velocities,
         Eigen::MatrixXd J_task = it->task_jacobian_;
         Eigen::MatrixXd J_temp = J_task * projector_i;
         Eigen::VectorXd v_task = it->task_;
-        Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(params_,t.db_, J_temp);
+        Eigen::MatrixXd J_temp_inv = pinv_calc_.calculate(params_,t.db_, J_temp, joint_states);
         q_i = q_i + J_temp_inv * (v_task - J_task * q_i);
         projector_i = projector_i - J_temp_inv * J_temp;
     }
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
index c0e24509..acba6e6f 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/task_priority_solver.cpp
@@ -48,7 +48,7 @@ Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,
 
     Eigen::MatrixXd qdots_out = Eigen::MatrixXd::Zero(this->jacobian_data_.cols(), 1);
     Eigen::VectorXd partial_cost_func = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
-    Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
+    Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_, joint_states);
     Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);
 
     Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
index 4c23edde..23af2985 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/unconstraint_solver.cpp
@@ -38,7 +38,7 @@
 Eigen::MatrixXd UnconstraintSolver::solve(const Vector6d_t& in_cart_velocities,
                                           const JointStates& joint_states)
 {
-    Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
+    Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_, joint_states);
     Eigen::MatrixXd qdots_out = pinv * in_cart_velocities;
     return qdots_out;
 }
diff --git a/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp b/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
index 120b961d..4cdc8250 100644
--- a/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
+++ b/cob_twist_controller/src/constraint_solvers/solvers/weighted_least_norm_solver.cpp
@@ -44,7 +44,7 @@ Eigen::MatrixXd WeightedLeastNormSolver::solve(const Vector6d_t& in_cart_velocit
 
     // SVD of JLA weighted Jacobian: Damping will be done later in calculatePinvJacobianBySVD for pseudo-inverse Jacobian with additional truncation etc.
     Eigen::MatrixXd weighted_jacobian = this->jacobian_data_ * inv_root_W_WLN;
-    Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, weighted_jacobian);
+    Eigen::MatrixXd pinv = pinv_calc_.calculate(this->params_, this->damping_, weighted_jacobian, joint_states);
 
     // Take care: W^(1/2) * q_dot = weighted_pinv_J * x_dot -> One must consider the weighting!!!
     Eigen::MatrixXd qdots_out = inv_root_W_WLN * pinv * in_cart_velocities;
diff --git a/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp b/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
index 89ab2a67..8424ea25 100644
--- a/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
+++ b/cob_twist_controller/src/inverse_jacobian_calculations/inverse_jacobian_calculation.cpp
@@ -63,7 +63,8 @@ Eigen::MatrixXd PInvBySVD::calculate(const Eigen::MatrixXd& jacobian) const
  */
 Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
                                      boost::shared_ptr<DampingBase> db,
-                                     const Eigen::MatrixXd& jacobian) const
+                                     const Eigen::MatrixXd& jacobian,
+                                     const JointStates& joint_states) const
 {
     Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
     double eps_truncation = params.eps_truncation;
@@ -72,6 +73,14 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
     double lambda = db->getDampingFactor(singularValues, jacobian);
     Eigen::MatrixXd result;
 
+    const LimiterParams limiter_params = params.limiter_params;
+
+    double alpha;
+    double distance;
+    Eigen::MatrixXd v_ith_row;
+    Eigen::MatrixXd svd_V = svd.matrixV();
+
+
     switch(params.singularity_avoidance)
     {
         case(SA_NF):
@@ -95,14 +104,20 @@ Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
             Eigen::VectorXd lambda_vec = Eigen::VectorXd::Zero(singularValues.size());
 
             Eigen::MatrixXd S = Eigen::MatrixXd::Zero(singularValues.size(),singularValues.size());
-
             for(unsigned i = 0; i < singularValues.size(); i++)
             {
+                distance = abs(abs(static_cast <double>(joint_states.current_q_(i))) - static_cast <double>(limiter_params.limits_max[i]));
+//                alpha = 1/(1 + exp(((distance - params.damping_delta )/ params.damping_slope)*(-1)));
+                alpha = 1/(1 + exp(((distance - 0.1)/ 0.01)*(-1)));
+
+                ROS_WARN_STREAM("alpha[" << i << "]: " << alpha << "   distance[" << i << "]: " << distance);
+
                 lambda_vec(i) = params.damping_gain /( 1+ exp(static_cast <double>(singularValues(i)) + params.damping_delta) / params.damping_slope);
                 S(i,i) = singularValues(i)/(pow(static_cast <double>(singularValues(i)),2) + lambda_vec(i));
+                v_ith_row = svd_V.row(i) * alpha;
+                svd_V.row(i) = v_ith_row;
             }
-
-            result = svd.matrixV() * S * svd.matrixU().transpose();
+            result = svd_V * S * svd.matrixU().transpose();
             break;
         }
         case(SA_QD):