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_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"),
@@ -47,26 +46,39 @@ 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)
 ctrl_interface.add("integrator_smoothing",   double_t, 0, "The factor used for exponential smoothing during simpson integration)",  0.2, 0, 1)
 
 # ==================================== 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("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)
 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)

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

@@ -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,
@@ -200,14 +208,17 @@ struct TwistControllerParams
         controller_interface(VELOCITY_INTERFACE),
         integrator_smoothing(0.2),
 
-        numerical_filtering(false),
+        singularity_avoidance(SA_SR),
         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),
@@ -242,14 +253,17 @@ struct TwistControllerParams
     ControllerInterfaceTypes controller_interface;
     double integrator_smoothing;
 
-    bool numerical_filtering;
+    SingularityAvoidanceTypes singularity_avoidance;
     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;
@@ -278,6 +292,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_;
@@ -256,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;
         }
 };

cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h

@@ -287,7 +287,8 @@ 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;
 
                 // Create a skew-symm matrix as transformation between the segment root and the critical point.
@@ -308,7 +309,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());
@@ -389,22 +391,21 @@ 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;
-
-            // 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);
+            // 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) << ")");
@@ -413,7 +414,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);

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

cob_twist_controller/include/cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h

@@ -45,9 +45,16 @@ 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() {}
+
+        enum InverseDampingMethods
+        {
+            numerical_filtering = 1 ,
+            singular_damping_value = 2
+        };
 };
 /* END PInvBySVD ************************************************************************************************/
 

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.

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;