init

CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 3.10 FATAL_ERROR)
+set(CMAKE_CXX_STANDARD 14)
+project(fanuc_stmotion_controller VERSION 1.0.0 LANGUAGES CXX)
+
+# /* -------------------------------------------------------------------------- */
+# /*                              Build Properties                              */
+# /* -------------------------------------------------------------------------- */
+
+add_compile_options(-std=c++11)
+add_definitions(-D_OS_UNIX)
+
+# /* -------------------------------------------------------------------------- */
+# /*                                Find Package                                */
+# /* -------------------------------------------------------------------------- */
+
+find_package(Eigen3 3.3.7 REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+catkin_package()
+catkin_python_setup()
+include_directories(
+    include
+    ${catkin_INCLUDE_DIRS})
+
+# /* -------------------------------------------------------------------------- */
+# /*                               Create Library                               */
+# /* -------------------------------------------------------------------------- */
+
+file(GLOB src_files src/*.cpp
+                    src/Utils/*.cpp)
+
+add_library(${PROJECT_NAME} SHARED ${src_files})
+
+target_link_libraries(${PROJECT_NAME}
+    Eigen3::Eigen
+    ${catkin_LIBRARIES}
+    jsoncpp
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+    PRIVATE src)
+
+set_target_properties(${PROJECT_NAME} PROPERTIES
+    VERSION ${PROJECT_VERSION}
+    SOVERSION 1)
+
+
+# /* -------------------------------------------------------------------------- */
+# /*                             Testing Executables                            */
+# /* -------------------------------------------------------------------------- */
+
+add_executable(${PROJECT_NAME}_controller_usage_example
+  examples/cpp/controller_usage_example.cpp
+)
+target_link_libraries(${PROJECT_NAME}_controller_usage_example
+    ${PROJECT_NAME}
+)
+
+add_executable(${PROJECT_NAME}_bringup
+  src/controller_node.cpp
+)
+target_link_libraries(${PROJECT_NAME}_bringup
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+)
+
+install(PROGRAMS examples/python/controller_usage_example.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

config/robot_properties/Fanuc_DH.txt

@@ -0,0 +1,6 @@
+0 0 0.05 -1.57079632679
+-1.57079632679 0 0.44 3.1415926
+0 0 0.035 -1.57079632679
+0 -0.42 0 1.57079632679
+0 0 0 -1.57079632679
+0 -0.08 0 3.1415926

config/robot_properties/Fanuc_base.txt

@@ -0,0 +1,3 @@
+0
+0
+0.33

config/robot_properties/robot_capsules.txt

@@ -0,0 +1,8 @@
+0.34771,-0.10894,0.0075375,0.13655,0.16605,-0.022357,0.12814
+0.37068,-0.16991,-0.19078,0.027915,-0.11744,-0.12978,0.098065
+0.14348,-0.99445,-0.012646,0.39245,-0.046324,-0.00097583,0.41529
+0.26558,-1.51,-0.040162,0.1731,-0.19747,0.015875,0.10792
+0.076343,-0.00049808,0.021828,0.041475,-0.0080274,0.045528,0.024543
+0.058695,-0.0010264,-0.0024201,0.0037234,-0.0048605,0.019719,0.041017
+0.022163,-0.0007788,0.00061582,-0.046828,0.00085503,0.00077449,-0.0011437
+0.13348,0.00027618,-0.10641,-0.24025,-0.00024491,-0.060434,-0.11137

config/user_config.json

@@ -0,0 +1,20 @@
+{
+    "DH_fname":"/config/robot_properties/Fanuc_DH.txt",
+    "robot_base_fname":"/config/robot_properties/Fanuc_base.txt",
+    "SSA_Enable":0,
+    "Use_Robot":0,
+    "JPC_travel_time":1,
+    "Infinite_tasks":1,
+    "Start_with_assemble":1,
+    "Robot_IP":"192.168.1.100",
+    "Simulation_controller_topic":
+    {
+        "J1":"/fanuc_gazebo/joint1_position_controller/command",
+        "J2":"/fanuc_gazebo/joint2_position_controller/command",
+        "J3":"/fanuc_gazebo/joint3_position_controller/command",
+        "J4":"/fanuc_gazebo/joint4_position_controller/command",
+        "J5":"/fanuc_gazebo/joint5_position_controller/command",
+        "J6":"/fanuc_gazebo/joint6_position_controller/command"
+    },
+    "Nominal_mode":"jpc"
+}

examples/cpp/controller_usage_example.cpp

@@ -0,0 +1,100 @@
+#include "Robot.hpp"
+
+using namespace std::chrono;
+
+fanuc_stmotion_controller::math::VectorJd robot_q = Eigen::MatrixXd::Zero(6, 1);
+fanuc_stmotion_controller::math::VectorJd robot_qd = Eigen::MatrixXd::Zero(6, 1);
+fanuc_stmotion_controller::math::VectorJd robot_qdd = Eigen::MatrixXd::Zero(6, 1);
+
+void robotStateCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
+{
+    robot_q << msg->data[0], msg->data[3], msg->data[6], msg->data[9], msg->data[12], msg->data[15];
+    robot_qd << msg->data[1], msg->data[4], msg->data[7], msg->data[10], msg->data[13], msg->data[16];
+    robot_qdd << msg->data[2], msg->data[5], msg->data[8], msg->data[11], msg->data[14], msg->data[17];
+}
+
+bool reached_goal(const fanuc_stmotion_controller::math::VectorJd& goal)
+{
+    for(int i=0; i<6; i++)
+    {
+        if(abs(goal(i) - robot_q(i)) > 0.01 || abs(robot_qd(i)) > 0.001 || abs(robot_qdd(i)) > 0.001)
+        {
+            return false;
+        }
+    }
+    return true;
+}
+
+int main(int argc, char **argv)
+{
+    try
+    {
+        ros::init(argc, argv, "use_controller_node");
+        ros::NodeHandle nh("~");
+        ROS_INFO_STREAM("namespace of nh = " << nh.getNamespace());
+        ros::Rate loop_rate(150);
+
+        Eigen::Matrix<double, 6, 5> joint_goal = Eigen::Matrix<double, 6, 5>::Zero();
+        joint_goal.col(0) << 0, 0, 0, 0, 0, 0;
+        joint_goal.col(1) << 0, 0, 0, 0, -90, 0;
+        joint_goal.col(2) << 0, 0, 0, 0, 0, 0;
+        joint_goal.col(3) << 0, 0, 0, 0, -90, 0;
+        joint_goal.col(4) << 0, 0, 0, 0, 0, 0;
+        int robot_dof = 6;
+
+        ros::Publisher goal_pub = nh.advertise<std_msgs::Float32MultiArray>("/fanuc_stmotion_controller_bringup/robot_goal", robot_dof);
+        ros::Publisher jpc_time_pub = nh.advertise<std_msgs::Float64>("/fanuc_stmotion_controller_bringup/jpc_travel_time", 1);
+        ros::Subscriber robot_state_sub = nh.subscribe("/fanuc_stmotion_controller_bringup/robot_state", robot_dof * 3, robotStateCallback);
+        std_msgs::Float32MultiArray goal_msg;
+        std_msgs::Float64 jpc_time_msg;
+
+        int num_tasks = joint_goal.cols();
+        int task_idx = -1;
+        fanuc_stmotion_controller::math::VectorJd cur_goal = Eigen::MatrixXd::Zero(6, 1);
+        cur_goal = joint_goal.col(0);
+
+        while(ros::ok)
+        {
+            if(task_idx == -1 || reached_goal(cur_goal))
+            {
+                if(task_idx == -1)
+                {
+                    jpc_time_msg.data = 0.5;
+                    jpc_time_pub.publish(jpc_time_msg);
+                }
+                else if (task_idx == 1)
+                {
+                    jpc_time_msg.data = 5;
+                    jpc_time_pub.publish(jpc_time_msg);
+                }
+                else if(task_idx == 3)
+                {
+                    jpc_time_msg.data = 0.1;
+                    jpc_time_pub.publish(jpc_time_msg);
+                }
+                task_idx ++;
+                if(task_idx >= joint_goal.cols())
+                {
+                    break;
+                }
+                cur_goal = joint_goal.col(task_idx);     
+            }
+            goal_msg.data.clear();
+            for(int j=0; j<robot_dof; j++)
+            {
+                goal_msg.data.push_back(cur_goal(j));
+            }
+            goal_pub.publish(goal_msg);
+            ros::spinOnce();
+        }
+        ROS_INFO_STREAM("Task Execution Done!");
+        ros::shutdown();
+        return 0;
+    }
+    catch(const std::exception& e)
+    {
+        std::cerr << e.what() << '\n';
+    }
+}
+
+

examples/python/controller_usage_example.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+
+import rospy
+import time
+import numpy as np
+from std_msgs.msg import Float32MultiArray
+from std_msgs.msg import Float64
+
+class Robot():
+    def __init__(self):
+        self.dof = 6
+        self.robot_goal_pub = rospy.Publisher('/fanuc_stmotion_controller_bringup/robot_goal', Float32MultiArray, queue_size=6)
+        self.robot_state_sub = rospy.Subscriber("/fanuc_stmotion_controller_bringup/robot_state", Float32MultiArray, self.robot_state_callback)
+        self.robot_travel_time_pub = rospy.Publisher('/fanuc_stmotion_controller_bringup/jpc_travel_time', Float64, queue_size=1)
+        self.robot_goal = np.zeros(self.dof)
+        self.robot_goal_msg = Float32MultiArray()
+        self.robot_state = np.zeros(self.dof)
+        self.robot_state_v = np.zeros(self.dof)
+        self.robot_state_a = np.zeros(self.dof)
+        self.robot_travel_time = Float64()
+
+    def robot_state_callback(self, data):
+        for i in range(self.dof):
+            self.robot_state[i] = data.data[i*3]
+            self.robot_state_v[i] = data.data[i*3 + 1]
+            self.robot_state_a[i] = data.data[i*3 + 2]
+
+    def set_robot_travel_time(self, t):
+        self.robot_travel_time.data = t
+        self.robot_travel_time_pub.publish(self.robot_travel_time)
+
+    def drive_robot(self, goal):
+        for i in range(self.dof):
+            self.robot_goal[i] = goal[i]
+        self.robot_goal_msg.data = goal
+        self.robot_goal_pub.publish(self.robot_goal_msg)
+
+    def goal_reached(self):
+        for i in range(self.dof):
+            if(abs(self.robot_state[i] - self.robot_goal[i]) > 0.01 or abs(self.robot_state_v[i]) > 0.001 or abs(self.robot_state_a[i] > 0.001)):
+                return False
+        return True
+
+def main():
+    rospy.init_node('use_controller_node')
+    fanuc = Robot()
+    goal_list = [[0, 0, 0, 0, 0, 0],
+                 [0, 0, 0, 0, -90, 0],
+                 [0, 0, 0, 0, 0, 0],
+                 [0, 0, 0, 0, -90, 0],
+                 [0 ,0, 0, 0, 0, 0]]
+
+    goal_idx = -1
+    cur_goal = goal_list[0]
+    time.sleep(0.1)
+    while not rospy.is_shutdown():
+        if(goal_idx == -1 or fanuc.goal_reached()):
+            print("Reached: ", goal_idx)
+            if(goal_idx == -1):
+                fanuc.set_robot_travel_time(0.5)
+            elif(goal_idx == 1):
+                fanuc.set_robot_travel_time(5)
+            elif(goal_idx == 3):
+                fanuc.set_robot_travel_time(0.1)
+
+            goal_idx += 1
+            if(goal_idx < len(goal_list)):
+                cur_goal = goal_list[goal_idx]   
+            else:
+                break
+            time.sleep(0.01)
+        fanuc.drive_robot(cur_goal)
+
+if __name__ == '__main__':
+    main()