cbf example is added

src/safety/control_barrier_function/cpp_example/test_cbf/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(test_cbf)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  sdu_math
+)
+find_package(osqp REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(CGAL)
+generate_messages(
+  DEPENDENCIES std_msgs  # Or other packages containing msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES belt_task
+  CATKIN_DEPENDS roscpp std_msgs
+  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+#set(MOSEK_LIBRARY_DIRS "/home/yik/mosek/10.0/tools/platform/linux64x86/bin/")
+#set(MOSEK_LIBRARIES
+#"/home/yik/mosek/10.0/tools/platform/linux64x86/bin/libmosek64.so.10.0"
+#)
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+#include_directories(/home/yik/osqp-cpp/include)
+include_directories(
+  include
+  include/test_cbf
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+set( SOURCE_FILES 
+ src/test_cbf.cpp
+ src/control_barrier_functions.cpp
+)
+
+#link_directories(/home/yik/mosek/10.0/tools/platform/linux64x86/bin/)
+add_executable(test_cbf ${SOURCE_FILES})
+#target_link_libraries(test_cbf ${catkin_LIBRARIES} rtde osqp::osqp mosek64 fusion64 cgal CGAL::CGAL)
+target_link_libraries(test_cbf ${catkin_LIBRARIES} rtde quadprog)
+#add_dependencies(test_mosek mosek64)

src/safety/control_barrier_function/cpp_example/test_cbf/include/test_cbf/control_barrier_functions.h

@@ -0,0 +1,141 @@
+/*
+ * control_barrier_functions.h
+ *
+ *  Created on: Feb 20, 2023
+ *      Author: yik
+ */
+
+#ifndef SDU_MOSEK_TEST_TEST_MOSEK_INCLUDE_TEST_MOSEK_CONTROL_BARRIER_FUNCTIONS_H_
+#define SDU_MOSEK_TEST_TEST_MOSEK_INCLUDE_TEST_MOSEK_CONTROL_BARRIER_FUNCTIONS_H_
+
+#include <Eigen/Dense>
+#include <iostream>
+#include "QuadProg++/Array.hh"
+#include "QuadProg++/QuadProg++.hh"
+//#include "osqp++.h"
+
+
+////      //mosek
+//#include "fusion.h"
+//
+//using namespace mosek::fusion;
+//using namespace monty;
+
+// CGAL
+//#include <cassert>
+//#include <CGAL/QP_models.h>
+//#include <CGAL/QP_functions.h>
+//// choose exact integral type
+//#ifdef CGAL_USE_GMP
+//#include <CGAL/Gmpz.h>
+//typedef CGAL::Gmpz ET;
+//#else
+//#include <CGAL/MP_Float.h>
+//typedef CGAL::MP_Float ET;
+//#endif
+//// program and solution types
+//typedef CGAL::Quadratic_program<double> Program;
+//typedef CGAL::Quadratic_program_solution<ET> Solution;
+
+class ControlBarrierFunctions
+{
+public:
+  ControlBarrierFunctions(double control_time);
+  ~ControlBarrierFunctions();
+  void CalculateKelvinVoigtContactModelSimulatedForce(double safe_contact_force);
+  void CBFKelvinVoigtContactModel(double x, double dot_x, double nominal_ctrl_input,double alpha_CBF);
+  void ACBFKelvinVoigtContactModel(double x, double dot_x, double nominal_ctrl_input,double alpha_aCBF);
+  void RACBFKelvinVoigtContactModel(double x, double dot_x, double nominal_ctrl_input,double alpha_RaCBF,double mp_error);
+
+
+  double GetSimulatedRealForce();
+  double GetSafeControlForce();
+  Eigen::Matrix<double, 2 ,1> GetSimulatedStates();
+
+  void SetSafetyForce(double force_min, double force_max);
+  void initialize();
+
+
+private:
+  double control_time_;
+
+  //theta
+  Eigen::Matrix<double, 2 ,1> theta_real_;
+
+
+  //modeled_force
+  double simulated_contact_force_;
+
+  //force limit
+  Eigen::Matrix<double, 2 ,1> safety_limit_; //force min max
+
+  //control output
+  double safe_contact_force_;
+
+  // control barrier function
+  double h_CBF_;
+  double f_min_;
+  double f_max_;
+
+  Eigen::Matrix<double, 1 ,2> delta_h_CBF_;
+  Eigen::Matrix<double, 2 ,1> f_h_CBF_;
+  Eigen::Matrix<double, 2 ,1> g_h_CBF_;
+  Eigen::Matrix<double, 2 ,2> F_h_CBF_;
+
+  Eigen::Matrix<double, 1 ,1> A_;
+  Eigen::Matrix<double, 1 ,1> B_;
+  Eigen::Matrix<double, 1 ,1> temp_B_;
+
+  quadprogpp::Matrix<double> G, CE, CI;
+  quadprogpp::Vector<double> g0, ce0, ci0, u;
+  int n, m, p;
+  double sum;
+  char ch;
+
+  double alpha_;
+
+  // adaptive control barrier function
+  double h_aCBF_;
+  double alpha_aCBF_;
+  Eigen::Matrix<double, 2 ,2> adaptive_gain_;
+
+  Eigen::Matrix<double, 1 ,2> delta_h_aCBF_;
+  Eigen::Matrix<double, 1 ,2> delta_param_h_aCBF_;
+  Eigen::Matrix<double, 2 ,1> f_h_aCBF_;
+  Eigen::Matrix<double, 2 ,1> g_h_aCBF_;
+  Eigen::Matrix<double, 2 ,2> F_h_aCBF_;
+
+  // robust adaptive control barrier function
+  double h_RaCBF_;
+  double alpha_RaCBF_;
+  Eigen::Matrix<double, 2 ,2> adaptive_gain_RaCBF_;
+
+  Eigen::Matrix<double, 1 ,2> delta_h_RaCBF_;
+  Eigen::Matrix<double, 1 ,2> delta_param_h_RaCBF_;
+  Eigen::Matrix<double, 2 ,1> f_h_RaCBF_;
+  Eigen::Matrix<double, 2 ,1> g_h_RaCBF_;
+  Eigen::Matrix<double, 2 ,2> F_h_RaCBF_;
+
+
+
+  //simulation states
+  double b_model_;
+  double k_model_;
+  double uncertain_k_;
+  double uncertain_b_;
+  double real_k_;
+  double real_b_;
+  Eigen::Matrix<double, 2 ,1> theta_model_;
+
+  double mass_;
+  double x_;
+  double dx_;
+  double ddx_;
+  Eigen::Matrix<double, 2 ,1>  states_;
+};
+
+
+
+
+
+#endif /* SDU_MOSEK_TEST_TEST_MOSEK_INCLUDE_TEST_MOSEK_CONTROL_BARRIER_FUNCTIONS_H_ */

src/safety/control_barrier_function/cpp_example/test_cbf/include/test_cbf/test_cbf.h

@@ -0,0 +1,88 @@
+/*
+ * test_mosek.h
+ *
+ *  Created on: Feb 20, 2023
+ *      Author: yik
+ */
+
+#ifndef SDU_MOSEK_TEST_TEST_MOSEK_INCLUDE_TEST_MOSEK_TEST_MOSEK_H_
+#define SDU_MOSEK_TEST_TEST_MOSEK_INCLUDE_TEST_MOSEK_TEST_MOSEK_H_
+
+#define LOOP_PERIOD 2e6 //Expressed in ticks // 2ms control time
+#define TIMESPEC_ADD(A,B) /* A += B */ \
+    do {                                   \
+      (A).tv_sec += (B).tv_sec;          \
+      (A).tv_nsec += (B).tv_nsec;        \
+      if ( (A).tv_nsec >= 1000000000 ) { \
+        (A).tv_sec++;                  \
+        (A).tv_nsec -= 1000000000;     \
+      }                                  \
+    } while (0)
+
+#define TIMESPEC_SUB(A,B) /* A -= B */ \
+    do {                                   \
+      (A).tv_sec -= (B).tv_sec;          \
+      (A).tv_nsec -= (B).tv_nsec;        \
+      if ( (A).tv_nsec < 0 ) {           \
+        (A).tv_sec--;                  \
+        (A).tv_nsec += 1000000000;     \
+      }                                  \
+    } while (0)
+      //typedef actionlib::SimpleActionClient<test_client::belt_task_actionAction> Client;
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <sstream>
+#include <map>
+#include "control_barrier_functions.h"
+#include "sdu_math/control_function.h"
+//      //osqp
+//#include "osqp.h"
+//
+
+//preempt rt system
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+#include <pthread.h>
+#include <sys/mman.h>
+#include <limits.h>
+#include <sched.h>
+#include <signal.h>
+
+
+//memory lock
+#include <mutex>
+
+#include "ros/ros.h"
+#include "std_msgs/Float64MultiArray.h"
+#include "std_msgs/Float64.h"
+#include "std_msgs/Bool.h"
+#include "std_msgs/String.h"
+
+#include <ur_rtde/rtde_receive_interface.h> // 로봇으로 데이터를 받을때 쓰는 헤더
+#include <ur_rtde/rtde_control_interface.h> // 로봇으로 데이터를 줄때 쓰는 헤더 (control)
+
+using std::cout; using std::cerr;
+using std::endl; using std::string;
+using std::ifstream; using std::ostringstream;
+using std::istringstream;
+
+#include <Eigen/Dense>
+
+std::mutex m;
+volatile bool exit_program = false;
+//ros
+ros::Publisher raw_force_torque_pub_;
+std_msgs::Float64MultiArray raw_force_torque_msg_;
+
+//control
+double control_time;
+std::shared_ptr<ControlBarrierFunctions> cbf_x;
+std::shared_ptr<PID_function> force_x_compensator_;
+
+double nominal_ctrl_output;
+double desired_force;
+
+#endif /* SDU_MOSEK_TEST_TEST_MOSEK_INCLUDE_TEST_MOSEK_TEST_MOSEK_H_ */

src/safety/control_barrier_function/cpp_example/test_cbf/package.xml

@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>test_cbf</name>
+  <version>0.0.0</version>
+  <description>The test_mosek package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="yik@todo.todo">yik</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/test_mosek</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sdu_math</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>sdu_math</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>