[WIP] Implemented moveit based distance computation

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/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/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.
@@ -297,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;
@@ -308,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());
@@ -389,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) << ")");
@@ -413,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);

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

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

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

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

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

cob_twist_controller/src/cob_twist_controller.cpp

@@ -142,7 +142,8 @@ 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 +165,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 +310,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;
@@ -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
 }

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

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

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

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

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