Twist controller extensions

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}

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;
 

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;

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)
 add_dependencies(damping_methods ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(damping_methods ${catkin_LIBRARIES})
@@ -39,7 +43,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)
+target_link_libraries(constraint_solvers damping_methods inv_calculations jacobian_derivative)
 
 add_library(limiters src/limiters/limiter.cpp)
 add_dependencies(limiters ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

cob_twist_controller/cfg/TwistController.cfg

@@ -27,14 +27,19 @@ 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"),
                        gen.const("JLA_MID",     int_t, 2, "Special JLA: Keep joint pos in the middle of the limited range."),
                        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)

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.

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

cob_twist_controller/include/cob_twist_controller/cob_twist_controller_data_types.h

@@ -93,13 +93,20 @@ 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,
     CA,
     JLA,
     JLA_MID,
     JLA_INEQ,
+    JSA,
 };
 
 enum LookatAxisTypes
@@ -201,13 +208,17 @@ struct TwistControllerParams
         integrator_smoothing(0.2),
 
         numerical_filtering(false),
+        singular_value_damping(true),
         damping_method(MANIPULABILITY),
         damping_factor(0.2),
         lambda_max(0.1),
         w_threshold(0.005),
         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,13 +259,17 @@ struct TwistControllerParams
     double integrator_smoothing;
 
     bool numerical_filtering;
+    bool singular_value_damping;
     DampingMethodTypes damping_method;
     double damping_factor;
     double lambda_max;
     double w_threshold;
     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

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

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_;