├── .github └── workflows │ └── docs_pages_workflow.yml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── bash_scripts ├── clone_libfranka.sh ├── make_catkin.sh ├── make_franka_interface.sh ├── make_libfranka.sh ├── run_franka_interface.sh └── set_rosmaster.sh ├── catkin_ws ├── .catkin_workspace └── src │ └── franka_ros_interface │ ├── .gitignore │ ├── CMakeLists.txt │ ├── include │ └── franka_ros_interface │ │ ├── execute_skill_action_server.h │ │ ├── franka_interface_status_publisher.h │ │ ├── get_current_franka_interface_status_server.h │ │ ├── get_current_robot_state_server.h │ │ ├── robot_state_publisher.h │ │ ├── run_loop_process_info_state_publisher.h │ │ ├── sensor_data │ │ └── sensor_subscriber_handler.h │ │ └── shared_memory_handler.h │ ├── launch │ ├── franka_gripper.launch │ └── franka_ros_interface.launch │ ├── package.xml │ └── src │ ├── execute_skill_action_server.cpp │ ├── execute_skill_action_server_node.cpp │ ├── franka_interface_status_publisher.cpp │ ├── franka_interface_status_publisher_node.cpp │ ├── get_current_franka_interface_status_server.cpp │ ├── get_current_franka_interface_status_server_node.cpp │ ├── get_current_robot_state_server.cpp │ ├── get_current_robot_state_server_node.cpp │ ├── robot_state_publisher.cpp │ ├── robot_state_publisher_node.cpp │ ├── run_loop_process_info_state_publisher.cpp │ ├── run_loop_process_info_state_publisher_node.cpp │ ├── sensor_data │ ├── sensor_subscriber.cpp │ └── sensor_subscriber_handler.cpp │ └── shared_memory_handler.cpp ├── docs ├── .gitignore ├── Makefile ├── buildDocs.sh ├── conf.py ├── imgs │ ├── control_pc_network.png │ ├── franka_control_interface.png │ └── franka_network.png ├── index.rst ├── install.rst ├── make.bat ├── network.rst └── support.rst ├── franka-interface-common ├── CMakeLists.txt ├── include │ └── franka-interface-common │ │ ├── SharedMemoryInfo.h │ │ ├── definitions.h │ │ ├── franka_interface_state_info.h │ │ └── run_loop_process_info.h └── src │ ├── SharedMemoryInfo.cpp │ ├── franka_interface_state_info.cpp │ └── run_loop_process_info.cpp ├── franka-interface ├── CMakeLists.txt ├── include │ └── franka-interface │ │ ├── duration.h │ │ ├── feedback_controller │ │ ├── cartesian_impedance_feedback_controller.h │ │ ├── ee_cartesian_impedance_feedback_controller.h │ │ ├── feedback_controller.h │ │ ├── force_axis_impedence_feedback_controller.h │ │ ├── force_position_feedback_controller.h │ │ ├── joint_impedance_feedback_controller.h │ │ ├── noop_feedback_controller.h │ │ ├── pass_through_feedback_controller.h │ │ ├── pass_through_joint_torque_feedback_controller.h │ │ └── set_internal_impedance_feedback_controller.h │ │ ├── feedback_controller_factory.h │ │ ├── file_stream_logger.h │ │ ├── franka_gripper.h │ │ ├── franka_robot.h │ │ ├── robot_state_data.h │ │ ├── run_loop.h │ │ ├── run_loop_logger.h │ │ ├── run_loop_shared_memory_handler.h │ │ ├── save_robot_state_data_to_shared_memory_buffer.h │ │ ├── sensor_data.h │ │ ├── sensor_data_manager.h │ │ ├── skill_info_manager.h │ │ ├── skills │ │ ├── base_meta_skill.h │ │ ├── base_skill.h │ │ ├── cartesian_pose_skill.h │ │ ├── cartesian_velocity_skill.h │ │ ├── force_torque_skill.h │ │ ├── gripper_skill.h │ │ ├── impedance_control_skill.h │ │ ├── joint_position_continuous_skill.h │ │ ├── joint_position_skill.h │ │ └── joint_velocity_skill.h │ │ ├── termination_handler │ │ ├── contact_termination_handler.h │ │ ├── final_joint_termination_handler.h │ │ ├── final_pose_termination_handler.h │ │ ├── noop_termination_handler.h │ │ ├── termination_handler.h │ │ └── time_termination_handler.h │ │ ├── termination_handler_factory.h │ │ ├── trajectory_generator │ │ ├── cartesian_velocity_trajectory_generator.h │ │ ├── cubic_hermite_spline_joint_trajectory_generator.h │ │ ├── cubic_hermite_spline_pose_trajectory_generator.h │ │ ├── force_position_trajectory_generator.h │ │ ├── goal_pose_dmp_trajectory_generator.h │ │ ├── gripper_trajectory_generator.h │ │ ├── impulse_trajectory_generator.h │ │ ├── joint_dmp_trajectory_generator.h │ │ ├── joint_trajectory_generator.h │ │ ├── joint_velocity_trajectory_generator.h │ │ ├── linear_force_position_trajectory_generator.h │ │ ├── linear_joint_trajectory_generator.h │ │ ├── linear_pose_trajectory_generator.h │ │ ├── min_jerk_joint_trajectory_generator.h │ │ ├── min_jerk_pose_trajectory_generator.h │ │ ├── pass_through_cartesian_velocity_trajectory_generator.h │ │ ├── pass_through_force_position_trajectory_generator.h │ │ ├── pass_through_joint_trajectory_generator.h │ │ ├── pass_through_joint_velocity_trajectory_generator.h │ │ ├── pass_through_pose_trajectory_generator.h │ │ ├── pose_dmp_trajectory_generator.h │ │ ├── pose_trajectory_generator.h │ │ ├── quaternion_pose_dmp_trajectory_generator.h │ │ ├── relative_linear_pose_trajectory_generator.h │ │ ├── relative_min_jerk_pose_trajectory_generator.h │ │ ├── relative_pose_trajectory_generator.h │ │ ├── sine_joint_trajectory_generator.h │ │ ├── sine_pose_trajectory_generator.h │ │ ├── stay_in_initial_joints_trajectory_generator.h │ │ ├── stay_in_initial_pose_trajectory_generator.h │ │ └── trajectory_generator.h │ │ ├── trajectory_generator_factory.h │ │ └── utils │ │ └── logger_utils.h ├── proto │ ├── CMakeLists.txt │ ├── feedback_controller_params_msg.proto │ ├── robot_state_msg.proto │ ├── sensor_msg.proto │ ├── termination_handler_params_msg.proto │ └── trajectory_generator_params_msg.proto └── src │ ├── duration.cpp │ ├── feedback_controller │ ├── cartesian_impedance_feedback_controller.cpp │ ├── ee_cartesian_impedance_feedback_controller.cpp │ ├── feedback_controller.cpp │ ├── force_axis_impedence_feedback_controller.cpp │ ├── force_position_feedback_controller.cpp │ ├── joint_impedance_feedback_controller.cpp │ ├── noop_feedback_controller.cpp │ ├── pass_through_feedback_controller.cpp │ ├── pass_through_joint_torque_feedback_controller.cpp │ └── set_internal_impedance_feedback_controller.cpp │ ├── feedback_controller_factory.cpp │ ├── file_stream_logger.cpp │ ├── robot_state_data.cpp │ ├── run_loop.cpp │ ├── run_loop_logger.cpp │ ├── run_loop_shared_memory_handler.cpp │ ├── save_robot_state_data_to_shared_memory_buffer.cpp │ ├── sensor_data.cpp │ ├── sensor_data_manager.cpp │ ├── skill_info_manager.cpp │ ├── skills │ ├── base_meta_skill.cpp │ ├── base_skill.cpp │ ├── cartesian_pose_skill.cpp │ ├── cartesian_velocity_skill.cpp │ ├── force_torque_skill.cpp │ ├── gripper_skill.cpp │ ├── impedance_control_skill.cpp │ ├── joint_position_continuous_skill.cpp │ ├── joint_position_skill.cpp │ └── joint_velocity_skill.cpp │ ├── termination_handler │ ├── contact_termination_handler.cpp │ ├── final_joint_termination_handler.cpp │ ├── final_pose_termination_handler.cpp │ ├── noop_termination_handler.cpp │ ├── termination_handler.cpp │ └── time_termination_handler.cpp │ ├── termination_handler_factory.cpp │ ├── trajectory_generator │ ├── cartesian_velocity_trajectory_generator.cpp │ ├── cubic_hermite_spline_joint_trajectory_generator.cpp │ ├── cubic_hermite_spline_pose_trajectory_generator.cpp │ ├── goal_pose_dmp_trajectory_generator.cpp │ ├── gripper_trajectory_generator.cpp │ ├── impulse_trajectory_generator.cpp │ ├── joint_dmp_trajectory_generator.cpp │ ├── joint_trajectory_generator.cpp │ ├── joint_velocity_trajectory_generator.cpp │ ├── linear_force_position_trajectory_generator.cpp │ ├── linear_joint_trajectory_generator.cpp │ ├── linear_pose_trajectory_generator.cpp │ ├── min_jerk_joint_trajectory_generator.cpp │ ├── min_jerk_pose_trajectory_generator.cpp │ ├── pass_through_cartesian_velocity_trajectory_generator.cpp │ ├── pass_through_force_position_trajectory_generator.cpp │ ├── pass_through_joint_trajectory_generator.cpp │ ├── pass_through_joint_velocity_trajectory_generator.cpp │ ├── pass_through_pose_trajectory_generator.cpp │ ├── pose_dmp_trajectory_generator.cpp │ ├── pose_trajectory_generator.cpp │ ├── quaternion_pose_dmp_trajectory_generator.cpp │ ├── relative_linear_pose_trajectory_generator.cpp │ ├── relative_min_jerk_pose_trajectory_generator.cpp │ ├── relative_pose_trajectory_generator.cpp │ ├── sine_joint_trajectory_generator.cpp │ ├── sine_pose_trajectory_generator.cpp │ ├── stay_in_initial_joints_trajectory_generator.cpp │ └── stay_in_initial_pose_trajectory_generator.cpp │ ├── trajectory_generator_factory.cpp │ └── utils │ └── logger_utils.cpp ├── old_docs ├── control_pc_ubuntu_setup_guide.md └── franka_control_pc_setup_guide.md └── src └── franka_interface.cpp /.github/workflows/docs_pages_workflow.yml: -------------------------------------------------------------------------------- 1 | name: docs_pages_workflow 2 | 3 | # execute this workflow automatically when a we push to master 4 | on: 5 | push: 6 | branches: [ master ] 7 | 8 | jobs: 9 | 10 | build_docs_job: 11 | runs-on: ubuntu-latest 12 | container: debian:buster-slim 13 | 14 | steps: 15 | 16 | - name: Prereqs 17 | env: 18 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 19 | run: | 20 | apt-get update 21 | apt-get install -y git 22 | git clone --depth 1 "https://token:${GITHUB_TOKEN}@github.com/${GITHUB_REPOSITORY}.git" . 23 | shell: bash 24 | 25 | - name: Execute script to build our documentation and update pages 26 | env: 27 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 28 | run: "docs/buildDocs.sh" 29 | shell: bash 30 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | Compiled source # 2 | ################### 3 | *.com 4 | *.class 5 | *.dll 6 | *.exe 7 | *.o 8 | *.so 9 | *.pyc 10 | *.pyo 11 | *.out 12 | 13 | # Packages # 14 | ############ 15 | # it's better to unpack these files and commit the raw source 16 | # git has its own built in compression methods 17 | *.7z 18 | *.dmg 19 | *.gz 20 | *.iso 21 | *.jar 22 | *.rar 23 | *.tar 24 | *.zip 25 | 26 | # Logs and databases # 27 | ###################### 28 | *.log 29 | *.sql 30 | *.sqlite 31 | logs/* 32 | 33 | # OS generated files # 34 | ###################### 35 | .DS_Store 36 | .DS_Store? 37 | ._* 38 | .Spotlight-V100 39 | .Trashes 40 | ehthumbs.db 41 | Thumbs.db 42 | 43 | # IDE generated files # 44 | ###################### 45 | .classpath 46 | .project 47 | .settings 48 | .idea 49 | .metadata 50 | *.cbp 51 | *.iml 52 | *.ipr 53 | *.swp 54 | *.swo 55 | 56 | # CMakeFiles # 57 | ###################### 58 | *.cmake 59 | CMakeFiles/ 60 | Makefile 61 | CMakeCache.txt 62 | franka-interface/cmake-build-debug/ 63 | 64 | # Add workspace build files 65 | catkin_ws/build 66 | catkin_ws/devel 67 | src/catkin_ws/build 68 | src/catkin_ws/devel 69 | catkin_ws/src/CMakeLists.txt 70 | catkin_ws/.catkin_tools 71 | catkin_ws/logs 72 | 73 | build 74 | Release 75 | test_franka-interface 76 | franka-interface/build 77 | franka-interface-common/build 78 | 79 | # Ipython notebook ignore 80 | notebooks/.ipynb_checkpoints 81 | 82 | *cmake-build-debug 83 | *imgui.ini 84 | *egg-info 85 | 86 | # Vscode 87 | .vscode 88 | 89 | # Saved data 90 | catkin_ws/src/franka_ros_interface/out* 91 | 92 | # Docker 93 | .docker_tmp 94 | 95 | # Output files 96 | outs/ 97 | *.ipynb_checkpoints 98 | 99 | libfranka/ 100 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "catkin_ws/src/franka-interface-msgs"] 2 | path = catkin_ws/src/franka-interface-msgs 3 | url = https://github.com/iamlab-cmu/franka-interface-msgs.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 3.4) 2 | project (franka-interface 3 | VERSION 0.0.1 4 | LANGUAGES CXX 5 | ) 6 | 7 | list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_SOURCE_DIR}/cmake) 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 12 | 13 | add_compile_options(-Wall -Wextra) 14 | 15 | find_package(Eigen3 REQUIRED) 16 | find_package(Boost COMPONENTS program_options REQUIRED) 17 | 18 | include_directories(${Boost_INCLUDE_DIRS}) 19 | 20 | add_subdirectory(franka-interface) 21 | add_subdirectory(franka-interface-common) 22 | add_subdirectory(libfranka) 23 | 24 | add_executable(franka_interface src/franka_interface.cpp) 25 | 26 | target_link_libraries(franka_interface 27 | franka-interface 28 | franka-interface-common 29 | Franka::Franka 30 | Eigen3::Eigen3 31 | ${Boost_LIBRARIES} 32 | ) 33 | 34 | install(TARGETS franka_interface DESTINATION bin) 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Carnegie Mellon University 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /bash_scripts/clone_libfranka.sh: -------------------------------------------------------------------------------- 1 | firmware_version=$1 2 | 3 | valid_version=true 4 | case $firmware_version in 5 | 2) 6 | commit=5ca631e 7 | ;; 8 | 3) 9 | commit=80197c6 10 | ;; 11 | 4) 12 | commit=f1f46fb 13 | ;; 14 | 5) 15 | commit=83e931c 16 | ;; 17 | 6) 18 | commit=4f9e3cc 19 | ;; 20 | *) 21 | valid_version=false 22 | echo "Please enter your robot's firmware version as 2, 3, 4, 5, or 6." 23 | ;; 24 | esac 25 | 26 | if [ "$valid_version" = true ] ; then 27 | rm -rf libfranka 28 | git clone https://github.com/frankaemika/libfranka.git 29 | cd libfranka 30 | git checkout $commit 31 | git submodule update --init --recursive 32 | fi 33 | -------------------------------------------------------------------------------- /bash_scripts/make_catkin.sh: -------------------------------------------------------------------------------- 1 | cd catkin_ws 2 | catkin build 3 | source devel/setup.bash 4 | cd .. 5 | -------------------------------------------------------------------------------- /bash_scripts/make_franka_interface.sh: -------------------------------------------------------------------------------- 1 | # Get CPU core count 2 | n_cores=$(nproc) 3 | 4 | [ -d build ] || mkdir build 5 | cd build 6 | cmake -DCMAKE_BUILD_TYPE=Release .. -DCMAKE_CXX_FLAGS="-Wno-unused-variable -Wno-unused-parameter -Wno-unused-but-set-variable" && make -j$n_cores 7 | cd .. -------------------------------------------------------------------------------- /bash_scripts/make_libfranka.sh: -------------------------------------------------------------------------------- 1 | cd libfranka 2 | 3 | test_line_number=$(grep -n "Build tests" CMakeLists.txt | cut -f1 -d:) 4 | 5 | # Change built test to Off 6 | sed -i $test_line_number's/.*/option(BUILD_TESTS "Build tests" OFF)/' CMakeLists.txt 7 | 8 | # Get CPU core count 9 | n_cores=$(grep ^cpu\\scores /proc/cpuinfo | uniq | awk '{print $4}') 10 | 11 | # Build 12 | mkdir build && cd build 13 | cmake -DCMAKE_BUILD_TYPE=Release .. 14 | cmake --build . --config -- -j$n_cores 15 | 16 | cd ../.. 17 | 18 | # Copy needed files from libfranka cmake. -n means don't copy if dest. file exists 19 | [ -d cmake ] || mkdir cmake 20 | cp -n libfranka/cmake/FindEigen3.cmake cmake/ 21 | cp -n libfranka/cmake/FindPoco.cmake cmake/ -------------------------------------------------------------------------------- /bash_scripts/run_franka_interface.sh: -------------------------------------------------------------------------------- 1 | ./build/franka_interface -------------------------------------------------------------------------------- /bash_scripts/set_rosmaster.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | control_pc_ip_address=$1 4 | workstation_ip_address=$2 5 | 6 | export ROS_MASTER_URI="http://$workstation_ip_address:11311" 7 | export ROS_HOSTNAME="${control_pc_ip_address}" 8 | export ROS_IP="${control_pc_ip_address}" 9 | -------------------------------------------------------------------------------- /catkin_ws/.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/.gitignore: -------------------------------------------------------------------------------- 1 | Compiled source # 2 | ################### 3 | *.com 4 | *.class 5 | *.dll 6 | *.exe 7 | *.o 8 | *.so 9 | *.pyc 10 | *.pyo 11 | *.out 12 | 13 | # Packages # 14 | ############ 15 | # it's better to unpack these files and commit the raw source 16 | # git has its own built in compression methods 17 | *.7z 18 | *.dmg 19 | *.gz 20 | *.iso 21 | *.jar 22 | *.rar 23 | *.tar 24 | *.zip 25 | 26 | # Logs and databases # 27 | ###################### 28 | *.log 29 | *.sql 30 | *.sqlite 31 | 32 | # OS generated files # 33 | ###################### 34 | .DS_Store 35 | .DS_Store? 36 | ._* 37 | .Spotlight-V100 38 | .Trashes 39 | ehthumbs.db 40 | Thumbs.db 41 | 42 | # IDE generated files # 43 | ###################### 44 | .classpath 45 | .project 46 | .settings 47 | .idea 48 | .metadata 49 | *.cbp 50 | *.iml 51 | *.ipr 52 | *.swp 53 | *.swo 54 | 55 | # CMakeFiles # 56 | ###################### 57 | *.cmake 58 | CMakeFiles/ 59 | Makefile 60 | CMakeCache.txt 61 | franka-interface/cmake-build-debug/ 62 | 63 | # Add workspace build files 64 | src/catkin_ws/build 65 | src/catkin_ws/devel 66 | 67 | build 68 | test_franka_interface 69 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/include/franka_ros_interface/execute_skill_action_server.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_ROS_INTERFACE_EXECUTE_SKILL_ACTION_SERVER_H 2 | #define FRANKA_ROS_INTERFACE_EXECUTE_SKILL_ACTION_SERVER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include // Note: "Action" is appended 11 | #include 12 | #include 13 | 14 | #include "franka_ros_interface/shared_memory_handler.h" 15 | 16 | namespace franka_ros_interface 17 | { 18 | class ExecuteSkillActionServer 19 | { 20 | protected: 21 | 22 | ros::NodeHandle nh_; 23 | actionlib::SimpleActionServer as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs. 24 | std::string action_name_; 25 | 26 | double publish_frequency_; 27 | 28 | // create messages that are used to published feedback/result 29 | franka_interface_msgs::ExecuteSkillFeedback feedback_; 30 | franka_interface_msgs::ExecuteSkillResult result_; 31 | 32 | franka_ros_interface::SharedMemoryHandler shared_memory_handler_; 33 | franka_interface_msgs::FrankaInterfaceStatus franka_interface_status_; 34 | 35 | public: 36 | 37 | ExecuteSkillActionServer(std::string name); 38 | 39 | ~ExecuteSkillActionServer(void){} 40 | 41 | void executeCB(const franka_interface_msgs::ExecuteSkillGoalConstPtr &goal); 42 | 43 | }; 44 | } 45 | 46 | #endif // FRANKA_ROS_INTERFACE_EXECUTE_SKILL_ACTION_SERVER_H -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/include/franka_ros_interface/franka_interface_status_publisher.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_ROS_INTERFACE_ROBOT_STATE_PUBLISHER_H 2 | #define FRANKA_ROS_INTERFACE_ROBOT_STATE_PUBLISHER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "franka_interface_msgs/FrankaInterfaceStatus.h" 12 | #include "franka_ros_interface/shared_memory_handler.h" 13 | 14 | namespace franka_ros_interface 15 | { 16 | class FrankaInterfaceStatusPublisher 17 | { 18 | protected: 19 | 20 | ros::NodeHandle nh_; 21 | ros::Publisher franka_interface_status_pub_; // NodeHandle instance must be created before this line. Otherwise strange error occurs. 22 | std::string topic_name_; 23 | 24 | franka_interface_msgs::FrankaInterfaceStatus last_franka_interface_status_; 25 | bool has_seen_one_franka_interface_status_; 26 | int stale_count = 0; 27 | 28 | double publish_frequency_; 29 | franka_ros_interface::SharedMemoryHandler *shared_memory_handler_; 30 | 31 | public: 32 | 33 | FrankaInterfaceStatusPublisher(std::string name); 34 | 35 | ~FrankaInterfaceStatusPublisher(void){} 36 | 37 | }; 38 | } 39 | 40 | #endif // FRANKA_ROS_INTERFACE_ROBOT_STATE_PUBLISHER_H -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/include/franka_ros_interface/get_current_franka_interface_status_server.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_ROS_INTERFACE_GET_CURRENT_FRANKA_INTERFACE_STATUS_SERVER_H 2 | #define FRANKA_ROS_INTERFACE_GET_CURRENT_FRANKA_INTERFACE_STATUS_SERVER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "franka_interface_msgs/GetCurrentFrankaInterfaceStatusCmd.h" 10 | #include "franka_interface_msgs/FrankaInterfaceStatus.h" 11 | 12 | #include 13 | 14 | namespace franka_ros_interface 15 | { 16 | class GetCurrentFrankaInterfaceStatusServer 17 | { 18 | protected: 19 | 20 | ros::NodeHandle nh_; 21 | ros::ServiceServer server; // NodeHandle instance must be created before this line. Otherwise strange error occurs. 22 | std::string franka_interface_status_topic_name_; 23 | static std::mutex current_franka_interface_status_mutex_; 24 | static franka_interface_msgs::FrankaInterfaceStatus current_franka_interface_status_; 25 | 26 | public: 27 | 28 | GetCurrentFrankaInterfaceStatusServer(std::string name); 29 | 30 | ~GetCurrentFrankaInterfaceStatusServer(void){} 31 | 32 | static bool get_current_franka_interface_status(franka_interface_msgs::GetCurrentFrankaInterfaceStatusCmd::Request &req, franka_interface_msgs::GetCurrentFrankaInterfaceStatusCmd::Response &res); 33 | static void franka_interface_status_sub_cb(const franka_interface_msgs::FrankaInterfaceStatus& franka_interface_status); 34 | 35 | }; 36 | } 37 | 38 | #endif // FRANKA_ROS_INTERFACE_GET_CURRENT_FRANKA_INTERFACE_STATUS_SERVER_H -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/include/franka_ros_interface/get_current_robot_state_server.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_ROS_INTERFACE_GET_CURRENT_ROBOT_STATE_SERVER_H 2 | #define FRANKA_ROS_INTERFACE_GET_CURRENT_ROBOT_STATE_SERVER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "franka_interface_msgs/GetCurrentRobotStateCmd.h" 10 | #include "franka_interface_msgs/RobotState.h" 11 | 12 | namespace franka_ros_interface 13 | { 14 | class GetCurrentRobotStateServer 15 | { 16 | protected: 17 | 18 | ros::NodeHandle nh_; 19 | ros::ServiceServer server; // NodeHandle instance must be created before this line. Otherwise strange error occurs. 20 | std::string robot_state_topic_name_; 21 | static std::mutex current_robot_state_mutex_; 22 | static franka_interface_msgs::RobotState current_robot_state_; 23 | 24 | public: 25 | 26 | GetCurrentRobotStateServer(std::string name); 27 | 28 | ~GetCurrentRobotStateServer(void){} 29 | 30 | static bool get_current_robot_state(franka_interface_msgs::GetCurrentRobotStateCmd::Request &req, franka_interface_msgs::GetCurrentRobotStateCmd::Response &res); 31 | static void robot_state_sub_cb(const franka_interface_msgs::RobotState& robot_state); 32 | 33 | }; 34 | } 35 | 36 | #endif // FRANKA_ROS_INTERFACE_GET_CURRENT_ROBOT_STATE_SERVER_H -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/include/franka_ros_interface/robot_state_publisher.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_ROS_INTERFACE_ROBOT_STATE_PUBLISHER_H 2 | #define FRANKA_ROS_INTERFACE_ROBOT_STATE_PUBLISHER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include // Note: "Action" is appended 14 | 15 | #include "franka_ros_interface/shared_memory_handler.h" 16 | 17 | namespace franka_ros_interface 18 | { 19 | class RobotStatePublisher 20 | { 21 | protected: 22 | 23 | ros::NodeHandle nh_; 24 | ros::Publisher robot_state_pub_; // NodeHandle instance must be created before this line. Otherwise strange error occurs. 25 | std::string topic_name_; 26 | 27 | double publish_frequency_; 28 | franka_ros_interface::SharedMemoryHandler shared_memory_handler_; 29 | 30 | bool has_seen_one_robot_state_; 31 | franka_interface_msgs::RobotState last_robot_state_; 32 | std::array last_robot_frames_; 33 | 34 | void BroadcastRobotFrames(); 35 | 36 | const double finger_offset_ = 0.0584; 37 | tf::TransformBroadcaster br_; 38 | const std::vector frame_names_ = {"panda_link1", "panda_link2", "panda_link3", 39 | "panda_link4", "panda_link5", "panda_link6", 40 | "panda_link7", "panda_link8", "panda_end_effector"}; 41 | 42 | public: 43 | 44 | RobotStatePublisher(std::string name); 45 | 46 | ~RobotStatePublisher(void){} 47 | 48 | }; 49 | } 50 | 51 | #endif // FRANKA_ROS_INTERFACE_ROBOT_STATE_PUBLISHER_H -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/include/franka_ros_interface/run_loop_process_info_state_publisher.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_ROS_INTERFACE_RUN_LOOP_PROCESS_INFO_STATE_PUBLISHER_H 2 | #define FRANKA_ROS_INTERFACE_RUN_LOOP_PROCESS_INFO_STATE_PUBLISHER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "franka_ros_interface/shared_memory_handler.h" 13 | 14 | namespace franka_ros_interface 15 | { 16 | class RunLoopProcessInfoStatePublisher 17 | { 18 | protected: 19 | 20 | ros::NodeHandle nh_; 21 | ros::Publisher run_loop_process_info_state_pub_; // NodeHandle instance must be created before this line. Otherwise strange error occurs. 22 | std::string topic_name_; 23 | 24 | double publish_frequency_; 25 | franka_ros_interface::SharedMemoryHandler shared_memory_handler_; 26 | 27 | bool has_seen_one_run_loop_process_info_state_; 28 | franka_interface_msgs::RunLoopProcessInfoState last_run_loop_process_info_state_; 29 | 30 | public: 31 | 32 | RunLoopProcessInfoStatePublisher(std::string name); 33 | 34 | ~RunLoopProcessInfoStatePublisher(void){} 35 | 36 | }; 37 | } 38 | 39 | #endif // FRANKA_ROS_INTERFACE_RUN_LOOP_PROCESS_INFO_STATE_PUBLISHER_H -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/include/franka_ros_interface/sensor_data/sensor_subscriber_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_ROS_INTERFACE_SENSOR_SUBSCRIBER_HANDLER_H 2 | #define FRANKA_ROS_INTERFACE_SENSOR_SUBSCRIBER_HANDLER_H 3 | 4 | #include 5 | #include "franka_interface_msgs/SensorDataGroup.h" 6 | #include "franka_ros_interface/shared_memory_handler.h" 7 | 8 | namespace franka_ros_interface 9 | { 10 | class SensorSubscriberHandler 11 | { 12 | protected: 13 | 14 | ros::NodeHandle nh_; 15 | 16 | double write_frequency_; 17 | SharedMemoryHandler shared_memory_handler_; 18 | 19 | public: 20 | 21 | SensorSubscriberHandler(ros::NodeHandle& nh); 22 | ~SensorSubscriberHandler(){}; 23 | 24 | void SensorSubscriberCallback(const franka_interface_msgs::SensorDataGroup::ConstPtr& sensor_group_msg); 25 | }; 26 | } 27 | 28 | #endif // -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/launch/franka_gripper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | $(arg joint_names) 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/launch/franka_ros_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | franka_ros_interface 4 | 0.0.0 5 | The franka_ros_interface package 6 | 7 | 8 | 9 | 10 | Kevin Zhang 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | actionlib 53 | actionlib_msgs 54 | roscpp 55 | rospy 56 | franka_interface_msgs 57 | 58 | catkin 59 | message_runtime 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/execute_skill_action_server_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "franka_ros_interface/execute_skill_action_server.h" 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "execute_skill_action_server_node", ros::init_options::AnonymousName); 8 | 9 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); 10 | 11 | franka_ros_interface::ExecuteSkillActionServer execute_skill_action_server("execute_skill"); 12 | ros::spin(); 13 | 14 | return 0; 15 | } -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/franka_interface_status_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "franka_ros_interface/franka_interface_status_publisher.h" 2 | 3 | namespace franka_ros_interface 4 | { 5 | FrankaInterfaceStatusPublisher::FrankaInterfaceStatusPublisher(std::string name) : nh_("~"), 6 | topic_name_(name) 7 | { 8 | nh_.param("publish_frequency", publish_frequency_, (double) 100.0); 9 | franka_interface_status_pub_ = nh_.advertise(topic_name_, 100); 10 | 11 | shared_memory_handler_ = new franka_ros_interface::SharedMemoryHandler(); 12 | 13 | ROS_INFO("FrankaInterface Status Publisher Started"); 14 | 15 | ros::Rate loop_rate(publish_frequency_); 16 | while (ros::ok()) 17 | { 18 | // get franka_interface 19 | franka_interface_msgs::FrankaInterfaceStatus franka_interface_status_ = shared_memory_handler_->getFrankaInterfaceStatus(); 20 | 21 | if (franka_interface_status_.is_fresh) { 22 | last_franka_interface_status_ = franka_interface_status_; 23 | stale_count = 0; 24 | has_seen_one_franka_interface_status_ = true; 25 | } else { 26 | // use previous franka_interface_status if available 27 | if (has_seen_one_franka_interface_status_) { 28 | franka_interface_status_ = last_franka_interface_status_; 29 | stale_count++; 30 | } 31 | } 32 | 33 | // only proceed if received at least 1 franka_interface status 34 | if (has_seen_one_franka_interface_status_) { 35 | // TODO(jacky): MAGIC NUMBER - Signal not ok if 10 consecutive franka_interface statuses have been stale. Roughly 100 ms 36 | if (stale_count > 10) { 37 | franka_interface_status_.is_ready = false; 38 | franka_interface_status_.is_fresh = false; 39 | } 40 | 41 | // publish 42 | franka_interface_status_pub_.publish(franka_interface_status_); 43 | 44 | // increment watchdog counter 45 | shared_memory_handler_->incrementWatchdogCounter(); 46 | } 47 | 48 | ros::spinOnce(); 49 | loop_rate.sleep(); 50 | } 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/franka_interface_status_publisher_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "franka_ros_interface/franka_interface_status_publisher.h" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "franka_interface_status_publisher_node", ros::init_options::AnonymousName); 7 | 8 | franka_ros_interface::FrankaInterfaceStatusPublisher franka_interface_status_publisher("franka_interface_status"); 9 | 10 | return 0; 11 | } -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/get_current_franka_interface_status_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "franka_ros_interface/get_current_franka_interface_status_server.h" 3 | 4 | namespace franka_ros_interface 5 | { 6 | std::mutex GetCurrentFrankaInterfaceStatusServer::current_franka_interface_status_mutex_; 7 | franka_interface_msgs::FrankaInterfaceStatus GetCurrentFrankaInterfaceStatusServer::current_franka_interface_status_; 8 | 9 | GetCurrentFrankaInterfaceStatusServer::GetCurrentFrankaInterfaceStatusServer(std::string name) : nh_("~") 10 | { 11 | nh_.param("franka_interface_status_topic_name", franka_interface_status_topic_name_, std::string("/franka_interface_status_publisher_node/franka_interface_status")); 12 | 13 | ros::Subscriber sub = nh_.subscribe(franka_interface_status_topic_name_, 10, franka_interface_status_sub_cb); 14 | ros::ServiceServer service = nh_.advertiseService("get_current_franka_interface_status_server", get_current_franka_interface_status); 15 | ROS_INFO("Get Current FrankaInterface Status Server Started"); 16 | ros::spin(); 17 | } 18 | 19 | void GetCurrentFrankaInterfaceStatusServer::franka_interface_status_sub_cb(const franka_interface_msgs::FrankaInterfaceStatus& franka_interface_status) 20 | { 21 | if (current_franka_interface_status_mutex_.try_lock()) { 22 | current_franka_interface_status_ = franka_interface_status; 23 | current_franka_interface_status_mutex_.unlock(); 24 | } 25 | } 26 | 27 | bool GetCurrentFrankaInterfaceStatusServer::get_current_franka_interface_status(franka_interface_msgs::GetCurrentFrankaInterfaceStatusCmd::Request &req, 28 | franka_interface_msgs::GetCurrentFrankaInterfaceStatusCmd::Response &res) 29 | { 30 | ROS_DEBUG("Get Current FrankaInterface Status Server request received."); 31 | current_franka_interface_status_mutex_.lock(); 32 | res.franka_interface_status = current_franka_interface_status_; 33 | current_franka_interface_status_mutex_.unlock(); 34 | ROS_DEBUG("Get Current FrankaInterface Status Servier request processed."); 35 | 36 | return true; 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/get_current_franka_interface_status_server_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "franka_ros_interface/get_current_franka_interface_status_server.h" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "get_franka_interface_status_server", ros::init_options::AnonymousName); 7 | 8 | franka_ros_interface::GetCurrentFrankaInterfaceStatusServer get_current_franka_interface_status_service("get_franka_interface_status_server"); 9 | 10 | return 0; 11 | } -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/get_current_robot_state_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "franka_ros_interface/get_current_robot_state_server.h" 3 | 4 | namespace franka_ros_interface 5 | { 6 | std::mutex GetCurrentRobotStateServer::current_robot_state_mutex_; 7 | franka_interface_msgs::RobotState GetCurrentRobotStateServer::current_robot_state_; 8 | 9 | GetCurrentRobotStateServer::GetCurrentRobotStateServer(std::string name) : nh_("~") 10 | { 11 | nh_.param("robot_state_topic_name", robot_state_topic_name_, std::string("/robot_state_publisher_node/robot_state")); 12 | 13 | ros::Subscriber sub = nh_.subscribe(robot_state_topic_name_, 10, robot_state_sub_cb); 14 | ros::ServiceServer service = nh_.advertiseService("get_current_robot_state_server", get_current_robot_state); 15 | ROS_INFO("Get Current Robot State Server Started"); 16 | ros::spin(); 17 | } 18 | 19 | void GetCurrentRobotStateServer::robot_state_sub_cb(const franka_interface_msgs::RobotState& robot_state) 20 | { 21 | if (current_robot_state_mutex_.try_lock()) { 22 | current_robot_state_ = robot_state; 23 | current_robot_state_mutex_.unlock(); 24 | } 25 | } 26 | 27 | bool GetCurrentRobotStateServer::get_current_robot_state(franka_interface_msgs::GetCurrentRobotStateCmd::Request &req, 28 | franka_interface_msgs::GetCurrentRobotStateCmd::Response &res) 29 | { 30 | ROS_DEBUG("Get Current Robot State Server request received."); 31 | current_robot_state_mutex_.lock(); 32 | res.robot_state = current_robot_state_; 33 | current_robot_state_mutex_.unlock(); 34 | ROS_DEBUG("Get Current Robot State Server request processed."); 35 | 36 | return true; 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/get_current_robot_state_server_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "franka_ros_interface/get_current_robot_state_server.h" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "get_robot_state_server", ros::init_options::AnonymousName); 7 | 8 | franka_ros_interface::GetCurrentRobotStateServer get_current_robot_state_service("get_robot_state_server"); 9 | 10 | return 0; 11 | } -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/robot_state_publisher_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "franka_ros_interface/robot_state_publisher.h" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "robot_state_publisher_node", ros::init_options::AnonymousName); 7 | 8 | franka_ros_interface::RobotStatePublisher robot_state_publisher("robot_state"); 9 | 10 | return 0; 11 | } -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/run_loop_process_info_state_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "franka_ros_interface/run_loop_process_info_state_publisher.h" 2 | 3 | namespace franka_ros_interface 4 | { 5 | RunLoopProcessInfoStatePublisher::RunLoopProcessInfoStatePublisher(std::string name) : nh_("~"), 6 | topic_name_(name), 7 | shared_memory_handler_() 8 | { 9 | nh_.param("publish_frequency", publish_frequency_, (double) 100.0); 10 | run_loop_process_info_state_pub_ = nh_.advertise(topic_name_, 100); 11 | 12 | ROS_INFO("Run Loop Process Info State Publisher Started"); 13 | 14 | ros::Rate loop_rate(publish_frequency_); 15 | while (ros::ok()) 16 | { 17 | franka_interface_msgs::RunLoopProcessInfoState run_loop_process_info_state_ = shared_memory_handler_.getRunLoopProcessInfoState(); 18 | 19 | if (run_loop_process_info_state_.is_fresh) { 20 | run_loop_process_info_state_pub_.publish(run_loop_process_info_state_); 21 | 22 | has_seen_one_run_loop_process_info_state_ = true; 23 | last_run_loop_process_info_state_ = run_loop_process_info_state_; 24 | last_run_loop_process_info_state_.is_fresh = false; 25 | } else { 26 | if (has_seen_one_run_loop_process_info_state_) { 27 | run_loop_process_info_state_pub_.publish(last_run_loop_process_info_state_); 28 | } 29 | } 30 | 31 | ros::spinOnce(); 32 | loop_rate.sleep(); 33 | } 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/run_loop_process_info_state_publisher_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "franka_ros_interface/run_loop_process_info_state_publisher.h" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "run_loop_process_info_state_publisher_node", ros::init_options::AnonymousName); 7 | 8 | franka_ros_interface::RunLoopProcessInfoStatePublisher run_loop_process_info_state_publisher("run_loop_process_info_state"); 9 | 10 | return 0; 11 | } -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/sensor_data/sensor_subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include "franka_ros_interface/sensor_data/sensor_subscriber_handler.h" 2 | 3 | int main(int argc, char **argv) { 4 | ros::init(argc, argv, "sensor_data_subscriber_node", ros::init_options::AnonymousName); 5 | 6 | ros::NodeHandle n("~"); 7 | franka_ros_interface::SensorSubscriberHandler handler(n); 8 | int robot_num; 9 | n.param("robot_num", robot_num, 1); 10 | ros::Subscriber sub; 11 | if (robot_num == 1) 12 | { 13 | sub = n.subscribe("/franka_ros_interface/sensor", 10, &franka_ros_interface::SensorSubscriberHandler::SensorSubscriberCallback, &handler); 14 | } 15 | else 16 | { 17 | std::string robot_num_string = std::to_string(robot_num); 18 | sub = n.subscribe("/franka_ros_interface_"+robot_num_string+"/sensor", 10, &franka_ros_interface::SensorSubscriberHandler::SensorSubscriberCallback, &handler); 19 | } 20 | 21 | ros::spin(); 22 | 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /catkin_ws/src/franka_ros_interface/src/sensor_data/sensor_subscriber_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "franka_ros_interface/sensor_data/sensor_subscriber_handler.h" 2 | 3 | namespace franka_ros_interface 4 | { 5 | SensorSubscriberHandler::SensorSubscriberHandler(ros::NodeHandle& nh) 6 | : shared_memory_handler_ () 7 | , nh_ (nh) { 8 | ROS_INFO("created_sensor_subscriber_handler"); 9 | } 10 | 11 | void SensorSubscriberHandler::SensorSubscriberCallback(const franka_interface_msgs::SensorDataGroup::ConstPtr& sensor_group_msg) { 12 | ROS_INFO("Got sensor message! TG %s | FC %s | TH %s", 13 | sensor_group_msg->has_trajectory_generator_sensor_data ? "yes" : "no", 14 | sensor_group_msg->has_feedback_controller_sensor_data ? "yes" : "no", 15 | sensor_group_msg->has_termination_handler_sensor_data ? "yes" : "no" 16 | ); 17 | 18 | shared_memory_handler_.tryToLoadSensorDataGroupIntoSharedMemory(sensor_group_msg); 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /docs/.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | /_build 3 | /doctrees 4 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/buildDocs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -x 3 | ################################################################################ 4 | # File: buildDocs.sh 5 | # Purpose: Script that builds our documentation using sphinx and updates GitHub 6 | # Pages. This script is executed by: 7 | # .github/workflows/docs_pages_workflow.yml 8 | # 9 | # Authors: Michael Altfield 10 | # Created: 2020-07-17 11 | # Updated: 2020-07-17 12 | # Version: 0.1 13 | ################################################################################ 14 | 15 | ################### 16 | # INSTALL DEPENDS # 17 | ################### 18 | 19 | apt-get update 20 | apt-get -y install git rsync python3-sphinx python3-sphinx-rtd-theme 21 | 22 | ##################### 23 | # DECLARE VARIABLES # 24 | ##################### 25 | 26 | git config --global --add safe.directory /__w/franka-interface/franka-interface 27 | pwd 28 | ls -lah 29 | export SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct) 30 | 31 | ############## 32 | # BUILD DOCS # 33 | ############## 34 | 35 | # build our documentation with sphinx (see docs/conf.py) 36 | # * https://www.sphinx-doc.org/en/master/usage/quickstart.html#running-the-build 37 | make -C docs clean 38 | make -C docs html 39 | 40 | ####################### 41 | # Update GitHub Pages # 42 | ####################### 43 | 44 | git config --global user.name "${GITHUB_ACTOR}" 45 | git config --global user.email "${GITHUB_ACTOR}@users.noreply.github.com" 46 | 47 | docroot=`mktemp -d` 48 | rsync -av "docs/_build/html/" "${docroot}/" 49 | 50 | pushd "${docroot}" 51 | 52 | # don't bother maintaining history; just generate fresh 53 | git init 54 | git remote add deploy "https://token:${GITHUB_TOKEN}@github.com/${GITHUB_REPOSITORY}.git" 55 | git checkout -b gh-pages 56 | 57 | # add .nojekyll to the root so that github won't 404 on content added to dirs 58 | # that start with an underscore (_), such as our "_content" dir.. 59 | touch .nojekyll 60 | 61 | # Add README 62 | cat > README.md <`_. 12 | 13 | If this library proves useful to your research, please cite the paper below:: 14 | 15 | @article{zhang2020modular, 16 | title={A modular robotic arm control stack for research: Franka-interface and frankapy}, 17 | author={Zhang, Kevin and Sharma, Mohit and Liang, Jacky and Kroemer, Oliver}, 18 | journal={arXiv preprint arXiv:2011.02398}, 19 | year={2020} 20 | } 21 | 22 | Note that this library has been released with the MIT license. 23 | 24 | .. toctree:: 25 | :maxdepth: 2 26 | 27 | install 28 | network 29 | support 30 | 31 | 32 | 33 | Indices and tables 34 | ================== 35 | 36 | * :ref:`genindex` 37 | * :ref:`modindex` 38 | * :ref:`search` 39 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=. 11 | set BUILDDIR=_build 12 | 13 | if "%1" == "" goto help 14 | 15 | %SPHINXBUILD% >NUL 2>NUL 16 | if errorlevel 9009 ( 17 | echo. 18 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 19 | echo.installed, then set the SPHINXBUILD environment variable to point 20 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 21 | echo.may add the Sphinx directory to PATH. 22 | echo. 23 | echo.If you don't have Sphinx installed, grab it from 24 | echo.http://sphinx-doc.org/ 25 | exit /b 1 26 | ) 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /docs/network.rst: -------------------------------------------------------------------------------- 1 | Network Configuration 2 | ===================== 3 | 4 | Setting Up the Robot 5 | -------------------- 6 | 1. If this is the first time you are using the robot, please follow the Franka Robot Setup instructions located in the booklet that accompanies the robot and set the username and password for the robot. 7 | 8 | 2. Next set the Robot Network to be ``172.16.0.2`` with a netmask of ``255.255.255.0`` as shown in the photo below. 9 | 10 | .. image:: imgs/franka_network.png 11 | :width: 600 12 | :alt: Franka Network Photo 13 | 14 | 15 | Editing the Ethernet Network Configuration 16 | ------------------------------------------ 17 | 1. Insert the ethernet cable from the Black Franka Control box to the Control PC. 18 | 2. Turn on the Franka Control box. 19 | 3. Go to Edit Connections in the Ubuntu Network Connections Menu of your Control PC. 20 | 4. Select the Ethernet connection that corresponds to the port that you plugged the ethernet cable into and then click edit. 21 | 5. Go to the IPv4 Settings Tab and switch from Automatic (DHCP) to Manual. 22 | 6. Add an address of ``172.16.0.1`` with netmask ``255.255.255.0`` as shown in the picture below and then click save. 23 | .. image:: imgs/control_pc_network.png 24 | :width: 600 25 | :alt: Franka Network Photo 26 | 27 | 7. Check to see if you can ping ``172.16.0.2`` from any terminal. 28 | 8. If any issues arise, refer to here: `https://frankaemika.github.io/docs/troubleshooting.html#robot-is-not-reachable `_ 29 | 30 | Entering Franka Desk 31 | -------------------- 32 | 1. Go to the web browser and type in ``https://172.16.0.2/`` 33 | 2. Add an exception if there is a security certificate error. 34 | 3. Login to the robot with the correct credentials or complete setup with the robot. You should now be able to see the Franka Desk GUI in the web browser. 35 | 4. If the username and password do not work, there is a method of factory resetting the Franka located here: `https://www.franka-community.de/t/reset-admin-password-or-reset-to-factory-settings/184 `_ 36 | 37 | 38 | Making sure Franka Control Interface is Installed 39 | ------------------------------------------------- 40 | 1. Go to the Settings menu of Franka Desk using the dropdown menu in the upper right corner. 41 | 2. Click on System on the left hand side. 42 | 3. Make sure Franka Control Interface (FCI) is installed as shown in the photo below. 43 | .. image:: imgs/franka_control_interface.png 44 | :width: 600 45 | :alt: Franka Network Photo 46 | 4. If it is not installed, our Franka-Interface will not work, so you will need to contact the distributor you purchased the Franka Panda from or Franka Emika to receive the Feature file. 47 | 5. Then you will need to press the install feature button and upload the file to the Franka Control Box. 48 | 6. The Franka may need to be rebooted before it shows up as installed. 49 | 50 | Now you are ready to install FrankaPy to control the robot following instructions `here `_. 51 | -------------------------------------------------------------------------------- /docs/support.rst: -------------------------------------------------------------------------------- 1 | Support 2 | ======= 3 | 4 | The easiest way to get help with the library is to join the FrankaPy and Franka-Interface Discord using this link `here `_. -------------------------------------------------------------------------------- /franka-interface-common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 3.4) 2 | project (franka-interface-common 3 | VERSION 0.0.1 4 | LANGUAGES CXX 5 | ) 6 | 7 | set(CMAKE_CXX_STANDARD 14) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 10 | 11 | ## Dependencies 12 | # find_package(Poco REQUIRED COMPONENTS Net Foundation) 13 | find_package(Eigen3 REQUIRED) 14 | 15 | set(THREADS_PREFER_PTHREAD_FLAG ON) 16 | find_package(Threads REQUIRED) 17 | 18 | find_package(Boost COMPONENTS REQUIRED filesystem system thread) 19 | 20 | include_directories(${Boost_INCLUDE_DIRS}) 21 | 22 | ## Library 23 | add_library(franka-interface-common SHARED 24 | src/franka_interface_state_info.cpp 25 | src/run_loop_process_info.cpp 26 | src/SharedMemoryInfo.cpp 27 | ) 28 | 29 | target_link_libraries(franka-interface-common PUBLIC 30 | Threads::Threads 31 | ${Boost_LIBRARIES} 32 | rt 33 | ) 34 | 35 | 36 | target_include_directories(franka-interface-common PUBLIC 37 | $ 38 | $ 39 | PRIVATE src 40 | ) 41 | 42 | ## Installation 43 | include(GNUInstallDirs) 44 | install(TARGETS franka-interface-common 45 | EXPORT franka-interface-common 46 | LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" 47 | RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" 48 | PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/franka-interface-common" 49 | ) 50 | 51 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 52 | -------------------------------------------------------------------------------- /franka-interface-common/include/franka-interface-common/franka_interface_state_info.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class FrankaInterfaceStateInfo { 7 | public: 8 | FrankaInterfaceStateInfo(){}; 9 | 10 | bool get_is_ready(); 11 | void set_is_ready(bool is_ready); 12 | 13 | int get_watchdog_counter(); 14 | void reset_watchdog_counter(); // to be used by franka_interface 15 | void increment_watchdog_counter(); // to be used by franka_ros_interface 16 | 17 | std::string get_error_description(); 18 | void set_error_description(std::string description); // to be used by franka_interface 19 | void clear_error_description(); // to be used by franka_ros_interface 20 | 21 | private: 22 | bool is_ready_{false}; 23 | int watchdog_counter_{0}; 24 | char error_description_[1000]; 25 | size_t error_description_len_=0; 26 | }; 27 | 28 | -------------------------------------------------------------------------------- /franka-interface-common/src/franka_interface_state_info.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | bool FrankaInterfaceStateInfo::get_is_ready() { 6 | return is_ready_; 7 | } 8 | 9 | void FrankaInterfaceStateInfo::set_is_ready(bool is_ready) { 10 | is_ready_ = is_ready; 11 | } 12 | 13 | int FrankaInterfaceStateInfo::get_watchdog_counter() { 14 | return watchdog_counter_; 15 | } 16 | 17 | void FrankaInterfaceStateInfo::reset_watchdog_counter() { 18 | watchdog_counter_ = 0; 19 | } 20 | 21 | void FrankaInterfaceStateInfo::increment_watchdog_counter() { 22 | watchdog_counter_++; 23 | } 24 | 25 | std::string FrankaInterfaceStateInfo::get_error_description() { 26 | std::string desc(error_description_, 27 | error_description_ + sizeof(error_description_[0]) * error_description_len_); 28 | return desc; 29 | } 30 | 31 | void FrankaInterfaceStateInfo::set_error_description(std::string description){ 32 | std::memcpy(&error_description_, description.c_str(), description.size()); 33 | error_description_len_ = description.size(); 34 | } 35 | 36 | void FrankaInterfaceStateInfo::clear_error_description() { 37 | set_error_description(""); 38 | } -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/cartesian_impedance_feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_CARTESIAN_IMPEDANCE_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_CARTESIAN_IMPEDANCE_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/feedback_controller/feedback_controller.h" 7 | 8 | class CartesianImpedanceFeedbackController : public FeedbackController { 9 | public: 10 | using FeedbackController::FeedbackController; 11 | 12 | void parse_parameters() override; 13 | 14 | void initialize_controller(FrankaRobot *robot) override; 15 | 16 | void parse_sensor_data(const franka::RobotState &robot_state) override; 17 | 18 | void get_next_step(const franka::RobotState &robot_state, 19 | TrajectoryGenerator *traj_generator) override; 20 | 21 | protected: 22 | CartesianImpedanceFeedbackControllerMessage cartesian_impedance_feedback_params_; 23 | CartesianImpedanceSensorMessage cartesian_impedance_sensor_msg_; 24 | 25 | const franka::Model *model_; 26 | 27 | std::array translational_stiffnesses_ = {{600.0, 600.0, 600.0}}; 28 | std::array rotational_stiffnesses_ = {{50.0, 50.0, 50.0}}; 29 | Eigen::MatrixXd stiffness_; 30 | Eigen::MatrixXd damping_; 31 | }; 32 | 33 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_CARTESIAN_IMPEDANCE_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/ee_cartesian_impedance_feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_EE_CARTESIAN_IMPEDANCE_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_EE_CARTESIAN_IMPEDANCE_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/feedback_controller/cartesian_impedance_feedback_controller.h" 7 | 8 | class EECartesianImpedanceFeedbackController : public CartesianImpedanceFeedbackController { 9 | public: 10 | using CartesianImpedanceFeedbackController::CartesianImpedanceFeedbackController; 11 | 12 | void initialize_controller(FrankaRobot *robot) override; 13 | 14 | void get_next_step(const franka::RobotState &robot_state, 15 | TrajectoryGenerator *traj_generator) override; 16 | 17 | private: 18 | Eigen::Matrix rot_stiffness_ = Eigen::MatrixXd::Identity(3, 3); 19 | Eigen::Matrix transl_stiffness_ = Eigen::MatrixXd::Identity(3, 3); 20 | Eigen::Matrix rot_damping_ = Eigen::MatrixXd::Identity(3, 3); 21 | Eigen::Matrix transl_damping_ = Eigen::MatrixXd::Identity(3, 3); 22 | }; 23 | 24 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_EE_CARTESIAN_IMPEDANCE_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "franka-interface/trajectory_generator/trajectory_generator.h" 11 | #include "franka-interface/franka_robot.h" 12 | #include "feedback_controller_params_msg.pb.h" 13 | 14 | class FeedbackController { 15 | public: 16 | explicit FeedbackController(SharedBufferTypePtr p, SensorDataManager* sensor_data_manager) : 17 | params_{p}, 18 | sensor_data_manager_{sensor_data_manager} {}; 19 | 20 | /** 21 | * Parse parameters from memory. 22 | */ 23 | virtual void parse_parameters() = 0; 24 | 25 | /** 26 | * Initialize trajectory generation after parameter parsing. 27 | */ 28 | virtual void initialize_controller(FrankaRobot *robot) = 0; 29 | 30 | /** 31 | * Get next trajectory step. 32 | */ 33 | virtual void get_next_step(const franka::RobotState &robot_state, 34 | TrajectoryGenerator *traj_generator) = 0; 35 | 36 | /** 37 | * Parse sensor data 38 | */ 39 | virtual void parse_sensor_data(const franka::RobotState &robot_state) {}; 40 | 41 | std::array tau_d_array_{}; 42 | 43 | protected: 44 | SharedBufferTypePtr params_=0; 45 | SensorDataManager* sensor_data_manager_; 46 | }; 47 | 48 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/force_axis_impedence_feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FORCE_AXIS_IMPEDENCE_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FORCE_AXIS_IMPEDENCE_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/feedback_controller/feedback_controller.h" 7 | 8 | // An impedence controller that allows applying a force along a constrained 9 | // axis. 10 | class ForceAxisImpedenceFeedbackController : public FeedbackController { 11 | public: 12 | using FeedbackController::FeedbackController; 13 | 14 | void parse_parameters() override; 15 | 16 | void initialize_controller(FrankaRobot *robot) override; 17 | 18 | void get_next_step(const franka::RobotState &robot_state, 19 | TrajectoryGenerator *traj_generator) override; 20 | 21 | private: 22 | ForceAxisFeedbackControllerMessage force_axis_feedback_params_; 23 | 24 | const franka::Model *model_; 25 | 26 | double translational_stiffness_ = 600; 27 | double rotational_stiffness_ = 50; 28 | Eigen::MatrixXd stiffness_; 29 | Eigen::MatrixXd damping_; 30 | 31 | Eigen::Vector3d axis_; 32 | }; 33 | 34 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FORCE_AXIS_IMPEDENCE_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/force_position_feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FORCE_POSITION_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FORCE_POSITION_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/feedback_controller/feedback_controller.h" 7 | 8 | // A Hybrid Force Position Controller 9 | class ForcePositionFeedbackController : public FeedbackController { 10 | public: 11 | using FeedbackController::FeedbackController; 12 | 13 | void parse_parameters() override; 14 | 15 | void initialize_controller(FrankaRobot *robot) override; 16 | 17 | void get_next_step(const franka::RobotState &robot_state, 18 | TrajectoryGenerator *traj_generator) override; 19 | 20 | void parse_sensor_data(const franka::RobotState &robot_state) override; 21 | 22 | private: 23 | ForcePositionFeedbackControllerMessage force_position_feedback_params_; 24 | ForcePositionControllerSensorMessage force_position_sensor_msg_; 25 | const franka::Model *model_; 26 | bool use_cartesian_gains_; 27 | 28 | Eigen::Matrix position_kps_cart_ = Eigen::MatrixXd::Zero(6, 6); 29 | Eigen::Matrix position_kds_cart_ = Eigen::MatrixXd::Zero(6, 6); 30 | Eigen::Matrix force_kps_cart_ = Eigen::MatrixXd::Zero(6, 6); 31 | Eigen::Matrix force_kis_cart_ = Eigen::MatrixXd::Zero(6, 6); 32 | 33 | Eigen::Matrix position_kps_joint_ = Eigen::MatrixXd::Zero(7, 7); 34 | Eigen::Matrix position_kds_joint_ = Eigen::MatrixXd::Zero(7, 7); 35 | Eigen::Matrix force_kps_joint_ = Eigen::MatrixXd::Zero(7, 7); 36 | Eigen::Matrix force_kis_joint_ = Eigen::MatrixXd::Zero(7, 7); 37 | 38 | Eigen::Matrix S_ = Eigen::MatrixXd::Identity(6, 6); 39 | Eigen::Matrix Sp_= Eigen::MatrixXd::Zero(6, 6); 40 | 41 | Eigen::Matrix total_fes_ = Eigen::MatrixXd::Zero(6, 1); 42 | Eigen::Matrix total_tau_es_ = Eigen::MatrixXd::Zero(7, 1); 43 | 44 | Eigen::Matrix xe_, fe_, xes_, fes_; 45 | Eigen::Matrix tau_x_, tau_f_, tau_task_, tau_d_, q_es_, tau_es_; 46 | }; 47 | 48 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FORCE_POSITION_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/joint_impedance_feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_JOINT_IMPEDANCE_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_JOINT_IMPEDANCE_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include "franka-interface/feedback_controller/feedback_controller.h" 5 | 6 | #include 7 | 8 | class JointImpedanceFeedbackController : public FeedbackController { 9 | public: 10 | using FeedbackController::FeedbackController; 11 | 12 | void parse_parameters() override; 13 | 14 | void initialize_controller(FrankaRobot *robot) override; 15 | 16 | void get_next_step(const franka::RobotState &robot_state, TrajectoryGenerator *traj_generator) override; 17 | 18 | private: 19 | JointImpedanceFeedbackControllerMessage joint_impedance_feedback_params_; 20 | 21 | const franka::Model *model_; 22 | // Stiffness 23 | std::array k_gains_ = {{600.0, 600.0, 600.0, 600.0, 24 | 250.0, 150.0, 50.0}}; 25 | // Damping 26 | std::array d_gains_ = {{50.0, 50.0, 50.0, 50.0, 27 | 30.0, 25.0, 15.0}}; 28 | }; 29 | 30 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_JOINT_IMPEDANCE_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/noop_feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_NOOP_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_NOOP_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include "franka-interface/feedback_controller/feedback_controller.h" 5 | 6 | class NoopFeedbackController : public FeedbackController { 7 | public: 8 | using FeedbackController::FeedbackController; 9 | 10 | void parse_parameters() override; 11 | 12 | void initialize_controller(FrankaRobot *robot) override; 13 | 14 | void get_next_step(const franka::RobotState &robot_state, 15 | TrajectoryGenerator *traj_generator) override; 16 | }; 17 | 18 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_NOOP_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/pass_through_feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_PASSTHROUGH_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_PASSTHROUGH_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/feedback_controller/feedback_controller.h" 7 | 8 | // A passthrough feedback controller that just usese the desired force 9 | // torque of traj gen and passes it to the robot joint torques through 10 | // the jacobian 11 | class PassThroughFeedbackController : public FeedbackController { 12 | public: 13 | using FeedbackController::FeedbackController; 14 | 15 | void parse_parameters() override; 16 | 17 | void initialize_controller(FrankaRobot *robot) override; 18 | 19 | void get_next_step(const franka::RobotState &robot_state, 20 | TrajectoryGenerator *traj_generator) override; 21 | 22 | private: 23 | const franka::Model *model_; 24 | 25 | }; 26 | 27 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_PASSTHROUGH_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/pass_through_joint_torque_feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_PASSTHROUGH_JOINT_TORQUE_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_PASSTHROUGH_JOINT_TORQUE_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include "franka-interface/feedback_controller/feedback_controller.h" 5 | 6 | // A passthrough feedback controller that just uses the desired joint 7 | // torques from the sensor subscriber and passes it to the robot 8 | class PassThroughJointTorqueFeedbackController : public FeedbackController { 9 | public: 10 | using FeedbackController::FeedbackController; 11 | 12 | void parse_parameters() override; 13 | 14 | void initialize_controller(FrankaRobot *robot) override; 15 | 16 | void get_next_step(const franka::RobotState &robot_state, 17 | TrajectoryGenerator *traj_generator) override; 18 | 19 | void parse_sensor_data(const franka::RobotState &robot_state) override; 20 | 21 | private: 22 | JointTorqueFeedbackControllerMessage joint_torque_feedback_params_; 23 | JointTorqueControllerSensorMessage joint_torque_sensor_msg_; 24 | const franka::Model *model_; 25 | 26 | std::array S_= {}; 27 | std::array remove_gravity_= {}; 28 | std::array desired_joint_torques_= {}; 29 | 30 | // Stiffness 31 | std::array k_gains_ = {{600.0, 600.0, 600.0, 600.0, 32 | 250.0, 150.0, 50.0}}; 33 | // Damping 34 | std::array d_gains_ = {{50.0, 50.0, 50.0, 50.0, 35 | 30.0, 25.0, 15.0}}; 36 | }; 37 | 38 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_PASSTHROUGH_JOINT_TORQUE_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller/set_internal_impedance_feedback_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_SET_INTERNAL_IMPEDANCE_FEEDBACK_CONTROLLER_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_SET_INTERNAL_IMPEDANCE_FEEDBACK_CONTROLLER_H_ 3 | 4 | #include "franka-interface/feedback_controller/feedback_controller.h" 5 | 6 | class SetInternalImpedanceFeedbackController : public FeedbackController { 7 | public: 8 | using FeedbackController::FeedbackController; 9 | 10 | void parse_parameters() override; 11 | 12 | void initialize_controller(FrankaRobot *robot) override; 13 | 14 | void get_next_step(const franka::RobotState &robot_state, 15 | TrajectoryGenerator *traj_generator) override; 16 | 17 | bool set_cartesian_impedance_ = false; 18 | bool set_joint_impedance_ = false; 19 | 20 | private: 21 | InternalImpedanceFeedbackControllerMessage internal_impedance_feedback_params_; 22 | 23 | // Max Joint and Cartesian Impedance Values. 24 | // TODO: Check to see what they actually are. 25 | // Kevin simply guessed 10000.0 26 | double max_cartesian_impedance_ = 10000.0; 27 | double max_joint_impedance_ = 10000.0; 28 | 29 | 30 | std::array cartesian_impedances_ = {{3000, 3000, 3000, 300, 300, 300}}; 31 | std::array joint_impedances_ = {{3000, 3000, 3000, 2500, 2500, 2000, 2000}}; 32 | }; 33 | 34 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_SET_INTERNAL_IMPEDANCE_FEEDBACK_CONTROLLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/feedback_controller_factory.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FACTORY_H_ 2 | #define FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FACTORY_H_ 3 | 4 | #include "franka-interface-common/definitions.h" 5 | #include "franka-interface/sensor_data_manager.h" 6 | 7 | class FeedbackController; 8 | 9 | class FeedbackControllerFactory { 10 | public: 11 | FeedbackControllerFactory() {}; 12 | 13 | /** 14 | * Get feedback controller for skill. 15 | * 16 | * @param memory_region Region of the memory where the parameters 17 | * will be stored. 18 | * @return FeedbackController instance for this skill 19 | */ 20 | FeedbackController* getFeedbackControllerForSkill(SharedBufferTypePtr buffer, SensorDataManager* sensor_data_manager); 21 | 22 | }; 23 | 24 | #endif // FRANKA_INTERFACE_FEEDBACK_CONTROLLER_FACTORY_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/franka_gripper.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_ROBOTS_FRANKA_GRIPPER_H_ 2 | #define FRANKA_INTERFACE_ROBOTS_FRANKA_GRIPPER_H_ 3 | 4 | #include 5 | 6 | class FrankaGripper 7 | { 8 | public: 9 | 10 | FrankaGripper(std::string &robot_ip) : gripper_(robot_ip) 11 | {}; 12 | 13 | franka::GripperState getGripperState() 14 | { 15 | return gripper_.readOnce(); 16 | } 17 | 18 | franka::Gripper gripper_; 19 | 20 | virtual ~FrankaGripper() = default; 21 | }; 22 | 23 | #endif // FRANKA_INTERFACE_ROBOTS_FRANKA_GRIPPER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/franka_robot.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_ROBOTS_FRANKA_ROBOT_H_ 2 | #define FRANKA_INTERFACE_ROBOTS_FRANKA_ROBOT_H_ 3 | 4 | #include 5 | #include 6 | 7 | 8 | class FrankaRobot 9 | { 10 | public: 11 | 12 | FrankaRobot(std::string &robot_ip) : 13 | robot_(robot_ip), 14 | model_(robot_.loadModel()) 15 | {}; 16 | 17 | franka::Model* getModel() 18 | { 19 | return &model_; 20 | } 21 | 22 | franka::RobotState getRobotState() 23 | { 24 | return robot_.readOnce(); 25 | } 26 | 27 | franka::Robot robot_; 28 | franka::Model model_; 29 | 30 | virtual ~FrankaRobot() = default; 31 | 32 | void automaticErrorRecovery() { 33 | robot_.automaticErrorRecovery(); 34 | } 35 | 36 | void setJointImpedance(const std::array& K_theta) { 37 | robot_.setJointImpedance(K_theta); 38 | } 39 | 40 | void setCartesianImpedance(const std::array& K_x) { 41 | robot_.setCartesianImpedance(K_x); 42 | } 43 | }; 44 | 45 | #endif // FRANKA_INTERFACE_ROBOTS_FRANKA_ROBOT_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/run_loop_logger.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_RUN_LOOP_LOGGER_H_ 2 | #define FRANKA_INTERFACE_RUN_LOOP_LOGGER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | enum RunLoopLoggerCode {INFO, WARNING, ERROR, ABORT}; 9 | 10 | class RunLoopLogger { 11 | public: 12 | RunLoopLogger(std::mutex& m) : mutex_(m) {}; 13 | 14 | std::mutex& mutex_; 15 | 16 | void add_log(std::string log, RunLoopLoggerCode code); 17 | 18 | void add_error_log(std::string log); 19 | 20 | void add_warning_log(std::string log); 21 | 22 | void add_info_log(std::string log); 23 | 24 | void print_error_log(); 25 | 26 | void print_warning_log(); 27 | 28 | void print_info_log(); 29 | 30 | private: 31 | std::vector error_logs_={}; 32 | std::vector warning_logs_={}; 33 | std::vector info_logs_={}; 34 | 35 | void print_error_logs_to_console(std::vector& logs); 36 | }; 37 | 38 | #endif // FRANKA_INTERFACE_RUN_LOOP_LOGGER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/save_robot_state_data_to_shared_memory_buffer.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SAVE_ROBOT_STATE_TO_SHARED_MEMORY_BUFFER_H_ 2 | #define FRANKA_INTERFACE_SAVE_ROBOT_STATE_TO_SHARED_MEMORY_BUFFER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "franka-interface/robot_state_data.h" 9 | #include "franka-interface/run_loop_shared_memory_handler.h" 10 | #include "robot_state_msg.pb.h" 11 | 12 | #include 13 | 14 | void save_current_robot_state_data_to_shared_memory_buffer(RunLoopSharedMemoryHandler* shared_memory_handler, 15 | RobotStateData* robot_state_data); 16 | 17 | #endif // FRANKA_INTERFACE_SAVE_ROBOT_STATE_TO_SHARED_MEMORY_BUFFER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/sensor_data.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SENSOR_DATA_H_ 2 | #define FRANKA_INTERFACE_SENSOR_DATA_H_ 3 | 4 | #include 5 | 6 | class SensorData { 7 | public: 8 | explicit SensorData(SharedBufferTypePtr v) : values_{v} {}; 9 | 10 | /** 11 | * Parse data from memory. 12 | */ 13 | virtual void parse_data() = 0; 14 | 15 | /** 16 | * Initialize any other hand. 17 | */ 18 | virtual void initialize_data() = 0; 19 | 20 | /** 21 | * Should we terminate the current skill. 22 | */ 23 | virtual bool update_data() = 0; 24 | 25 | protected: 26 | SharedBufferTypePtr values_=0; 27 | }; 28 | 29 | #endif // FRANKA_INTERFACE_SENSOR_DATA_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skill_info_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILL_INFO_MANAGER_H_ 2 | #define FRANKA_INTERFACE_SKILL_INFO_MANAGER_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/skills/base_skill.h" 7 | #include "franka-interface/skills/base_meta_skill.h" 8 | 9 | class BaseSkill; 10 | class BaseMetaSkill; 11 | 12 | class SkillInfoManager { 13 | public: 14 | SkillInfoManager(); 15 | 16 | BaseSkill* get_current_skill(); 17 | 18 | BaseMetaSkill* get_current_meta_skill(); 19 | 20 | bool is_currently_executing_skill(); 21 | 22 | bool is_waiting_for_new_skill(); 23 | 24 | void add_skill(BaseSkill *skill); 25 | 26 | void add_meta_skill(BaseMetaSkill* skill); 27 | 28 | void clear_skill_and_meta_skill_list(); 29 | 30 | BaseMetaSkill* get_meta_skill_with_id(int meta_skill_id); 31 | 32 | private: 33 | std::vector skill_list_{}; 34 | std::vector meta_skill_list_{}; 35 | }; 36 | 37 | #endif // FRANKA_INTERFACE_SKILL_INFO_MANAGER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skills/base_meta_skill.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILLS_BASE_META_SKILL_H_ 2 | #define FRANKA_INTERFACE_SKILLS_BASE_META_SKILL_H_ 3 | 4 | #include "franka-interface/skills/base_skill.h" 5 | 6 | class run_loop; 7 | 8 | class BaseMetaSkill { 9 | public: 10 | BaseMetaSkill(int skill_idx): skill_idx_(skill_idx), 11 | skill_status_(SkillStatus::TO_START) 12 | {}; 13 | 14 | int getMetaSkillId(); 15 | 16 | virtual bool isComposableSkill(); 17 | 18 | void setMetaSkillStatus(SkillStatus new_task_status); 19 | 20 | SkillStatus getCurrentMetaSkillStatus(); 21 | 22 | virtual void execute_skill_on_franka(run_loop* run_loop, 23 | FrankaRobot* robot, 24 | FrankaGripper* gripper, 25 | RobotStateData* robot_state_data); 26 | 27 | protected: 28 | int skill_idx_; 29 | SkillStatus skill_status_; 30 | bool is_composable_{false}; 31 | double current_period_; 32 | franka::Model* model_= nullptr; 33 | }; 34 | 35 | #endif // FRANKA_INTERFACE_SKILLS_BASE_META_SKILL_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skills/cartesian_pose_skill.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILLS_CARTESIAN_POSE_SKILL_H_ 2 | #define FRANKA_INTERFACE_SKILLS_CARTESIAN_POSE_SKILL_H_ 3 | 4 | #include "franka-interface/skills/base_skill.h" 5 | 6 | class CartesianPoseSkill : public BaseSkill { 7 | public: 8 | CartesianPoseSkill(int skill_idx, int meta_skill_idx, std::string description) : 9 | BaseSkill(skill_idx, meta_skill_idx, description) 10 | { 11 | for(int i = 0; i < 3; i++) { 12 | previous_velocity_[i] = 0.0; 13 | previous_acceleration_[i] = 0.0; 14 | current_velocity_[i] = 0.0; 15 | current_acceleration_[i] = 0.0; 16 | current_jerk_[i] = 0.0; 17 | previous_error_[i] = 0.0; 18 | } 19 | 20 | }; 21 | 22 | std::array limit_position(std::array &desired_pose, double period); 23 | 24 | std::array limit_position_to_stop(std::array ¤t_pose, double period); 25 | 26 | void execute_skill_on_franka(run_loop* run_loop, 27 | FrankaRobot* robot, 28 | FrankaGripper* gripper, 29 | RobotStateData* robot_state_data) override; 30 | 31 | private: 32 | bool return_status_{false}; 33 | 34 | std::array previous_desired_pose_; 35 | 36 | std::array current_error_; 37 | std::array previous_error_; 38 | std::array integral_; 39 | std::array derivative_; 40 | 41 | std::array previous_position_; 42 | std::array previous_velocity_; 43 | std::array previous_acceleration_; 44 | 45 | std::array current_position_; 46 | std::array current_velocity_; 47 | std::array current_acceleration_; 48 | std::array current_jerk_; 49 | 50 | std::array next_position_; 51 | std::array next_velocity_; 52 | std::array next_acceleration_; 53 | std::array next_jerk_; 54 | 55 | double safety_factor = 0.1; 56 | double Kp_ = 30.0; 57 | double Ki_ = 0.0; // 0.1; 58 | double Kd_ = 0.01; 59 | double eps_ = 0.0001; 60 | 61 | // Franka Parameters from https://frankaemika.github.io/docs/control_parameters.html 62 | const double max_cartesian_translation_velocity_ = 1.7; // m / s 63 | const double max_cartesian_rotation_velocity_ = 2.5; // rad / s 64 | const double max_cartesian_elbow_velocity_ = 2.175; // rad / s 65 | const double max_cartesian_translation_acceleration_ = 13.0; // m / s^2 66 | const double max_cartesian_rotation_acceleration_ = 25.0; // rad / s^2 67 | const double max_cartesian_elbow_acceleration_ = 10.0; // rad / s^2 68 | const double max_cartesian_translation_jerk_ = 6500.0; // m / s^3 69 | const double max_cartesian_rotation_jerk_ = 12500.0; // rad / s^3 70 | const double max_cartesian_elbow_jerk_ = 5000.0; // rad / s^3 71 | }; 72 | 73 | #endif // FRANKA_INTERFACE_SKILLS_CARTESIAN_POSE_SKILL_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skills/cartesian_velocity_skill.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILLS_CARTESIAN_VELOCITY_SKILL_H_ 2 | #define FRANKA_INTERFACE_SKILLS_CARTESIAN_VELOCITY_SKILL_H_ 3 | 4 | #include "franka-interface/skills/base_skill.h" 5 | 6 | class CartesianVelocitySkill : public BaseSkill { 7 | public: 8 | CartesianVelocitySkill(int skill_idx, int meta_skill_idx, std::string description) : 9 | BaseSkill(skill_idx, meta_skill_idx, description) 10 | {}; 11 | 12 | void execute_skill_on_franka(run_loop* run_loop, 13 | FrankaRobot* robot, 14 | FrankaGripper* gripper, 15 | RobotStateData* robot_state_data) override; 16 | 17 | private: 18 | bool return_status_{false}; 19 | }; 20 | 21 | #endif // FRANKA_INTERFACE_SKILLS_CARTESIAN_VELOCITY_SKILL_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skills/force_torque_skill.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILLS_FORCE_TORQUE_SKILL_H_ 2 | #define FRANKA_INTERFACE_SKILLS_FORCE_TORQUE_SKILL_H_ 3 | 4 | #include "franka-interface/skills/base_skill.h" 5 | 6 | class ForceTorqueSkill : public BaseSkill { 7 | public: 8 | ForceTorqueSkill(int skill_idx, int meta_skill_idx, std::string description) : 9 | BaseSkill(skill_idx, meta_skill_idx, description) 10 | {}; 11 | 12 | void limit_current_joint_torques(double period); 13 | 14 | void execute_skill_on_franka(run_loop* run_loop, 15 | FrankaRobot* robot, 16 | FrankaGripper* gripper, 17 | RobotStateData *robot_state_data) override; 18 | 19 | private: 20 | bool return_status_{false}; 21 | double safety_factor = 0.1; 22 | 23 | std::array previous_joint_torques_; 24 | std::array current_joint_torques_; 25 | std::array current_joint_rotatums_; 26 | 27 | // Franka Parameters from https://frankaemika.github.io/docs/control_parameters.html 28 | std::array max_joint_torques_{87, 87, 87, 87, 12, 12, 12}; // Nm 29 | std::array max_joint_rotatums_{1000, 1000, 1000, 1000, 1000, 1000, 1000}; // Nm / s 30 | }; 31 | 32 | #endif // FRANKA_INTERFACE_SKILLS_FORCE_TORQUE_SKILL_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skills/gripper_skill.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILLS_GRIPPER_SKILL_H_ 2 | #define FRANKA_INTERFACE_SKILLS_GRIPPER_SKILL_H_ 3 | 4 | #include "franka-interface/skills/base_skill.h" 5 | 6 | class GripperSkill : public BaseSkill { 7 | public: 8 | GripperSkill(int skill_idx, int meta_skill_idx, std::string description) : 9 | BaseSkill(skill_idx, meta_skill_idx, description) 10 | {}; 11 | 12 | void execute_skill_on_franka(run_loop* run_loop, 13 | FrankaRobot* robot, 14 | FrankaGripper* gripper, 15 | RobotStateData* robot_state_data) override; 16 | 17 | bool has_terminated(FrankaRobot* robot) override; 18 | 19 | private: 20 | bool return_status_{false}; 21 | }; 22 | 23 | #endif // FRANKA_INTERFACE_SKILLS_GRIPPER_SKILL_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skills/impedance_control_skill.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILLS_IMPEDANCE_CONTROL_SKILL_H_ 2 | #define FRANKA_INTERFACE_SKILLS_IMPEDANCE_CONTROL_SKILL_H_ 3 | 4 | #include "franka-interface/skills/base_skill.h" 5 | 6 | class ImpedanceControlSkill : public BaseSkill { 7 | public: 8 | ImpedanceControlSkill(int skill_idx, int meta_skill_idx, std::string description) : 9 | BaseSkill(skill_idx, meta_skill_idx, description) 10 | {}; 11 | 12 | void limit_current_joint_torques(double period); 13 | 14 | void execute_skill_on_franka(run_loop* run_loop, 15 | FrankaRobot* robot, 16 | FrankaGripper* gripper, 17 | RobotStateData* robot_state_data) override; 18 | 19 | private: 20 | const std::array k_gains_ = {{600.0, 600.0, 600.0, 600.0, 21 | 250.0, 150.0, 50.0}}; 22 | // Damping 23 | const std::array d_gains_ = {{50.0, 50.0, 50.0, 50.0, 24 | 30.0, 25.0, 15.0}}; 25 | 26 | double safety_factor = 0.1; 27 | 28 | std::array previous_joint_torques_; 29 | std::array current_joint_torques_; 30 | std::array current_joint_rotatums_; 31 | 32 | // Franka Parameters from https://frankaemika.github.io/docs/control_parameters.html 33 | std::array max_joint_torques_{87, 87, 87, 87, 12, 12, 12}; // Nm 34 | std::array max_joint_rotatums_{1000, 1000, 1000, 1000, 1000, 1000, 1000}; // Nm / s 35 | }; 36 | 37 | #endif // FRANKA_INTERFACE_SKILLS_IMPEDANCE_CONTROL_SKILL_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skills/joint_position_continuous_skill.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILLS_JOINT_POSITION_CONTINUOUS_SKILL_H_ 2 | #define FRANKA_INTERFACE_SKILLS_JOINT_POSITION_CONTINUOUS_SKILL_H_ 3 | 4 | #include "franka-interface/skills/base_meta_skill.h" 5 | 6 | class JointPositionContinuousSkill : public BaseMetaSkill { 7 | public: 8 | JointPositionContinuousSkill(int skill_idx): BaseMetaSkill(skill_idx) { 9 | is_composable_ = true; 10 | }; 11 | 12 | bool isComposableSkill() override; 13 | 14 | void execute_skill_on_franka(run_loop *run_loop, 15 | FrankaRobot* robot, 16 | FrankaGripper* gripper, 17 | RobotStateData* robot_state_data) override; 18 | 19 | private: 20 | bool return_status_{false}; 21 | }; 22 | 23 | #endif // FRANKA_INTERFACE_SKILLS_JOINT_POSITION_CONTINUOUS_SKILL_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skills/joint_position_skill.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILLS_JOINT_POSITION_SKILL_H_ 2 | #define FRANKA_INTERFACE_SKILLS_JOINT_POSITION_SKILL_H_ 3 | 4 | #include "franka-interface/skills/base_skill.h" 5 | 6 | class JointPositionSkill : public BaseSkill { 7 | public: 8 | JointPositionSkill(int skill_idx, int meta_skill_idx, std::string description) : 9 | BaseSkill(skill_idx, meta_skill_idx, description) 10 | {}; 11 | 12 | void execute_skill_on_franka(run_loop* run_loop, 13 | FrankaRobot* robot, 14 | FrankaGripper* gripper, 15 | RobotStateData* robot_state_data) override; 16 | 17 | private: 18 | bool return_status_{false}; 19 | }; 20 | 21 | #endif // FRANKA_INTERFACE_SKILLS_JOINT_POSITION_SKILL_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/skills/joint_velocity_skill.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_SKILLS_JOINT_VELOCITY_SKILL_H_ 2 | #define FRANKA_INTERFACE_SKILLS_JOINT_VELOCITY_SKILL_H_ 3 | 4 | #include "franka-interface/skills/base_skill.h" 5 | 6 | class JointVelocitySkill : public BaseSkill { 7 | public: 8 | JointVelocitySkill(int skill_idx, int meta_skill_idx, std::string description) : 9 | BaseSkill(skill_idx, meta_skill_idx, description) 10 | {}; 11 | 12 | void execute_skill_on_franka(run_loop* run_loop, 13 | FrankaRobot* robot, 14 | FrankaGripper* gripper, 15 | RobotStateData* robot_state_data) override; 16 | 17 | private: 18 | bool return_status_{false}; 19 | }; 20 | 21 | #endif // FRANKA_INTERFACE_SKILLS_JOINT_VELOCITY_SKILL_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/termination_handler/contact_termination_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TERMINATION_HANDLER_CONTACT_TERMINATION_HANDLER_H_ 2 | #define FRANKA_INTERFACE_TERMINATION_HANDLER_CONTACT_TERMINATION_HANDLER_H_ 3 | 4 | #include "franka-interface/termination_handler/termination_handler.h" 5 | 6 | class ContactTerminationHandler : public TerminationHandler { 7 | public: 8 | using TerminationHandler::TerminationHandler; 9 | 10 | /** 11 | * Parse parameters from memory. 12 | */ 13 | void parse_parameters() override; 14 | 15 | /** 16 | * Initialize termination handler after parameter parsing. 17 | */ 18 | void initialize_handler(FrankaRobot *robot) override; 19 | 20 | /** 21 | * Should we terminate the current skill. 22 | */ 23 | bool should_terminate(const franka::RobotState &robot_state, 24 | franka::Model *robot_model, 25 | TrajectoryGenerator *traj_generator) override; 26 | 27 | private: 28 | ContactTerminationHandlerMessage contact_termination_params_; 29 | 30 | std::array cartesian_contacts_to_use_{{1.0, 1.0, 1.0, 31 | 1.0, 1.0, 1.0}}; 32 | 33 | std::array lower_torque_thresholds_acceleration_{}; 34 | std::array upper_torque_thresholds_acceleration_{}; 35 | std::array lower_torque_thresholds_nominal_{}; 36 | std::array upper_torque_thresholds_nominal_{}; 37 | std::array lower_force_thresholds_acceleration_{}; 38 | std::array upper_force_thresholds_acceleration_{}; 39 | std::array lower_force_thresholds_nominal_{}; 40 | std::array upper_force_thresholds_nominal_{}; 41 | 42 | const std::array default_lower_torque_thresholds_acceleration_{{20.0,20.0,18.0,18.0,16.0,14.0,12.0}}; 43 | const std::array default_upper_torque_thresholds_acceleration_{{120.0,120.0,118.0,118.0,116.0,114.0,112.0}}; 44 | const std::array default_lower_torque_thresholds_nominal_{{20.0,20.0,18.0,18.0,16.0,14.0,12.0}}; 45 | const std::array default_upper_torque_thresholds_nominal_{{120.0,120.0,118.0,118.0,116.0,114.0,112.0}}; 46 | const std::array default_lower_force_thresholds_acceleration_{{20.0,20.0,20.0,25.0,25.0,25.0}}; 47 | const std::array default_upper_force_thresholds_acceleration_{{120.0,120.0,120.0,125.0,125.0,125.0}}; 48 | const std::array default_lower_force_thresholds_nominal_{{20.0,20.0,20.0,25.0,25.0,25.0}}; 49 | const std::array default_upper_force_thresholds_nominal_{{120.0,120.0,120.0,125.0,125.0,125.0}}; 50 | }; 51 | 52 | #endif // FRANKA_INTERFACE_TERMINATION_HANDLER_CONTACT_TERMINATION_HANDLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/termination_handler/final_joint_termination_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TERMINATION_HANDLER_FINAL_JOINT_TERMINATION_HANDLER_H_ 2 | #define FRANKA_INTERFACE_TERMINATION_HANDLER_FINAL_JOINT_TERMINATION_HANDLER_H_ 3 | 4 | #include "franka-interface/termination_handler/termination_handler.h" 5 | 6 | class FinalJointTerminationHandler :public TerminationHandler{ 7 | public: 8 | using TerminationHandler::TerminationHandler; 9 | 10 | /** 11 | * Parse parameters from memory. 12 | */ 13 | void parse_parameters() override; 14 | 15 | /** 16 | * Initialize termination handler after parameter parsing. 17 | */ 18 | void initialize_handler(FrankaRobot *robot) {}; 19 | 20 | /** 21 | * Should we terminate the current skill. 22 | */ 23 | bool should_terminate(const franka::RobotState &robot_state, 24 | franka::Model *robot_model, 25 | TrajectoryGenerator *traj_generator) override; 26 | 27 | protected: 28 | JointThresholdMessage joint_termination_params_; 29 | 30 | double default_joint_threshold_ = 0.0001; 31 | std::array joint_thresholds_; 32 | }; 33 | 34 | #endif // FRANKA_INTERFACE_TERMINATION_HANDLER_FINAL_JOINT_TERMINATION_HANDLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/termination_handler/final_pose_termination_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TERMINATION_HANDLER_FINAL_POSE_TERMINATION_HANDLER_H_ 2 | #define FRANKA_INTERFACE_TERMINATION_HANDLER_FINAL_POSE_TERMINATION_HANDLER_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/termination_handler/termination_handler.h" 7 | 8 | class FinalPoseTerminationHandler : public TerminationHandler { 9 | public: 10 | using TerminationHandler::TerminationHandler; 11 | 12 | /** 13 | * Parse parameters from memory. 14 | */ 15 | void parse_parameters() override; 16 | 17 | /** 18 | * Initialize termination handler after parameter parsing. 19 | */ 20 | void initialize_handler(FrankaRobot *robot) {}; 21 | 22 | /** 23 | * Should we terminate the current skill. 24 | */ 25 | virtual bool should_terminate(const franka::RobotState &robot_state, 26 | franka::Model *robot_model, 27 | TrajectoryGenerator *traj_generator) override; 28 | 29 | private: 30 | PoseThresholdMessage pose_termination_params_; 31 | double default_position_threshold_ = 0.0001; 32 | double default_orientation_threshold_ = 0.0001; 33 | Eigen::Vector3d position_thresholds_; 34 | Eigen::Vector3d orientation_thresholds_; 35 | }; 36 | 37 | #endif // FRANKA_INTERFACE_TERMINATION_HANDLER_FINAL_POSE_TERMINATION_HANDLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/termination_handler/noop_termination_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TERMINATION_HANDLER_NOOP_TERMINATION_HANDLER_H_ 2 | #define FRANKA_INTERFACE_TERMINATION_HANDLER_NOOP_TERMINATION_HANDLER_H_ 3 | 4 | #include "franka-interface/termination_handler/termination_handler.h" 5 | 6 | class NoopTerminationHandler : public TerminationHandler { 7 | public: 8 | using TerminationHandler::TerminationHandler; 9 | 10 | /** 11 | * Parse parameters from memory. 12 | */ 13 | void parse_parameters() {}; 14 | 15 | /** 16 | * Initialize termination handler after parameter parsing. 17 | */ 18 | void initialize_handler(FrankaRobot *robot) {}; 19 | 20 | /** 21 | * Should we terminate the current skill. 22 | */ 23 | bool should_terminate(const franka::RobotState &robot_state, 24 | franka::Model *robot_model, 25 | TrajectoryGenerator *traj_generator) override; 26 | 27 | }; 28 | 29 | #endif // FRANKA_INTERFACE_TERMINATION_HANDLER_NOOP_TERMINATION_HANDLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/termination_handler/time_termination_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TERMINATION_HANDLER_TIME_TERMINATION_HANDLER_H_ 2 | #define FRANKA_INTERFACE_TERMINATION_HANDLER_TIME_TERMINATION_HANDLER_H_ 3 | 4 | #include "franka-interface/termination_handler/termination_handler.h" 5 | 6 | class TimeTerminationHandler : public TerminationHandler { 7 | public: 8 | using TerminationHandler::TerminationHandler; 9 | 10 | /** 11 | * Parse parameters from memory. 12 | */ 13 | void parse_parameters() override; 14 | 15 | /** 16 | * Initialize termination handler after parameter parsing. 17 | */ 18 | void initialize_handler(FrankaRobot *robot) {}; 19 | 20 | /** 21 | * Should we terminate the current skill. 22 | */ 23 | bool should_terminate(const franka::RobotState &robot_state, 24 | franka::Model *robot_model, 25 | TrajectoryGenerator *traj_generator) override; 26 | protected: 27 | TimeTerminationHandlerMessage time_termination_params_; 28 | }; 29 | 30 | #endif // FRANKA_INTERFACE_TERMINATION_HANDLER_TIME_TERMINATION_HANDLER_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/termination_handler_factory.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TERMINATION_HANDLER_FACTORY_H_ 2 | #define FRANKA_INTERFACE_TERMINATION_HANDLER_FACTORY_H_ 3 | 4 | #include 5 | #include 6 | #include "franka-interface/sensor_data_manager.h" 7 | 8 | class TerminationHandler; 9 | 10 | class TerminationHandlerFactory { 11 | public: 12 | TerminationHandlerFactory() {}; 13 | 14 | /** 15 | * Get termination handler for skill. 16 | * 17 | * @param memory_region Region of the memory where the parameters 18 | * will be stored. 19 | * @return TermatinationHanndler instance for this skill 20 | */ 21 | TerminationHandler* getTerminationHandlerForSkill(SharedBufferTypePtr buffer, 22 | RunLoopProcessInfo *run_loop_info, 23 | SensorDataManager* sensor_data_manager); 24 | 25 | }; 26 | 27 | #endif // FRANKA_INTERFACE_TERMINATION_HANDLER_FACTORY_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/cartesian_velocity_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/trajectory_generator/trajectory_generator.h" 7 | 8 | class CartesianVelocityTrajectoryGenerator : public TrajectoryGenerator { 9 | public: 10 | using TrajectoryGenerator::TrajectoryGenerator; 11 | 12 | void parse_parameters() override; 13 | 14 | void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type=SkillType::CartesianVelocitySkill) override; 15 | 16 | /** 17 | * Initialize initial and desired Cartesian velocities from robot state 18 | */ 19 | void initialize_initial_cartesian_velocities(const franka::RobotState &robot_state, SkillType skill_type); 20 | 21 | /** 22 | * Returns the desired Cartesian velocities. This method is called at every time step of the control loop to 23 | * find the Cartesian velocities to send at the next step. 24 | */ 25 | const std::array& get_desired_cartesian_velocities() const; 26 | 27 | protected: 28 | CartesianVelocityTrajectoryGeneratorMessage cartesian_velocity_trajectory_params_; 29 | 30 | std::array initial_cartesian_velocities_{}; 31 | std::array cartesian_accelerations_{}; 32 | std::array desired_cartesian_velocities_{}; 33 | std::array goal_cartesian_velocities_{}; 34 | }; 35 | 36 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/cubic_hermite_spline_joint_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CUBIC_HERMITE_SPLINE_JOINT_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CUBIC_HERMITE_SPLINE_JOINT_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 5 | 6 | class CubicHermiteSplineJointTrajectoryGenerator : public JointTrajectoryGenerator { 7 | public: 8 | using JointTrajectoryGenerator::JointTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | 12 | void parse_sensor_data(const franka::RobotState &robot_state) override; 13 | 14 | private: 15 | double seg_start_time_ = 0.; 16 | double seg_run_time = 0.; 17 | 18 | std::array initial_joint_velocities_{{0., 0., 0., 0., 0., 0., 0.}}; 19 | std::array goal_joint_velocities_{{0., 0., 0., 0., 0., 0., 0.}}; 20 | 21 | JointPositionVelocitySensorMessage joint_sensor_msg_; 22 | 23 | }; 24 | 25 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CUBIC_HERMITE_SPLINE_JOINT_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/cubic_hermite_spline_pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CUBIC_HERMITE_SPLINE_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CUBIC_HERMITE_SPLINE_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | class CubicHermiteSplinePoseTrajectoryGenerator : public PoseTrajectoryGenerator { 7 | public: 8 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 9 | 10 | void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type=SkillType::ImpedanceControlSkill) override; 11 | 12 | void get_next_step(const franka::RobotState &robot_state) override; 13 | 14 | void parse_sensor_data(const franka::RobotState &robot_state) override; 15 | 16 | private: 17 | double seg_start_time_ = 0.; 18 | double seg_run_time = 0.; 19 | 20 | std::array initial_pose_velocities_{{0., 0., 0., 0., 0., 0.}}; 21 | std::array goal_pose_velocities_{{0., 0., 0., 0., 0., 0.}}; 22 | 23 | Eigen::Vector3d initial_euler_; 24 | Eigen::Vector3d goal_euler_; 25 | Eigen::Vector3d desired_euler_; 26 | 27 | PosePositionVelocitySensorMessage pose_sensor_msg_; 28 | 29 | }; 30 | 31 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_CUBIC_HERMITE_SPLINE_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/force_position_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_FORCE_POSITION_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_FORCE_POSITION_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include 5 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 6 | 7 | class ForcePositionTrajectoryGenerator : public PoseTrajectoryGenerator { 8 | public: 9 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 10 | 11 | virtual const std::array& get_desired_pose() = 0; 12 | virtual const std::array& get_desired_force() = 0; 13 | 14 | protected: 15 | RunTimeMessage run_time_params_; 16 | ForcePositionSensorMessage force_position_sensor_msg_; 17 | 18 | std::array desired_force_{}; 19 | }; 20 | 21 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_FORCE_POSITION_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/goal_pose_dmp_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_GOAL_POSE_DMP_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_GOAL_POSE_DMP_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | class GoalPoseDmpTrajectoryGenerator : public PoseTrajectoryGenerator { 7 | public: 8 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 9 | 10 | void parse_parameters() override; 11 | 12 | void initialize_trajectory(const franka::RobotState &robot_state, 13 | SkillType skill_type=SkillType::CartesianPoseSkill) override; 14 | 15 | void get_next_step(const franka::RobotState &robot_state) override; 16 | 17 | std::array y_={}; 18 | std::array dy_={}; 19 | 20 | // min_z was found by attaching the knife to the robot arm and 21 | // placing it on the cutting board. Then I used the print_joint_poses 22 | // in libfranka/examples to see the actual min z 23 | double min_z = 0.02653; 24 | double eps = -0.01; 25 | 26 | private: 27 | PoseDMPTrajectoryGeneratorMessage pose_dmp_trajectory_params_; 28 | 29 | bool orientation_only_ = false; 30 | bool position_only_ = false; 31 | 32 | // Variables initialized from shared memory should be doubles. 33 | double alpha_=5.0; 34 | double beta_=5.0/4.0; 35 | double tau_=0.0; 36 | double x_=1.0; 37 | int num_basis_=40; 38 | int num_dims_=6; 39 | int num_sensor_values_=10; 40 | std::array basis_mean_{}; 41 | std::array basis_std_{}; 42 | // 20 represents number of basis functions and 10 represent the number 43 | // of sensor values 44 | // std::array, 10>, 3> weights_{}; 45 | std::array, 10>, 6> weights_{}; 46 | std::array, 6> initial_sensor_values_{}; 47 | std::array y0_={}; 48 | 49 | void getInitialMeanAndStd(); 50 | }; 51 | 52 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_GOAL_POSE_DMP_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/gripper_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_GRIPPER_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_GRIPPER_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/trajectory_generator.h" 5 | 6 | /** 7 | * Used in Gripper skill. Specifies 3 parameters in the following order 8 | * 9 | * 1) gripper width 10 | * 2) gripper speed 11 | * 3) gripper force 12 | */ 13 | class GripperTrajectoryGenerator : public TrajectoryGenerator { 14 | public: 15 | using TrajectoryGenerator::TrajectoryGenerator; 16 | 17 | void parse_parameters() override; 18 | 19 | void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type) {}; 20 | 21 | void get_next_step(const franka::RobotState &robot_state) {}; 22 | 23 | /** 24 | * Get width to move the gripper to. 25 | * @return width 26 | */ 27 | double getWidth(); 28 | 29 | /** 30 | * Get speed to move the gripper. 31 | * @return gripper speed. 32 | */ 33 | double getSpeed(); 34 | 35 | /** 36 | * Get Force to grasp an object. 37 | * @return gripper force 38 | */ 39 | double getForce(); 40 | 41 | /** 42 | * Check if the skill requires to grasp the object. 43 | * @return True if the skill requires to grasp the object, returns false if it does not. 44 | */ 45 | bool isGraspSkill(); 46 | 47 | private: 48 | GripperTrajectoryGeneratorMessage gripper_trajectory_params_; 49 | 50 | double width_=1.0; 51 | double speed_=0.0; 52 | double force_=0.0; 53 | bool is_grasp_skill_{false}; 54 | }; 55 | 56 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_GRIPPER_TRAJECTORY_GENERATOR_H_ 57 | -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/impulse_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_IMPULSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_IMPULSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "franka-interface/trajectory_generator/trajectory_generator.h" 8 | 9 | class ImpulseTrajectoryGenerator : public TrajectoryGenerator { 10 | public: 11 | using TrajectoryGenerator::TrajectoryGenerator; 12 | 13 | void parse_parameters() override; 14 | 15 | void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type=SkillType::ForceTorqueSkill) override; 16 | 17 | void initialize_initial_states(const franka::RobotState &robot_state, SkillType skill_type); 18 | 19 | void get_next_step(const franka::RobotState &robot_state) override; 20 | 21 | void check_displacement_cap(const franka::RobotState &robot_state); 22 | 23 | const std::array& get_desired_force_torque() const; 24 | 25 | const Eigen::Vector3d& get_initial_position() const; 26 | 27 | const Eigen::Quaterniond& get_initial_orientation() const; 28 | 29 | private: 30 | ImpulseTrajectoryGeneratorMessage impulse_trajectory_params_; 31 | 32 | double acc_time_ = 0.0; 33 | 34 | std::array initial_pose_{}; 35 | 36 | std::array target_force_torque_{}; 37 | bool should_deacc_ = false; 38 | 39 | double max_translation_{0.0}; 40 | double max_rotation_{0.0}; 41 | 42 | std::array desired_force_torque_{}; 43 | Eigen::Vector3d initial_position_; 44 | Eigen::Quaterniond initial_orientation_; 45 | 46 | Eigen::Vector3d current_position_; 47 | Eigen::Quaterniond current_orientation_; 48 | }; 49 | 50 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_IMPULSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/joint_dmp_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_DMP_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_DMP_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 5 | 6 | class JointDmpTrajectoryGenerator : public JointTrajectoryGenerator { 7 | public: 8 | using JointTrajectoryGenerator::JointTrajectoryGenerator; 9 | 10 | void parse_parameters() override; 11 | 12 | void initialize_trajectory(const franka::RobotState &robot_state, 13 | SkillType skill_type=SkillType::JointPositionSkill) override; 14 | 15 | void get_next_step(const franka::RobotState &robot_state) override; 16 | 17 | std::array y_={}; 18 | std::array dy_={}; 19 | 20 | private: 21 | JointDMPTrajectoryGeneratorMessage joint_dmp_trajectory_params_; 22 | 23 | // Variables initialized from shared memory should be doubles. 24 | double alpha_=5.0; 25 | double beta_=5.0/4.0; 26 | double tau_=0.0; 27 | double x_=1.0; 28 | int num_basis_=42; 29 | int num_dims_=7; 30 | int num_sensor_values_=10; 31 | std::array basis_mean_{}; 32 | std::array basis_std_{}; 33 | std::array, 10>, 7> weights_{}; 34 | std::array initial_sensor_values_{{1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}}; 35 | std::array y0_={}; 36 | 37 | void getInitialMeanAndStd(); 38 | }; 39 | 40 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_DMP_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/joint_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/trajectory_generator/trajectory_generator.h" 7 | 8 | class JointTrajectoryGenerator : public TrajectoryGenerator { 9 | public: 10 | using TrajectoryGenerator::TrajectoryGenerator; 11 | 12 | void parse_parameters() override; 13 | 14 | void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type=SkillType::JointPositionSkill) override; 15 | 16 | /** 17 | * Initialize initial and desired joints from robot state 18 | */ 19 | void initialize_initial_and_desired_joints(const franka::RobotState &robot_state, SkillType skill_type); 20 | 21 | /** 22 | * Set the goal joints to new value. This is called when new data is received from sensor buffer. 23 | * @param joints 24 | */ 25 | void setGoalJoints(const std::array joints); 26 | 27 | /** 28 | * Set the initial joints to new value. This is called when new data is received from sensor buffer. 29 | * @param joints 30 | */ 31 | void setInitialJoints(const std::array joints); 32 | 33 | /** 34 | * Returns the desired joints. This method is called at every time step of the control loop to 35 | * find the joint positions to move to at the next step. 36 | */ 37 | const std::array& get_desired_joints() const; 38 | 39 | /** 40 | * Returns the goal joints. These are the final set of joint positions that the skill needs to reach. 41 | */ 42 | const std::array& get_goal_joints() const; 43 | 44 | protected: 45 | JointTrajectoryGeneratorMessage joint_trajectory_params_; 46 | 47 | std::array initial_joints_{}; 48 | std::array desired_joints_{}; 49 | std::array goal_joints_{}; 50 | }; 51 | 52 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/joint_velocity_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include 5 | 6 | #include "franka-interface/trajectory_generator/trajectory_generator.h" 7 | 8 | class JointVelocityTrajectoryGenerator : public TrajectoryGenerator { 9 | public: 10 | using TrajectoryGenerator::TrajectoryGenerator; 11 | 12 | void parse_parameters() override; 13 | 14 | void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type=SkillType::JointVelocitySkill) override; 15 | 16 | /** 17 | * Initialize initial and desired joint velocities from robot state 18 | */ 19 | void initialize_initial_joint_velocities(const franka::RobotState &robot_state, SkillType skill_type); 20 | 21 | /** 22 | * Returns the desired joint velocities. This method is called at every time step of the control loop to 23 | * find the joint velocities to send at the next step. 24 | */ 25 | const std::array& get_desired_joint_velocities() const; 26 | 27 | protected: 28 | JointVelocityTrajectoryGeneratorMessage joint_velocity_trajectory_params_; 29 | 30 | std::array initial_joint_velocities_{}; 31 | std::array joint_accelerations_{}; 32 | std::array desired_joint_velocities_{}; 33 | std::array goal_joint_velocities_{}; 34 | }; 35 | 36 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/linear_force_position_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_LINEAR_FORCE_POSITION_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_LINEAR_FORCE_POSITION_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include 5 | #include "franka-interface/trajectory_generator/force_position_trajectory_generator.h" 6 | 7 | class LinearForcePositionTrajectoryGenerator : public ForcePositionTrajectoryGenerator { 8 | public: 9 | using ForcePositionTrajectoryGenerator::ForcePositionTrajectoryGenerator; 10 | 11 | void get_next_step(const franka::RobotState &robot_state) {}; 12 | 13 | void parse_parameters() override; 14 | 15 | void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type) override; 16 | 17 | void parse_sensor_data(const franka::RobotState &robot_state) override; 18 | 19 | const std::array& get_desired_pose() override; 20 | 21 | const std::array& get_desired_force() override; 22 | 23 | private: 24 | double seg_run_time_ = 0.; 25 | double seg_start_time_; 26 | 27 | Eigen::Affine3d goal_transform_{}; 28 | }; 29 | 30 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_LINEAR_FORCE_POSITION_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/linear_joint_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_LINEAR_JOINT_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_LINEAR_JOINT_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 5 | 6 | class LinearJointTrajectoryGenerator : public JointTrajectoryGenerator { 7 | public: 8 | using JointTrajectoryGenerator::JointTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | 12 | }; 13 | 14 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_LINEAR_JOINT_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/linear_pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_LINEAR_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_LINEAR_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | class LinearPoseTrajectoryGenerator : public PoseTrajectoryGenerator { 7 | public: 8 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | }; 12 | 13 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_LINEAR_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/min_jerk_joint_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_MIN_JERK_JOINT_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_MIN_JERK_JOINT_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 5 | 6 | class MinJerkJointTrajectoryGenerator : public JointTrajectoryGenerator { 7 | public: 8 | using JointTrajectoryGenerator::JointTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | 12 | }; 13 | 14 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_MIN_JERK_JOINT_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/min_jerk_pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_MIN_JERK_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_MIN_JERK_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | class MinJerkPoseTrajectoryGenerator : public PoseTrajectoryGenerator { 7 | public: 8 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | 12 | private: 13 | double slerp_t_ = 0.0; 14 | }; 15 | 16 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_MIN_JERK_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/pass_through_cartesian_velocity_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/cartesian_velocity_trajectory_generator.h" 5 | 6 | class PassThroughCartesianVelocityTrajectoryGenerator : public CartesianVelocityTrajectoryGenerator { 7 | public: 8 | using CartesianVelocityTrajectoryGenerator::CartesianVelocityTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) {}; 11 | 12 | void parse_sensor_data(const franka::RobotState &robot_state) override; 13 | 14 | private: 15 | CartesianVelocitySensorMessage cartesian_velocity_sensor_msg_; 16 | 17 | }; 18 | 19 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_CARTESIAN_VELOCITY_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/pass_through_force_position_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_FORCE_POSITION_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_FORCE_POSITION_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include 5 | #include "franka-interface/trajectory_generator/force_position_trajectory_generator.h" 6 | 7 | class PassThroughForcePositionTrajectoryGenerator : public ForcePositionTrajectoryGenerator { 8 | public: 9 | using ForcePositionTrajectoryGenerator::ForcePositionTrajectoryGenerator; 10 | 11 | void get_next_step(const franka::RobotState &robot_state) {}; 12 | 13 | void parse_parameters() override; 14 | 15 | void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type) override; 16 | 17 | void parse_sensor_data(const franka::RobotState &robot_state) override; 18 | 19 | const std::array& get_desired_pose() override; 20 | 21 | const std::array& get_desired_force() override; 22 | }; 23 | 24 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_FORCE_POSITION_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/pass_through_joint_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_JOINT_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_JOINT_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 5 | 6 | class PassThroughJointTrajectoryGenerator : public JointTrajectoryGenerator { 7 | public: 8 | using JointTrajectoryGenerator::JointTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) {}; 11 | 12 | void parse_sensor_data(const franka::RobotState &robot_state) override; 13 | 14 | private: 15 | JointPositionSensorMessage joint_sensor_msg_; 16 | 17 | }; 18 | 19 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_JOINT_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/pass_through_joint_velocity_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/joint_velocity_trajectory_generator.h" 5 | 6 | class PassThroughJointVelocityTrajectoryGenerator : public JointVelocityTrajectoryGenerator { 7 | public: 8 | using JointVelocityTrajectoryGenerator::JointVelocityTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) {}; 11 | 12 | void parse_sensor_data(const franka::RobotState &robot_state) override; 13 | 14 | private: 15 | JointVelocitySensorMessage joint_velocity_sensor_msg_; 16 | 17 | }; 18 | 19 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_JOINT_VELOCITY_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/pass_through_pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | class PassThroughPoseTrajectoryGenerator : public PoseTrajectoryGenerator { 7 | public: 8 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | 12 | void parse_sensor_data(const franka::RobotState &robot_state) override; 13 | 14 | private: 15 | PosePositionSensorMessage pose_sensor_msg_; 16 | 17 | }; 18 | 19 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_PASS_THROUGH_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/pose_dmp_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_POSE_DMP_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_POSE_DMP_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | class PoseDmpTrajectoryGenerator : public PoseTrajectoryGenerator { 7 | public: 8 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 9 | 10 | void parse_parameters() override; 11 | 12 | void initialize_trajectory(const franka::RobotState &robot_state, 13 | SkillType skill_type=SkillType::CartesianPoseSkill) override; 14 | 15 | void get_next_step(const franka::RobotState &robot_state) override; 16 | 17 | std::array y_={}; 18 | std::array dy_={}; 19 | 20 | private: 21 | PoseDMPTrajectoryGeneratorMessage pose_dmp_trajectory_params_; 22 | 23 | bool orientation_only_ = false; 24 | bool position_only_ = false; 25 | bool ee_frame_ = false; 26 | 27 | // Variables initialized from shared memory should be doubles. 28 | double alpha_=5.0; 29 | double beta_=5.0/4.0; 30 | double tau_=0.0; 31 | double x_=1.0; 32 | int num_basis_=40; 33 | int num_dims_=6; 34 | int num_sensor_values_=10; 35 | std::array basis_mean_{}; 36 | std::array basis_std_{}; 37 | std::array, 10>, 6> weights_{}; 38 | std::array, 6> initial_sensor_values_{}; 39 | std::array y0_={}; 40 | 41 | void getInitialMeanAndStd(); 42 | }; 43 | 44 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_POSE_DMP_TRAJECTORY_GENERATOR_H_ 45 | -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "franka-interface/trajectory_generator/trajectory_generator.h" 8 | 9 | class PoseTrajectoryGenerator : public TrajectoryGenerator { 10 | public: 11 | using TrajectoryGenerator::TrajectoryGenerator; 12 | 13 | void parse_parameters() override; 14 | 15 | void initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type=SkillType::ImpedanceControlSkill) override; 16 | 17 | /** 18 | * Initialize initial and desired positions and orientations from robot state 19 | */ 20 | void initialize_initial_and_desired_poses(const franka::RobotState &robot_state, SkillType skill_type); 21 | 22 | /** 23 | * Calculate desired pose using the desired position and orientation 24 | */ 25 | void calculate_desired_pose(); 26 | 27 | /** 28 | * Calculate desired position (x, y, z) for the robot to move. 29 | */ 30 | void calculate_desired_position(); 31 | 32 | /** 33 | * Fix goal quaternion by flipping it if the result is negative when dotted with the initial quaternion. 34 | */ 35 | void fix_goal_quaternion(); 36 | 37 | /** 38 | * Returns the desired pose 39 | */ 40 | const std::array& get_desired_pose() const; 41 | 42 | /** 43 | * Returns the desired position 44 | */ 45 | const Eigen::Vector3d& get_desired_position() const; 46 | 47 | /** 48 | * Returns the desired orientation 49 | */ 50 | const Eigen::Quaterniond& get_desired_orientation() const; 51 | 52 | /** 53 | * Returns the goal position 54 | */ 55 | const Eigen::Vector3d& get_goal_position() const; 56 | 57 | /** 58 | * Returns the goal orientation 59 | */ 60 | const Eigen::Quaterniond& get_goal_orientation() const; 61 | 62 | protected: 63 | PoseTrajectoryGeneratorMessage pose_trajectory_params_; 64 | 65 | double quaternion_dist_threshold = 0.9995; 66 | bool same_orientation = false; 67 | 68 | std::array initial_pose_{}; 69 | std::array desired_pose_{}; 70 | std::array goal_pose_{}; 71 | 72 | Eigen::Affine3d initial_transform_{}; 73 | Eigen::Vector3d initial_position_; 74 | Eigen::Quaterniond initial_orientation_; 75 | Eigen::Vector3d desired_position_; 76 | Eigen::Quaterniond desired_orientation_; 77 | Eigen::Vector3d goal_position_; 78 | Eigen::Quaterniond goal_orientation_; 79 | }; 80 | 81 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/quaternion_pose_dmp_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_QUATERNION_POSE_DMP_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_QUATERNION_POSE_DMP_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | #include 7 | 8 | class QuaternionPoseDmpTrajectoryGenerator : public PoseTrajectoryGenerator { 9 | public: 10 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 11 | 12 | void parse_parameters() override; 13 | 14 | void initialize_trajectory(const franka::RobotState &robot_state, 15 | SkillType skill_type=SkillType::CartesianPoseSkill) override; 16 | 17 | void get_next_step(const franka::RobotState &robot_state) override; 18 | 19 | void get_next_quaternion_step(const franka::RobotState &robot_state); 20 | 21 | std::array y_={}; 22 | std::array dy_={}; 23 | std::vector> q_={}; 24 | std::array dq_={}; 25 | 26 | private: 27 | double quaternion_phase(double curr_t, double alpha, double goal_t, double start_t, double int_dt); 28 | Eigen::Array3d quaternion_log(const Eigen::Quaterniond& q); 29 | Eigen::Quaterniond vecExp(const Eigen::Vector3d& input); 30 | 31 | QuaternionPoseDMPTrajectoryGeneratorMessage quat_pose_dmp_trajectory_params_; 32 | 33 | bool ee_frame_ = false; 34 | 35 | // Variables initialized from shared memory should be doubles. 36 | double alpha_pos_=5.0; 37 | double beta_pos_=5.0/4.0; 38 | double tau_pos_=0.0; 39 | double x_pos_=1.0; 40 | double alpha_quat_=5.0; 41 | double beta_quat_=5.0/4.0; 42 | // This variable is used for calcualating the quaternion phase (canonical variable). 43 | // TODO(Mohit): Update this to be similar to the position trajectory. 44 | double alpha_quat_phase_=1.0; 45 | double tau_quat_=0.0; 46 | double x_quat_=1.0; 47 | double curr_time_quat_=0.0; 48 | double start_time_quat_=0.0; 49 | double goal_time_quat_=5.0; 50 | Eigen::Quaterniond goal_quat_; 51 | 52 | int num_basis_pos_=40; 53 | int num_basis_quat_=40; 54 | int num_dims_=7; 55 | int num_sensor_values_pos_=10; 56 | int num_sensor_values_quat_=10; 57 | std::array pos_basis_mean_{}; 58 | std::array pos_basis_std_{}; 59 | std::array, 10>, 3> pos_weights_{}; 60 | std::array, 3> pos_initial_sensor_values_{}; 61 | std::array y0_={}; 62 | 63 | std::array quat_basis_mean_{}; 64 | std::array quat_basis_std_{}; 65 | std::array, 20>, 3> quat_weights_{}; 66 | std::array, 3> quat_initial_sensor_values_{}; 67 | Eigen::Quaterniond q0_; 68 | 69 | Eigen::Quaterniond last_q_quat_; 70 | Eigen::Array3d last_qd_; 71 | Eigen::Array3d last_qdd_; 72 | Eigen::Quaterniond next_q_quat_; 73 | Eigen::Array3d next_qd_; 74 | Eigen::Array3d next_qdd_; 75 | 76 | void getInitialMeanAndStd(); 77 | }; 78 | 79 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_QUATERNION_POSE_DMP_TRAJECTORY_GENERATOR_H_ 80 | -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/relative_linear_pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_RELATIVE_LINEAR_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_RELATIVE_LINEAR_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/relative_pose_trajectory_generator.h" 5 | 6 | class RelativeLinearPoseTrajectoryGenerator : public RelativePoseTrajectoryGenerator { 7 | public: 8 | using RelativePoseTrajectoryGenerator::RelativePoseTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | 12 | }; 13 | 14 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_RELATIVE_LINEAR_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/relative_min_jerk_pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_RELATIVE_MIN_JERK_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_RELATIVE_MIN_JERK_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/relative_pose_trajectory_generator.h" 5 | 6 | class RelativeMinJerkPoseTrajectoryGenerator : public RelativePoseTrajectoryGenerator { 7 | public: 8 | using RelativePoseTrajectoryGenerator::RelativePoseTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | 12 | private: 13 | double slerp_t_ = 0.0; 14 | }; 15 | 16 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_RELATIVE_MIN_JERK_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/relative_pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_RELATIVE_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_RELATIVE_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | class RelativePoseTrajectoryGenerator : public PoseTrajectoryGenerator { 7 | public: 8 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 9 | 10 | void parse_parameters() override; 11 | 12 | void initialize_trajectory(const franka::RobotState &robot_state, 13 | SkillType skill_type=SkillType::ImpedanceControlSkill) override; 14 | 15 | protected: 16 | std::array relative_pose_{}; 17 | Eigen::Vector3d relative_position_; 18 | Eigen::Quaterniond relative_orientation_; 19 | 20 | }; 21 | 22 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_RELATIVE_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/sine_joint_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_SINE_JOINT_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_SINE_JOINT_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 5 | 6 | class SineJointTrajectoryGenerator : public JointTrajectoryGenerator { 7 | public: 8 | using JointTrajectoryGenerator::JointTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | 12 | double sine_t_ = 0.0; 13 | 14 | }; 15 | 16 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_SINE_JOINT_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/sine_pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_SINE_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_SINE_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | class SinePoseTrajectoryGenerator : public PoseTrajectoryGenerator { 7 | public: 8 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 9 | 10 | void get_next_step(const franka::RobotState &robot_state) override; 11 | 12 | private: 13 | double sine_t_ = 0.0; 14 | }; 15 | 16 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_SINE_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/stay_in_initial_joints_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_STAY_IN_INITIAL_JOINTS_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_STAY_IN_INITIAL_JOINTS_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 5 | 6 | class StayInInitialJointsTrajectoryGenerator : public JointTrajectoryGenerator { 7 | public: 8 | using JointTrajectoryGenerator::JointTrajectoryGenerator; 9 | 10 | void parse_parameters() override; 11 | 12 | void get_next_step(const franka::RobotState &robot_state) override; 13 | 14 | protected: 15 | RunTimeMessage run_time_msg_; 16 | }; 17 | 18 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_STAY_IN_INITIAL_JOINTS_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/stay_in_initial_pose_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_STAY_IN_INITIAL_POSE_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_STAY_IN_INITIAL_POSE_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include "franka-interface/trajectory_generator/pose_trajectory_generator.h" 5 | 6 | class StayInInitialPoseTrajectoryGenerator : public PoseTrajectoryGenerator { 7 | public: 8 | using PoseTrajectoryGenerator::PoseTrajectoryGenerator; 9 | 10 | void parse_parameters() override; 11 | 12 | void get_next_step(const franka::RobotState &robot_state) override; 13 | 14 | protected: 15 | RunTimeMessage run_time_msg_; 16 | }; 17 | 18 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_STAY_IN_INITIAL_POSE_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator/trajectory_generator.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_TRAJECTORY_GENERATOR_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_TRAJECTORY_GENERATOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "franka-interface/sensor_data_manager.h" 10 | #include "trajectory_generator_params_msg.pb.h" 11 | 12 | class TrajectoryGenerator { 13 | public: 14 | explicit TrajectoryGenerator(SharedBufferTypePtr p, SensorDataManager* sensor_data_manager) : 15 | params_{p}, 16 | sensor_data_manager_{sensor_data_manager} 17 | {}; 18 | 19 | /** 20 | * Parse parameters from memory. 21 | */ 22 | virtual void parse_parameters() = 0; 23 | 24 | /** 25 | * Initialize trajectory generation after parameter parsing. 26 | */ 27 | virtual void initialize_trajectory(const franka::RobotState &robot_state, 28 | SkillType skill_type) = 0; 29 | 30 | /** 31 | * Get next trajectory step. 32 | */ 33 | virtual void get_next_step(const franka::RobotState &robot_state) = 0; 34 | 35 | /** 36 | * Parse sensor data 37 | */ 38 | virtual void parse_sensor_data(const franka::RobotState &robot_state) {}; 39 | 40 | double run_time_ = 0.0; 41 | double dt_ = 0.001; 42 | double time_ = 0.0; 43 | double t_ = 0.0; 44 | 45 | protected: 46 | SharedBufferTypePtr params_=0; 47 | SensorDataManager* sensor_data_manager_; 48 | 49 | }; 50 | 51 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_TRAJECTORY_GENERATOR_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/trajectory_generator_factory.h: -------------------------------------------------------------------------------- 1 | #ifndef FRANKA_INTERFACE_TRAJECTORY_GENERATOR_FACTORY_H_ 2 | #define FRANKA_INTERFACE_TRAJECTORY_GENERATOR_FACTORY_H_ 3 | 4 | #include "franka-interface-common/definitions.h" 5 | #include "franka-interface/run_loop_shared_memory_handler.h" 6 | #include "franka-interface/sensor_data_manager.h" 7 | 8 | class TrajectoryGenerator; 9 | 10 | class TrajectoryGeneratorFactory { 11 | public: 12 | 13 | TrajectoryGeneratorFactory() {}; 14 | 15 | /** 16 | * Get trajectory generator for skill. 17 | * 18 | * @param memory_region Region of the memory where the parameters 19 | * will be stored. 20 | */ 21 | TrajectoryGenerator* getTrajectoryGeneratorForSkill(SharedBufferTypePtr buffer, SensorDataManager* sensor_data_manager); 22 | 23 | }; 24 | 25 | #endif // FRANKA_INTERFACE_TRAJECTORY_GENERATOR_FACTORY_H_ -------------------------------------------------------------------------------- /franka-interface/include/franka-interface/utils/logger_utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 3/23/19. 3 | // 4 | 5 | #ifndef FRANKA_INTERFACE_LOGGER_UTILS_H 6 | #define FRANKA_INTERFACE_LOGGER_UTILS_H 7 | 8 | #include 9 | #include 10 | 11 | class LoggerUtils { 12 | public: 13 | static std::vector all_logger_files(std::string logdir); 14 | 15 | static int integer_suffix_for_new_log_file(std::string logdir); 16 | }; 17 | 18 | #endif //FRANKA_INTERFACE_LOGGER_UTILS_H 19 | -------------------------------------------------------------------------------- /franka-interface/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 3.4) 2 | 3 | project(proto CXX) 4 | 5 | set(CMAKE_CXX_STANDARD 14) 6 | 7 | find_package(Protobuf REQUIRED) 8 | 9 | protobuf_generate_cpp(PROTO_FEEDBACK_CONTROLLER_PARAMS_MSG PROTO_FEEDBACK_CONTROLLER_PARAMS_MSG_HDRS feedback_controller_params_msg.proto) 10 | protobuf_generate_cpp(PROTO_ROBOT_STATE_MSG PROTO_ROBOT_STATE_MSG_HDRS robot_state_msg.proto) 11 | protobuf_generate_cpp(PROTO_SENSOR_MSG PROTO_SENSOR_MSG_HDRS sensor_msg.proto) 12 | protobuf_generate_cpp(PROTO_TERMINATION_HANDLER_PARAMS_MSG PROTO_TERMINATION_HANDLER_PARAMS_MSG_HDRS termination_handler_params_msg.proto) 13 | protobuf_generate_cpp(PROTO_TRAJECTORY_GENERATOR_PARAMS_MSG PROTO_TRAJECTORY_GENERATOR_PARAMS_MSG_HDRS trajectory_generator_params_msg.proto) 14 | 15 | 16 | set(ProtobufIncludePath ${CMAKE_CURRENT_BINARY_DIR} 17 | CACHE INTERNAL "Path to generated protobuf files.") 18 | 19 | add_library(proto SHARED ${PROTO_FEEDBACK_CONTROLLER_PARAMS_MSG} ${PROTO_FEEDBACK_CONTROLLER_PARAMS_MSG_HDRS} 20 | ${PROTO_ROBOT_STATE_MSG} ${PROTO_ROBOT_STATE_MSG_HDRS} 21 | ${PROTO_SENSOR_MSG} ${PROTO_SENSOR_MSG_HDRS} 22 | ${PROTO_TERMINATION_HANDLER_PARAMS_MSG} ${PROTO_TERMINATION_HANDLER_PARAMS_MSG_HDRS} 23 | ${PROTO_TRAJECTORY_GENERATOR_PARAMS_MSG} ${PROTO_TRAJECTORY_GENERATOR_PARAMS_MSG_HDRS}) 24 | target_link_libraries(proto ${PROTOBUF_LIBRARIES}) 25 | target_include_directories(proto PUBLIC ${CMAKE_CURRENT_BINARY_DIR}) 26 | 27 | # set_target_properties(proto PROPERTIES POSITION_INDEPENDENT_CODE ON) 28 | -------------------------------------------------------------------------------- /franka-interface/proto/feedback_controller_params_msg.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | message CartesianImpedanceFeedbackControllerMessage { 4 | repeated double translational_stiffnesses = 1; 5 | repeated double rotational_stiffnesses = 2; 6 | } 7 | 8 | message ForceAxisFeedbackControllerMessage { 9 | required double translational_stiffness = 1; 10 | required double rotational_stiffness = 2; 11 | 12 | repeated double axis = 3; 13 | } 14 | 15 | message JointImpedanceFeedbackControllerMessage { 16 | repeated double k_gains = 1; 17 | repeated double d_gains = 2; 18 | } 19 | 20 | message InternalImpedanceFeedbackControllerMessage { 21 | repeated double cartesian_impedances = 1; 22 | repeated double joint_impedances = 2; 23 | } 24 | 25 | message ForcePositionFeedbackControllerMessage { 26 | repeated double position_kps_cart = 1; 27 | repeated double force_kps_cart = 2; 28 | repeated double position_kps_joint = 3; 29 | repeated double force_kps_joint = 4; 30 | repeated double selection = 5; 31 | required bool use_cartesian_gains = 6; 32 | } 33 | 34 | message JointTorqueFeedbackControllerMessage { 35 | repeated double selection = 1; 36 | repeated double remove_gravity = 2; 37 | repeated double joint_torques = 3; 38 | repeated double k_gains = 4; 39 | repeated double d_gains = 5; 40 | } -------------------------------------------------------------------------------- /franka-interface/proto/sensor_msg.proto: -------------------------------------------------------------------------------- 1 | // franka-interface/proto/sensor_msg.proto and frankapy/protosensor_msg.proto should match 2 | 3 | syntax = "proto2"; 4 | 5 | message BoundingBox { 6 | required string name = 1; 7 | required int32 id = 2; 8 | 9 | required int32 x = 3; 10 | required int32 y = 4; 11 | required int32 w = 5; 12 | required int32 h = 6; 13 | } 14 | 15 | message JointPositionVelocitySensorMessage { 16 | required int32 id = 1; 17 | required double timestamp = 2; 18 | required double seg_run_time = 3; 19 | 20 | repeated double joints = 4; 21 | repeated double joint_vels = 5; 22 | } 23 | 24 | message PosePositionVelocitySensorMessage { 25 | required int32 id = 1; 26 | required double timestamp = 2; 27 | required double seg_run_time = 3; 28 | 29 | repeated double position = 4; 30 | repeated double quaternion = 5; 31 | repeated double pose_velocities = 6; 32 | } 33 | 34 | message JointVelocitySensorMessage { 35 | required int32 id = 1; 36 | required double timestamp = 2; 37 | 38 | repeated double joint_velocities = 3; 39 | } 40 | 41 | message JointPositionSensorMessage { 42 | required int32 id = 1; 43 | required double timestamp = 2; 44 | 45 | repeated double joints = 3; 46 | } 47 | 48 | message PosePositionSensorMessage { 49 | required int32 id = 1; 50 | required double timestamp = 2; 51 | 52 | repeated double position = 3; 53 | repeated double quaternion = 4; 54 | } 55 | 56 | message CartesianVelocitySensorMessage { 57 | required int32 id = 1; 58 | required double timestamp = 2; 59 | 60 | repeated double cartesian_velocities = 3; 61 | } 62 | 63 | message ShouldTerminateSensorMessage { 64 | required double timestamp = 1; 65 | required bool should_terminate = 2; 66 | } 67 | 68 | message CartesianImpedanceSensorMessage { 69 | required int32 id = 1; 70 | required double timestamp = 2; 71 | 72 | repeated double translational_stiffnesses = 3; 73 | repeated double rotational_stiffnesses = 4; 74 | } 75 | 76 | message ForcePositionSensorMessage { 77 | required int32 id = 1; 78 | required double timestamp = 2; 79 | required double seg_run_time = 3; 80 | 81 | repeated double pose = 4; 82 | repeated double force = 5; 83 | } 84 | 85 | message ForcePositionControllerSensorMessage { 86 | required int32 id = 1; 87 | required double timestamp = 2; 88 | 89 | repeated double position_kps_cart = 3; 90 | repeated double force_kps_cart = 4; 91 | repeated double position_kps_joint = 5; 92 | repeated double force_kps_joint = 6; 93 | repeated double selection = 7; 94 | } 95 | 96 | message JointTorqueControllerSensorMessage { 97 | required int32 id = 1; 98 | required double timestamp = 2; 99 | 100 | repeated double selection = 3; 101 | repeated double remove_gravity = 4; 102 | repeated double joint_torques = 5; 103 | repeated double k_gains = 6; 104 | repeated double d_gains = 7; 105 | } -------------------------------------------------------------------------------- /franka-interface/proto/termination_handler_params_msg.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | message ContactTerminationHandlerMessage { 4 | required double buffer_time = 1; 5 | 6 | repeated double force_thresholds = 2; 7 | repeated double torque_thresholds = 3; 8 | } 9 | 10 | message JointThresholdMessage { 11 | required double buffer_time = 1; 12 | 13 | repeated double joint_thresholds = 2; 14 | } 15 | 16 | message PoseThresholdMessage { 17 | required double buffer_time = 1; 18 | 19 | repeated double position_thresholds = 2; 20 | repeated double orientation_thresholds = 3; 21 | } 22 | 23 | message TimeTerminationHandlerMessage { 24 | required double buffer_time = 1; 25 | } -------------------------------------------------------------------------------- /franka-interface/src/duration.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017 Franka Emika GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | 5 | Duration::Duration() noexcept : duration_{0u} {} 6 | 7 | Duration::Duration(std::chrono::duration duration) noexcept 8 | : duration_{duration} {} 9 | 10 | Duration::Duration(uint64_t milliseconds) noexcept : duration_{milliseconds} {} 11 | 12 | Duration::operator std::chrono::duration() const noexcept { 13 | return duration_; 14 | } 15 | 16 | double Duration::toSec() const noexcept { 17 | return std::chrono::duration_cast>(duration_).count(); 18 | } 19 | 20 | uint64_t Duration::toMSec() const noexcept { 21 | return duration_.count(); 22 | } 23 | 24 | Duration Duration::operator+(const Duration& rhs) const noexcept { 25 | return duration_ + rhs.duration_; 26 | } 27 | 28 | Duration& Duration::operator+=(const Duration& rhs) noexcept { 29 | duration_ += rhs.duration_; 30 | return *this; 31 | } 32 | 33 | Duration Duration::operator-(const Duration& rhs) const noexcept { 34 | return duration_ - rhs.duration_; 35 | } 36 | 37 | Duration& Duration::operator-=(const Duration& rhs) noexcept { 38 | duration_ -= rhs.duration_; 39 | return *this; 40 | } 41 | 42 | Duration Duration::operator*(uint64_t rhs) const noexcept { 43 | return duration_ * rhs; 44 | } 45 | 46 | Duration& Duration::operator*=(uint64_t rhs) noexcept { 47 | duration_ *= rhs; 48 | return *this; 49 | } 50 | 51 | uint64_t Duration::operator/(const Duration& rhs) const noexcept { 52 | return duration_ / rhs.duration_; 53 | } 54 | 55 | Duration Duration::operator/(uint64_t rhs) const noexcept { 56 | return duration_ / rhs; 57 | } 58 | 59 | Duration& Duration::operator/=(uint64_t rhs) noexcept { 60 | duration_ /= rhs; 61 | return *this; 62 | } 63 | 64 | Duration Duration::operator%(const Duration& rhs) const noexcept { 65 | return duration_ % rhs.duration_; 66 | } 67 | 68 | Duration Duration::operator%(uint64_t rhs) const noexcept { 69 | return duration_ % rhs; 70 | } 71 | 72 | Duration& Duration::operator%=(const Duration& rhs) noexcept { 73 | duration_ %= rhs.duration_; 74 | return *this; 75 | } 76 | 77 | Duration& Duration::operator%=(uint64_t rhs) noexcept { 78 | duration_ %= rhs; 79 | return *this; 80 | } 81 | 82 | bool Duration::operator==(const Duration& rhs) const noexcept { 83 | return duration_ == rhs.duration_; 84 | } 85 | 86 | bool Duration::operator!=(const Duration& rhs) const noexcept { 87 | return duration_ != rhs.duration_; 88 | } 89 | 90 | bool Duration::operator<(const Duration& rhs) const noexcept { 91 | return duration_ < rhs.duration_; 92 | } 93 | 94 | bool Duration::operator<=(const Duration& rhs) const noexcept { 95 | return duration_ <= rhs.duration_; 96 | } 97 | 98 | bool Duration::operator>(const Duration& rhs) const noexcept { 99 | return duration_ > rhs.duration_; 100 | } 101 | 102 | bool Duration::operator>=(const Duration& rhs) const noexcept { 103 | return duration_ >= rhs.duration_; 104 | } 105 | 106 | Duration operator*(uint64_t lhs, const Duration& rhs) noexcept { 107 | return rhs * lhs; 108 | } 109 | -------------------------------------------------------------------------------- /franka-interface/src/feedback_controller/feedback_controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 11/25/18. 3 | // 4 | 5 | #include "franka-interface/feedback_controller/feedback_controller.h" 6 | -------------------------------------------------------------------------------- /franka-interface/src/feedback_controller/joint_impedance_feedback_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "franka-interface/feedback_controller/joint_impedance_feedback_controller.h" 2 | 3 | #include 4 | 5 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 6 | 7 | void JointImpedanceFeedbackController::parse_parameters() { 8 | // First parameter is reserved for the type 9 | 10 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 11 | 12 | bool parsed_params = joint_impedance_feedback_params_.ParseFromArray(params_ + 5, data_size); 13 | 14 | if(parsed_params){ 15 | 16 | if(joint_impedance_feedback_params_.k_gains_size() == 7){ 17 | for(size_t i = 0; i < 7; i++) { 18 | k_gains_[i] = joint_impedance_feedback_params_.k_gains(i); 19 | } 20 | } 21 | 22 | if(joint_impedance_feedback_params_.d_gains_size() == 7){ 23 | for(size_t i = 0; i < 7; i++) { 24 | d_gains_[i] = joint_impedance_feedback_params_.d_gains(i); 25 | } 26 | } 27 | } else { 28 | std::cout << "Parsing JointImpedanceFeedbackController params failed. Data size = " << data_size << std::endl; 29 | } 30 | } 31 | 32 | void JointImpedanceFeedbackController::initialize_controller(FrankaRobot *robot) { 33 | model_ = robot->getModel(); 34 | } 35 | 36 | void JointImpedanceFeedbackController::get_next_step(const franka::RobotState &robot_state, 37 | TrajectoryGenerator *traj_generator) { 38 | 39 | // Read current coriolis terms from model. 40 | std::array coriolis = model_->coriolis(robot_state); 41 | 42 | JointTrajectoryGenerator* joint_trajectory_generator = dynamic_cast(traj_generator); 43 | 44 | if(joint_trajectory_generator == nullptr) { 45 | throw std::bad_cast(); 46 | } 47 | 48 | std::array desired_joints = joint_trajectory_generator->get_desired_joints(); 49 | 50 | // Compute torque command from joint impedance control law. 51 | // Note: The answer to our Cartesian pose inverse kinematics is always in state.q_d with one 52 | // time step delay. 53 | std::array tau_d_calculated; 54 | for (size_t i = 0; i < 7; i++) { 55 | tau_d_calculated[i] = k_gains_[i] * (desired_joints[i] - robot_state.q[i]) 56 | - d_gains_[i] * robot_state.dq[i] + coriolis[i]; 57 | } 58 | 59 | // The following line is only necessary if rate limiting is not activate. If we activated 60 | // rate limiting for the control loop (activated by default), the torque would anyway be 61 | // adjusted! 62 | std::array tau_d_rate_limited = 63 | franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated, robot_state.tau_J_d); 64 | 65 | tau_d_array_ = tau_d_rate_limited; 66 | } 67 | -------------------------------------------------------------------------------- /franka-interface/src/feedback_controller/noop_feedback_controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 11/25/18. 3 | // 4 | 5 | #include "franka-interface/feedback_controller/noop_feedback_controller.h" 6 | 7 | void NoopFeedbackController::parse_parameters() { 8 | // pass 9 | } 10 | 11 | void NoopFeedbackController::initialize_controller(FrankaRobot *robot) { 12 | // pass 13 | } 14 | 15 | void NoopFeedbackController::get_next_step(const franka::RobotState &robot_state, 16 | TrajectoryGenerator *traj_generator) { 17 | // pass 18 | } -------------------------------------------------------------------------------- /franka-interface/src/feedback_controller/pass_through_feedback_controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jacky on 2/13/19. 3 | // 4 | 5 | #include "franka-interface/feedback_controller/pass_through_feedback_controller.h" 6 | 7 | #include "franka-interface/trajectory_generator/impulse_trajectory_generator.h" 8 | 9 | void PassThroughFeedbackController::parse_parameters() { 10 | // pass 11 | } 12 | 13 | void PassThroughFeedbackController::initialize_controller(FrankaRobot *robot) { 14 | model_ = robot->getModel(); 15 | } 16 | 17 | void PassThroughFeedbackController::get_next_step(const franka::RobotState &robot_state, 18 | TrajectoryGenerator *traj_generator) { 19 | 20 | ImpulseTrajectoryGenerator* impulse_trajectory_generator = dynamic_cast(traj_generator); 21 | 22 | if(impulse_trajectory_generator == nullptr) { 23 | throw std::bad_cast(); 24 | } 25 | 26 | std::array desired_force_torque_array = impulse_trajectory_generator->get_desired_force_torque(); 27 | Eigen::Map desired_force_torque(desired_force_torque_array.data(), 6); 28 | 29 | // get jacobian 30 | std::array jacobian_array = model_->zeroJacobian(franka::Frame::kEndEffector, robot_state); 31 | Eigen::Map> jacobian(jacobian_array.data()); 32 | 33 | std::array coriolis_array = model_->coriolis(robot_state); 34 | Eigen::Map> coriolis(coriolis_array.data()); 35 | 36 | // compute control torques 37 | Eigen::VectorXd tau_d(7); 38 | tau_d << jacobian.transpose() * desired_force_torque + coriolis; 39 | 40 | Eigen::VectorXd::Map(&tau_d_array_[0], 7) = tau_d; 41 | } -------------------------------------------------------------------------------- /franka-interface/src/feedback_controller/set_internal_impedance_feedback_controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by kevin on 3/27/19. 3 | // 4 | 5 | #include "franka-interface/feedback_controller/set_internal_impedance_feedback_controller.h" 6 | 7 | void SetInternalImpedanceFeedbackController::parse_parameters() { 8 | // First parameter is reserved for the type 9 | 10 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 11 | 12 | bool parsed_params = internal_impedance_feedback_params_.ParseFromArray(params_ + 5, data_size); 13 | 14 | if(parsed_params){ 15 | 16 | if(internal_impedance_feedback_params_.cartesian_impedances_size() == 6){ 17 | for(size_t i = 0; i < 7; i++) { 18 | cartesian_impedances_[i] = internal_impedance_feedback_params_.cartesian_impedances(i); 19 | } 20 | set_cartesian_impedance_ = true; 21 | } 22 | 23 | if(internal_impedance_feedback_params_.joint_impedances_size() == 7){ 24 | for(size_t i = 0; i < 7; i++) { 25 | joint_impedances_[i] = internal_impedance_feedback_params_.joint_impedances(i); 26 | } 27 | set_joint_impedance_ = true; 28 | } 29 | } else { 30 | std::cout << "Parsing SetInternalImpedanceFeedbackController params failed. Data size = " << data_size << std::endl; 31 | } 32 | } 33 | 34 | void SetInternalImpedanceFeedbackController::initialize_controller(FrankaRobot *robot) { 35 | 36 | if(set_cartesian_impedance_) { 37 | bool cartesian_impedance_values_valid = true; 38 | for(size_t i = 0; i < cartesian_impedances_.size(); i++) { 39 | if(cartesian_impedances_[i] < 0.0 or cartesian_impedances_[i] > max_cartesian_impedance_) { 40 | cartesian_impedance_values_valid = false; 41 | } 42 | } 43 | if(cartesian_impedance_values_valid) { 44 | robot->setCartesianImpedance(cartesian_impedances_); 45 | } 46 | } 47 | 48 | if(set_joint_impedance_) { 49 | bool joint_impedance_values_valid = true; 50 | for(size_t i = 0; i < joint_impedances_.size(); i++) { 51 | if(joint_impedances_[i] < 0.0 or joint_impedances_[i] > max_joint_impedance_) { 52 | joint_impedance_values_valid = false; 53 | } 54 | } 55 | if(joint_impedance_values_valid) { 56 | robot->setJointImpedance(joint_impedances_); 57 | } 58 | } 59 | } 60 | 61 | void SetInternalImpedanceFeedbackController::get_next_step(const franka::RobotState &robot_state, 62 | TrajectoryGenerator *traj_generator) { 63 | // pass 64 | } -------------------------------------------------------------------------------- /franka-interface/src/run_loop_logger.cpp: -------------------------------------------------------------------------------- 1 | #include "franka-interface/run_loop_logger.h" 2 | 3 | #include 4 | #include 5 | 6 | void RunLoopLogger::add_log(std::string log, RunLoopLoggerCode code) { 7 | if (code == RunLoopLoggerCode::INFO) { 8 | add_info_log(log); 9 | } else if (code == RunLoopLoggerCode::ERROR) { 10 | add_error_log(log); 11 | } else if (code == RunLoopLoggerCode::WARNING) { 12 | add_warning_log(log); 13 | } else if (code == RunLoopLoggerCode::ABORT) { 14 | // pass 15 | } else { 16 | // pass 17 | } 18 | } 19 | 20 | /** 21 | * TODO(Mohit): Add timestamps to all of these. 22 | */ 23 | 24 | void RunLoopLogger::add_error_log(std::string log) { 25 | if (mutex_.try_lock()) { 26 | error_logs_.push_back(log); 27 | mutex_.unlock(); 28 | } 29 | } 30 | 31 | void RunLoopLogger::add_warning_log(std::string log) { 32 | if (mutex_.try_lock()) { 33 | warning_logs_.push_back(log); 34 | mutex_.unlock(); 35 | } 36 | } 37 | 38 | void RunLoopLogger::add_info_log(std::string log) { 39 | if (mutex_.try_lock()) { 40 | info_logs_.push_back(log); 41 | mutex_.unlock(); 42 | } 43 | } 44 | 45 | void RunLoopLogger::print_error_logs_to_console(std::vector& logs) { 46 | if (mutex_.try_lock()) { 47 | for (auto it = logs.begin(); it != logs.end(); it++) { 48 | std::cout << *it << "\n"; 49 | } 50 | logs.clear(); 51 | mutex_.unlock(); 52 | } 53 | } 54 | 55 | void RunLoopLogger::print_error_log() { 56 | print_error_logs_to_console(error_logs_); 57 | } 58 | 59 | void RunLoopLogger::print_warning_log() { 60 | print_error_logs_to_console(warning_logs_); 61 | } 62 | 63 | void RunLoopLogger::print_info_log() { 64 | print_error_logs_to_console(info_logs_); 65 | } 66 | -------------------------------------------------------------------------------- /franka-interface/src/sensor_data.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 11/25/18. 3 | // 4 | 5 | #include "franka-interface/sensor_data.h" 6 | -------------------------------------------------------------------------------- /franka-interface/src/sensor_data_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "franka-interface/sensor_data_manager.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | SensorDataManagerReadStatus SensorDataManager::readTrajectoryGeneratorSensorMessage(google::protobuf::Message &message) { 8 | return readSensorMessage(message, trajectory_generator_buffer_); 9 | } 10 | 11 | SensorDataManagerReadStatus SensorDataManager::readFeedbackControllerSensorMessage(google::protobuf::Message &message) { 12 | return readSensorMessage(message, feedback_controller_buffer_); 13 | } 14 | 15 | SensorDataManagerReadStatus SensorDataManager::readTerminationHandlerSensorMessage(google::protobuf::Message &message) { 16 | return readSensorMessage(message, termination_handler_buffer_); 17 | } 18 | 19 | SensorDataManagerReadStatus SensorDataManager::readSensorMessage(google::protobuf::Message &message, SensorBufferTypePtr buffer) { 20 | std::function 21 | parse_callback = [&](const void *bytes, int data_size) -> bool { 22 | // get state variables 23 | return message.ParseFromArray(bytes, data_size); 24 | }; 25 | return readMessageAsBytes(parse_callback, buffer); 26 | } 27 | 28 | SensorDataManagerReadStatus SensorDataManager::readMessageAsBytes( 29 | std::function< bool(const void *bytes, int data_size)> parse_callback, SensorBufferTypePtr buffer) { 30 | SensorDataManagerReadStatus status; 31 | 32 | uint8_t has_new_message = buffer[0]; 33 | if (has_new_message == 1) { 34 | uint8_t sensor_msg_type = buffer[1]; // Currently not used 35 | int data_size = (buffer[2] + (buffer[3] << 8) + (buffer[4] << 16) + (buffer[5] << 24)); 36 | 37 | if (parse_callback(buffer + 6, data_size)) { 38 | status = SensorDataManagerReadStatus::SUCCESS; 39 | buffer[0] = 0; 40 | } else { 41 | status = SensorDataManagerReadStatus::FAIL_TO_READ; 42 | } 43 | } else { 44 | status = SensorDataManagerReadStatus::NO_NEW_MESSAGE; 45 | } 46 | 47 | return status; 48 | } 49 | 50 | void SensorDataManager::clearBuffers() { 51 | try { 52 | if (buffer_group_mutex_->try_lock()) { 53 | int sensor_buffer_size = shared_memory_info_.getSizeForSensorData(); 54 | memset(trajectory_generator_buffer_, 0, sensor_buffer_size); 55 | memset(feedback_controller_buffer_, 0, sensor_buffer_size); 56 | memset(termination_handler_buffer_, 0, sensor_buffer_size); 57 | buffer_group_mutex_->unlock(); 58 | } 59 | } catch (boost::interprocess::lock_exception) { 60 | } 61 | } 62 | 63 | boost::interprocess::interprocess_mutex* SensorDataManager::getSensorBufferGroupMutex() { 64 | return buffer_group_mutex_; 65 | } -------------------------------------------------------------------------------- /franka-interface/src/skill_info_manager.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 11/20/18. 3 | // 4 | 5 | #include "franka-interface/skill_info_manager.h" 6 | 7 | #include 8 | 9 | SkillInfoManager::SkillInfoManager() { 10 | } 11 | 12 | BaseSkill* SkillInfoManager::get_current_skill() { 13 | if (skill_list_.size() == 0) { 14 | // returns NULL 15 | return 0; 16 | } 17 | return skill_list_.back(); 18 | } 19 | 20 | BaseMetaSkill* SkillInfoManager::get_current_meta_skill() { 21 | if (meta_skill_list_.size() == 0) { 22 | // returns NULL 23 | return 0; 24 | } 25 | return meta_skill_list_.back(); 26 | } 27 | 28 | bool SkillInfoManager::is_currently_executing_skill() { 29 | if (skill_list_.size() == 0){ 30 | return false; 31 | } 32 | BaseSkill*skill = skill_list_.back(); 33 | SkillStatus status = skill->get_current_skill_status(); 34 | return (status == SkillStatus::TO_START or status == SkillStatus::RUNNING); 35 | } 36 | 37 | bool SkillInfoManager::is_waiting_for_new_skill() { 38 | if (skill_list_.size() == 0){ 39 | return true; 40 | } 41 | return (*skill_list_.back()).get_current_skill_status() == SkillStatus::FINISHED; 42 | } 43 | 44 | void SkillInfoManager::add_skill(BaseSkill *skill) { 45 | assert(is_waiting_for_new_skill()); 46 | skill_list_.push_back(skill); 47 | } 48 | 49 | void SkillInfoManager::add_meta_skill(BaseMetaSkill *skill) { 50 | meta_skill_list_.push_back(skill); 51 | } 52 | 53 | BaseMetaSkill* SkillInfoManager::get_meta_skill_with_id(int meta_skill_id) { 54 | for (auto it = meta_skill_list_.rbegin(); it != meta_skill_list_.rend(); it++) { 55 | if ((*it)->getMetaSkillId() == meta_skill_id) { 56 | return *it; 57 | } 58 | } 59 | return nullptr; 60 | } 61 | 62 | void SkillInfoManager::clear_skill_and_meta_skill_list() { 63 | skill_list_.clear(); 64 | meta_skill_list_.clear(); 65 | } 66 | -------------------------------------------------------------------------------- /franka-interface/src/skills/base_meta_skill.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 12/9/18. 3 | // 4 | 5 | #include "franka-interface/skills/base_meta_skill.h" 6 | 7 | #include "franka-interface/run_loop.h" 8 | 9 | int BaseMetaSkill::getMetaSkillId() { 10 | return skill_idx_; 11 | } 12 | 13 | bool BaseMetaSkill::isComposableSkill() { 14 | return is_composable_; 15 | } 16 | 17 | void BaseMetaSkill::setMetaSkillStatus(SkillStatus new_status) { 18 | skill_status_ = new_status; 19 | } 20 | 21 | SkillStatus BaseMetaSkill::getCurrentMetaSkillStatus() { 22 | return skill_status_; 23 | } 24 | 25 | void BaseMetaSkill::execute_skill_on_franka(run_loop* run_loop, FrankaRobot *robot, 26 | FrankaGripper* gripper, RobotStateData *robot_state_data) { 27 | BaseSkill* skill = run_loop->getSkillInfoManager()->get_current_skill(); 28 | if (skill != nullptr) { 29 | model_ = robot->getModel(); 30 | skill->execute_skill_on_franka(run_loop, robot, gripper, robot_state_data); 31 | run_loop->finish_current_skill(skill); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /franka-interface/src/skills/base_skill.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by iamlab on 12/2/18. 3 | // 4 | #include "franka-interface/skills/base_skill.h" 5 | 6 | int BaseSkill::get_skill_id() { 7 | return skill_idx_; 8 | } 9 | 10 | int BaseSkill::get_meta_skill_id() { 11 | return meta_skill_idx_; 12 | } 13 | 14 | std::string BaseSkill::get_description() { 15 | return description_; 16 | } 17 | 18 | void BaseSkill::set_skill_status(SkillStatus status) { 19 | // TODO(Mohit): Maybe add checks such that task status progresses 20 | // in one direction. 21 | skill_status_ = status; 22 | } 23 | 24 | SkillStatus BaseSkill::get_current_skill_status() { 25 | return skill_status_; 26 | } 27 | 28 | TrajectoryGenerator* BaseSkill::get_trajectory_generator() { 29 | return traj_generator_; 30 | } 31 | 32 | FeedbackController* BaseSkill::get_feedback_controller() { 33 | return feedback_controller_; 34 | } 35 | 36 | TerminationHandler* BaseSkill::get_termination_handler() { 37 | return termination_handler_; 38 | } 39 | 40 | void BaseSkill::start_skill(FrankaRobot* robot, 41 | TrajectoryGenerator* traj_generator, 42 | FeedbackController* feedback_controller, 43 | TerminationHandler* termination_handler) { 44 | skill_status_ = SkillStatus::TO_START; 45 | traj_generator_ = traj_generator; 46 | feedback_controller_ = feedback_controller; 47 | termination_handler_ = termination_handler; 48 | termination_handler_->initialize_handler(robot); 49 | feedback_controller_->initialize_controller(robot); 50 | model_ = robot->getModel(); 51 | } 52 | 53 | bool BaseSkill::has_terminated(FrankaRobot* robot) { 54 | return termination_handler_->has_terminated(); 55 | } 56 | 57 | bool BaseSkill::has_terminated_by_virt_coll() { 58 | return termination_handler_->has_terminated_by_virt_coll(); 59 | } 60 | 61 | void BaseSkill::write_result_to_shared_memory(SharedBufferTypePtr result_buffer, FrankaRobot* robot) { 62 | std::cout << "Should write result to shared memory\n"; 63 | } 64 | 65 | void BaseSkill::write_feedback_to_shared_memory(SharedBufferTypePtr feedback_buffer) { 66 | std::cout << "Should write feedback to shared memory\n"; 67 | } 68 | 69 | -------------------------------------------------------------------------------- /franka-interface/src/skills/gripper_skill.cpp: -------------------------------------------------------------------------------- 1 | #include "franka-interface/skills/gripper_skill.h" 2 | 3 | #include 4 | 5 | #include "franka-interface/run_loop.h" 6 | #include "franka-interface/run_loop_shared_memory_handler.h" 7 | #include "franka-interface/trajectory_generator/gripper_trajectory_generator.h" 8 | 9 | #include 10 | 11 | void GripperSkill::execute_skill_on_franka(run_loop* run_loop, 12 | FrankaRobot *robot, 13 | FrankaGripper *gripper, 14 | RobotStateData *robot_state_data) { 15 | RunLoopSharedMemoryHandler* shared_memory_handler = run_loop->get_shared_memory_handler(); 16 | RunLoopProcessInfo* run_loop_info = shared_memory_handler->getRunLoopProcessInfo(); 17 | boost::interprocess::scoped_lock lock( 18 | *(shared_memory_handler->getRunLoopProcessInfoMutex()), 19 | boost::interprocess::defer_lock); 20 | 21 | franka::RobotState robot_state = robot->getRobotState(); 22 | 23 | try { 24 | if (lock.try_lock()) { 25 | run_loop_info->set_time_skill_started_in_robot_time(robot_state.time.toSec()); 26 | lock.unlock(); 27 | } 28 | } catch (boost::interprocess::lock_exception) { 29 | // Do nothing 30 | } 31 | 32 | if (run_loop->with_gripper_) { 33 | 34 | franka::GripperState gripper_state = gripper->getGripperState(); 35 | 36 | // Check for the maximum grasping width. 37 | 38 | GripperTrajectoryGenerator *gripper_traj_generator = static_cast< 39 | GripperTrajectoryGenerator *>(traj_generator_); 40 | double desired_gripper_width = gripper_traj_generator->getWidth(); 41 | if (gripper_state.max_width < desired_gripper_width) { 42 | std::cout << "Object is too large for the current fingers on the gripper." << std::endl; 43 | return_status_ = false; 44 | return; 45 | } 46 | 47 | double desired_gripper_speed = gripper_traj_generator->getSpeed(); 48 | if (gripper_traj_generator->isGraspSkill()) { 49 | // TOOD(Mohit): Maybe stop the gripper before trying to grip again? 50 | franka::GripperState gripper_state = gripper->getGripperState(); 51 | if (!gripper_state.is_grasped) { 52 | return_status_ = gripper->gripper_.grasp(desired_gripper_width, 53 | desired_gripper_speed, gripper_traj_generator->getForce(), 54 | 0.1, 0.1); 55 | } 56 | } else { 57 | return_status_ = gripper->gripper_.move(desired_gripper_width, desired_gripper_speed); 58 | } 59 | 60 | gripper_state = gripper->getGripperState(); 61 | } 62 | 63 | robot_state = robot->getRobotState(); 64 | try { 65 | if (lock.try_lock()) { 66 | run_loop_info->set_time_skill_finished_in_robot_time(robot_state.time.toSec()); 67 | lock.unlock(); 68 | } 69 | } catch (boost::interprocess::lock_exception) { 70 | // Do nothing 71 | } 72 | 73 | termination_handler_->done_ = true; 74 | 75 | } 76 | 77 | bool GripperSkill::has_terminated(FrankaRobot *robot){ 78 | // Wait for some time before terminating this skill. 79 | return true; 80 | } 81 | -------------------------------------------------------------------------------- /franka-interface/src/termination_handler/final_joint_termination_handler.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 11/30/18. 3 | // 4 | 5 | #include "franka-interface/termination_handler/final_joint_termination_handler.h" 6 | 7 | #include 8 | 9 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 10 | 11 | void FinalJointTerminationHandler::parse_parameters() { 12 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 13 | 14 | bool parsed_params = joint_termination_params_.ParseFromArray(params_ + 5, data_size); 15 | 16 | if(parsed_params){ 17 | buffer_time_ = joint_termination_params_.buffer_time(); 18 | 19 | if (joint_termination_params_.joint_thresholds_size() == 7) { 20 | for (int i = 0; i < 7; i++) { 21 | joint_thresholds_[i] = joint_termination_params_.joint_thresholds(i); 22 | } 23 | } else { 24 | for (int i = 0; i < 7; i++) { 25 | joint_thresholds_[i] = default_joint_threshold_; 26 | } 27 | } 28 | } else { 29 | std::cout << "Parsing FinalJointTerminationHandler params failed. Data size = " << data_size << std::endl; 30 | } 31 | } 32 | 33 | bool FinalJointTerminationHandler::should_terminate(const franka::RobotState &robot_state, 34 | franka::Model *model, 35 | TrajectoryGenerator *trajectory_generator) { 36 | 37 | check_terminate_virtual_wall_collisions(robot_state, model); 38 | check_terminate_joint_limits(robot_state); 39 | check_terminate_preempt(); 40 | check_terminate_time(trajectory_generator); 41 | 42 | if (!done_) { 43 | JointTrajectoryGenerator *joint_traj_generator = 44 | dynamic_cast(trajectory_generator); 45 | 46 | if(joint_traj_generator == nullptr) { 47 | throw std::bad_cast(); 48 | } 49 | 50 | std::array desired_joints = joint_traj_generator->get_desired_joints(); 51 | std::array goal_joints = joint_traj_generator->get_goal_joints(); 52 | 53 | for(size_t i = 0; i < goal_joints.size(); i++) { 54 | if(fabs(goal_joints[i] - desired_joints[i]) > joint_thresholds_[i]) { 55 | return false; 56 | } 57 | } 58 | 59 | done_ = true; 60 | } 61 | 62 | return done_; 63 | } 64 | 65 | -------------------------------------------------------------------------------- /franka-interface/src/termination_handler/noop_termination_handler.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 11/26/18. 3 | // 4 | 5 | #include "franka-interface/termination_handler/noop_termination_handler.h" 6 | 7 | bool NoopTerminationHandler::should_terminate(const franka::RobotState &robot_state, 8 | franka::Model *model, 9 | TrajectoryGenerator *trajectory_generator) { 10 | check_terminate_preempt(); 11 | check_terminate_virtual_wall_collisions(robot_state, model); 12 | check_terminate_joint_limits(robot_state); 13 | 14 | return false; 15 | } 16 | -------------------------------------------------------------------------------- /franka-interface/src/termination_handler/termination_handler.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 11/25/18. 3 | // 4 | 5 | #include "franka-interface/termination_handler/termination_handler.h" 6 | 7 | void TerminationHandler::check_terminate_preempt() { 8 | if (!done_) { 9 | done_ = run_loop_info_->get_skill_preempted(); 10 | } 11 | } 12 | 13 | bool TerminationHandler::has_terminated() { 14 | return done_; 15 | } 16 | 17 | bool TerminationHandler::has_terminated_by_virt_coll() { 18 | return terminated_by_virt_coll_; 19 | } 20 | 21 | void TerminationHandler::check_terminate_time(TrajectoryGenerator *trajectory_generator) { 22 | if (!done_) { 23 | done_ = (trajectory_generator->time_ > trajectory_generator->run_time_ + buffer_time_); 24 | } 25 | } 26 | 27 | void TerminationHandler::check_terminate_virtual_wall_collisions(const franka::RobotState &robot_state, franka::Model *robot_model) { 28 | if (!done_) { 29 | int n_frame = 0; 30 | // Check all joints that are not base or EE 31 | for (franka::Frame frame = franka::Frame::kJoint2; frame <= franka::Frame::kFlange; frame++) { 32 | std::array pose = robot_model->pose(frame, robot_state); 33 | Eigen::Vector3d pos(pose[12], pose[13], pose[14]); 34 | 35 | for (uint n_plane = 0; n_plane < planes_.size(); n_plane++) { 36 | double dist = planes_[n_plane].absDistance(pos); 37 | 38 | if (dist < dist_thresholds_[n_frame]) { 39 | std::cout << "Frame " << n_frame + 1 << "is in collision with wall" << n_plane << "with distance " << dist << std::endl; 40 | done_ = true; 41 | terminated_by_virt_coll_ = true; 42 | break; 43 | } 44 | } 45 | 46 | n_frame++; 47 | if (done_) { break; } 48 | } 49 | } 50 | } 51 | 52 | void TerminationHandler::check_terminate_joint_limits(const franka::RobotState &robot_state) { 53 | if (!done_) { 54 | 55 | std::array current_joints = robot_state.q; 56 | 57 | for(size_t i = 0; i < current_joints.size(); i++) { 58 | if(current_joints[i] < min_joint_limits_[i] || current_joints[i] > max_joint_limits_[i]) { 59 | std::cout << "Joint " << i << "is approaching joint limits (" << min_joint_limits_[i] << ", " << max_joint_limits_[i] << ") with position: " << current_joints[i] << std::endl; 60 | done_ = true; 61 | terminated_by_virt_coll_ = true; 62 | break; 63 | } 64 | } 65 | } 66 | } 67 | 68 | void TerminationHandler::parse_sensor_data(const franka::RobotState &robot_state) { 69 | SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readTerminationHandlerSensorMessage(should_terminate_msg_); 70 | if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) { 71 | done_ = should_terminate_msg_.should_terminate(); 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /franka-interface/src/termination_handler/time_termination_handler.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 11/26/18. 3 | // 4 | 5 | #include "franka-interface/termination_handler/time_termination_handler.h" 6 | 7 | void TimeTerminationHandler::parse_parameters() { 8 | 9 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 10 | 11 | bool parsed_params = time_termination_params_.ParseFromArray(params_ + 5, data_size); 12 | 13 | if(parsed_params){ 14 | buffer_time_ = time_termination_params_.buffer_time(); 15 | 16 | } else { 17 | std::cout << "Parsing TimeTerminationHandler params failed. Data size = " << data_size << std::endl; 18 | } 19 | } 20 | 21 | bool TimeTerminationHandler::should_terminate(const franka::RobotState &robot_state, 22 | franka::Model *model, 23 | TrajectoryGenerator *trajectory_generator) { 24 | check_terminate_virtual_wall_collisions(robot_state, model); 25 | check_terminate_joint_limits(robot_state); 26 | check_terminate_preempt(); 27 | check_terminate_time(trajectory_generator); 28 | 29 | return done_; 30 | } 31 | -------------------------------------------------------------------------------- /franka-interface/src/termination_handler_factory.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 12/18/18. 3 | // 4 | 5 | #include "franka-interface/termination_handler_factory.h" 6 | 7 | #include 8 | #include 9 | 10 | #include "franka-interface/termination_handler/contact_termination_handler.h" 11 | #include "franka-interface/termination_handler/final_joint_termination_handler.h" 12 | #include "franka-interface/termination_handler/final_pose_termination_handler.h" 13 | #include "franka-interface/termination_handler/noop_termination_handler.h" 14 | #include "franka-interface/termination_handler/time_termination_handler.h" 15 | 16 | TerminationHandler* TerminationHandlerFactory::getTerminationHandlerForSkill(SharedBufferTypePtr buffer, RunLoopProcessInfo *run_loop_info, SensorDataManager* sensor_data_manager) { 17 | TerminationHandlerType termination_handler_type = static_cast(buffer[0]); 18 | 19 | std::string termination_handler_type_name; 20 | 21 | TerminationHandler *termination_handler = nullptr; 22 | switch (termination_handler_type) { 23 | case TerminationHandlerType::ContactTerminationHandler: 24 | termination_handler_type_name = "ContactTerminationHandler"; 25 | termination_handler = new ContactTerminationHandler(buffer, run_loop_info, sensor_data_manager); 26 | break; 27 | case TerminationHandlerType::FinalJointTerminationHandler: 28 | termination_handler_type_name = "FinalJointTerminationHandler"; 29 | termination_handler = new FinalJointTerminationHandler(buffer, run_loop_info, sensor_data_manager); 30 | break; 31 | case TerminationHandlerType::FinalPoseTerminationHandler: 32 | termination_handler_type_name = "FinalPoseTerminationHandler"; 33 | termination_handler = new FinalPoseTerminationHandler(buffer, run_loop_info, sensor_data_manager); 34 | break; 35 | case TerminationHandlerType::NoopTerminationHandler: 36 | termination_handler_type_name = "NoopTerminationHandler"; 37 | termination_handler = new NoopTerminationHandler(buffer, run_loop_info, sensor_data_manager); 38 | break; 39 | case TerminationHandlerType::TimeTerminationHandler: 40 | termination_handler_type_name = "TimeTerminationHandler"; 41 | termination_handler = new TimeTerminationHandler(buffer, run_loop_info, sensor_data_manager); 42 | break; 43 | default: 44 | std::cout << "Cannot create Termination Handler with type: " << 45 | static_cast::type>(termination_handler_type) << 46 | "\n" ; 47 | return nullptr; 48 | } 49 | std::cout << "Termination Handler Type: " << termination_handler_type_name << std::endl; 50 | 51 | termination_handler->parse_parameters(); 52 | return termination_handler; 53 | } 54 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/cartesian_velocity_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | #include "franka-interface/trajectory_generator/cartesian_velocity_trajectory_generator.h" 2 | 3 | void CartesianVelocityTrajectoryGenerator::parse_parameters() { 4 | // First parameter is reserved for the type 5 | 6 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 7 | bool parsed_params = cartesian_velocity_trajectory_params_.ParseFromArray(params_ + 5, data_size); 8 | 9 | if(parsed_params){ 10 | run_time_ = cartesian_velocity_trajectory_params_.run_time(); 11 | for(int i = 0; i < 6; i++) { 12 | goal_cartesian_velocities_[i] = cartesian_velocity_trajectory_params_.cartesian_velocities(i); 13 | cartesian_accelerations_[i] = cartesian_velocity_trajectory_params_.cartesian_accelerations(i); 14 | } 15 | } else { 16 | std::cout << "Parsing CartesianVelocityTrajectoryGenerator params failed. Data size = " << data_size << std::endl; 17 | } 18 | } 19 | 20 | void CartesianVelocityTrajectoryGenerator::initialize_trajectory(const franka::RobotState &robot_state, 21 | SkillType skill_type) { 22 | initialize_initial_cartesian_velocities(robot_state, skill_type); 23 | } 24 | 25 | void CartesianVelocityTrajectoryGenerator::initialize_initial_cartesian_velocities(const franka::RobotState &robot_state, 26 | SkillType skill_type) { 27 | switch(skill_type) { 28 | case SkillType::CartesianVelocitySkill: 29 | initial_cartesian_velocities_ = robot_state.O_dP_EE_d; 30 | desired_cartesian_velocities_ = robot_state.O_dP_EE_d; 31 | break; 32 | default: 33 | initial_cartesian_velocities_ = robot_state.O_dP_EE_d; 34 | desired_cartesian_velocities_ = robot_state.O_dP_EE_d; 35 | } 36 | } 37 | 38 | const std::array& CartesianVelocityTrajectoryGenerator::get_desired_cartesian_velocities() const { 39 | return desired_cartesian_velocities_; 40 | } 41 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/cubic_hermite_spline_joint_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jacky on 3/25/20. 3 | // From https://www.rose-hulman.edu/~finn/CCLI/Notes/day09.pdf 4 | // 5 | 6 | #include "franka-interface/trajectory_generator/cubic_hermite_spline_joint_trajectory_generator.h" 7 | 8 | #include 9 | 10 | void CubicHermiteSplineJointTrajectoryGenerator::parse_sensor_data(const franka::RobotState &robot_state) { 11 | SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readTrajectoryGeneratorSensorMessage(joint_sensor_msg_); 12 | if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) { 13 | for (int i = 0; i < 7; i++) { 14 | goal_joints_[i] = joint_sensor_msg_.joints(i); 15 | goal_joint_velocities_[i] = joint_sensor_msg_.joint_vels(i); 16 | initial_joints_[i] = robot_state.q[i]; 17 | initial_joint_velocities_[i] = robot_state.dq[i]; 18 | } 19 | 20 | seg_run_time = joint_sensor_msg_.seg_run_time(); 21 | seg_start_time_ = time_; 22 | } 23 | } 24 | 25 | void CubicHermiteSplineJointTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 26 | t_ = std::min(std::max((time_ - seg_start_time_) / seg_run_time, 0.0), 1.0); 27 | 28 | double t2 = t_ * t_; 29 | double t3 = t2 * t_; 30 | 31 | double H0 = 1 - 3*t2 + 2*t3; 32 | double H1 = t_ - 2*t2 + t3; 33 | double H2 = -t2 + t3; 34 | double H3 = 3*t2 - 2*t3; 35 | 36 | for (size_t i = 0; i < desired_joints_.size(); i++) { 37 | desired_joints_[i] = 38 | H0 * initial_joints_[i] + 39 | H1 * initial_joint_velocities_[i] + 40 | H2 * goal_joint_velocities_[i] + 41 | H3 * goal_joints_[i]; 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/cubic_hermite_spline_pose_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jacky on 3/25/20. 3 | // From https://www.rose-hulman.edu/~finn/CCLI/Notes/day09.pdf 4 | // 5 | 6 | #include "franka-interface/trajectory_generator/cubic_hermite_spline_pose_trajectory_generator.h" 7 | #include 8 | 9 | void CubicHermiteSplinePoseTrajectoryGenerator::initialize_trajectory(const franka::RobotState &robot_state, 10 | SkillType skill_type) { 11 | initialize_initial_and_desired_poses(robot_state, skill_type); 12 | fix_goal_quaternion(); 13 | 14 | initial_euler_ = initial_orientation_.toRotationMatrix().eulerAngles(0, 1, 2); 15 | goal_euler_ = goal_orientation_.toRotationMatrix().eulerAngles(0, 1, 2); 16 | } 17 | 18 | void CubicHermiteSplinePoseTrajectoryGenerator::parse_sensor_data(const franka::RobotState &robot_state) { 19 | SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readTrajectoryGeneratorSensorMessage(pose_sensor_msg_); 20 | if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) { 21 | for (int i = 0; i < 3; i++) { 22 | goal_position_[i] = pose_sensor_msg_.position(i); 23 | } 24 | 25 | goal_orientation_.w() = pose_sensor_msg_.quaternion(0); 26 | goal_orientation_.x() = pose_sensor_msg_.quaternion(1); 27 | goal_orientation_.y() = pose_sensor_msg_.quaternion(2); 28 | goal_orientation_.z() = pose_sensor_msg_.quaternion(3); 29 | 30 | for (int i = 0; i < 6; i++) { 31 | initial_pose_velocities_[i] = robot_state.O_dP_EE_c[i]; 32 | goal_pose_velocities_[0] = pose_sensor_msg_.pose_velocities(i); 33 | } 34 | for (int i = 0; i < 16; i++) { 35 | initial_pose_[i] = robot_state.O_T_EE[i]; 36 | } 37 | Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_pose_.data())); 38 | initial_position_ = Eigen::Vector3d(initial_transform.translation()); 39 | initial_orientation_ = Eigen::Quaterniond(initial_transform.linear()); 40 | 41 | initial_euler_ = initial_orientation_.toRotationMatrix().eulerAngles(0, 1, 2); 42 | goal_euler_ = goal_orientation_.toRotationMatrix().eulerAngles(0, 1, 2); 43 | 44 | seg_run_time = pose_sensor_msg_.seg_run_time(); 45 | seg_start_time_ = time_; 46 | } 47 | } 48 | 49 | void CubicHermiteSplinePoseTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 50 | 51 | 52 | t_ = std::min(std::max((time_ - seg_start_time_) / seg_run_time, 0.0), 1.0); 53 | 54 | double t2 = t_ * t_; 55 | double t3 = t2 * t_; 56 | 57 | double H0 = 1 - 3*t2 + 2*t3; 58 | double H1 = t_ - 2*t2 + t3; 59 | double H2 = -t2 + t3; 60 | double H3 = 3*t2 - 2*t3; 61 | 62 | for (size_t i = 0; i < 3; i++) { 63 | desired_position_[i] = 64 | H0 * initial_position_[i] + 65 | H1 * initial_pose_velocities_[i] + 66 | H2 * goal_pose_velocities_[i] + 67 | H3 * goal_position_[i]; 68 | } 69 | 70 | for (size_t i = 0; i < 3; i++) { 71 | desired_euler_[i] = 72 | H0 * initial_euler_[i] + 73 | H1 * initial_pose_velocities_[i + 3] + 74 | H2 * goal_pose_velocities_[i + 3] + 75 | H3 * goal_euler_[i]; 76 | } 77 | desired_orientation_ = Eigen::AngleAxisd(desired_euler_[0], Eigen::Vector3d::UnitX()) 78 | * Eigen::AngleAxisd(desired_euler_[1], Eigen::Vector3d::UnitY()) 79 | * Eigen::AngleAxisd(desired_euler_[2], Eigen::Vector3d::UnitZ()); 80 | 81 | calculate_desired_pose(); 82 | 83 | } 84 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/gripper_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by iamlab on 12/1/18. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/gripper_trajectory_generator.h" 6 | 7 | #include 8 | 9 | void GripperTrajectoryGenerator::parse_parameters() { 10 | // First parameter is reserved for the type 11 | 12 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 13 | 14 | bool parsed_params = gripper_trajectory_params_.ParseFromArray(params_ + 5, data_size); 15 | 16 | if(parsed_params){ 17 | is_grasp_skill_ = gripper_trajectory_params_.grasp(); 18 | 19 | width_ = gripper_trajectory_params_.width(); 20 | speed_ = gripper_trajectory_params_.speed(); 21 | 22 | if(is_grasp_skill_){ 23 | force_ = gripper_trajectory_params_.force(); 24 | } 25 | } else { 26 | std::cout << "Parsing GripperTrajectoryGenerator params failed. Data size = " << data_size << std::endl; 27 | } 28 | } 29 | 30 | double GripperTrajectoryGenerator::getWidth() { 31 | return width_; 32 | } 33 | 34 | double GripperTrajectoryGenerator::getSpeed() { 35 | return speed_; 36 | } 37 | 38 | double GripperTrajectoryGenerator::getForce() { 39 | return force_; 40 | } 41 | 42 | bool GripperTrajectoryGenerator::isGraspSkill(){ 43 | return is_grasp_skill_; 44 | } 45 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/joint_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | #include "franka-interface/trajectory_generator/joint_trajectory_generator.h" 2 | 3 | void JointTrajectoryGenerator::parse_parameters() { 4 | // First parameter is reserved for the type 5 | 6 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 7 | bool parsed_params = joint_trajectory_params_.ParseFromArray(params_ + 5, data_size); 8 | 9 | if(parsed_params){ 10 | run_time_ = joint_trajectory_params_.run_time(); 11 | for(int i = 0; i < 7; i++) { 12 | goal_joints_[i] = joint_trajectory_params_.joints(i); 13 | } 14 | } else { 15 | std::cout << "Parsing JointTrajectoryGenerator params failed. Data size = " << data_size << std::endl; 16 | } 17 | } 18 | 19 | void JointTrajectoryGenerator::initialize_trajectory(const franka::RobotState &robot_state, 20 | SkillType skill_type) { 21 | initialize_initial_and_desired_joints(robot_state, skill_type); 22 | } 23 | 24 | void JointTrajectoryGenerator::initialize_initial_and_desired_joints(const franka::RobotState &robot_state, 25 | SkillType skill_type) { 26 | switch(skill_type) { 27 | case SkillType::JointPositionSkill: 28 | initial_joints_ = robot_state.q_d; 29 | desired_joints_ = robot_state.q_d; 30 | break; 31 | case SkillType::ImpedanceControlSkill: 32 | initial_joints_ = robot_state.q; 33 | desired_joints_ = robot_state.q; 34 | break; 35 | default: 36 | initial_joints_ = robot_state.q_d; 37 | desired_joints_ = robot_state.q_d; 38 | } 39 | } 40 | void JointTrajectoryGenerator::setGoalJoints(const std::array joints) { 41 | for (int i = 0; i < 7; i++) { 42 | goal_joints_[i] = static_cast(joints[i]); 43 | } 44 | } 45 | 46 | void JointTrajectoryGenerator::setInitialJoints(const std::array joints) { 47 | for (int i = 0; i < 7; i++) { 48 | initial_joints_[i] = static_cast(joints[i]); 49 | } 50 | } 51 | 52 | const std::array& JointTrajectoryGenerator::get_desired_joints() const { 53 | return desired_joints_; 54 | } 55 | 56 | const std::array& JointTrajectoryGenerator::get_goal_joints() const { 57 | return goal_joints_; 58 | } -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/joint_velocity_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | #include "franka-interface/trajectory_generator/joint_velocity_trajectory_generator.h" 2 | 3 | void JointVelocityTrajectoryGenerator::parse_parameters() { 4 | // First parameter is reserved for the type 5 | 6 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 7 | bool parsed_params = joint_velocity_trajectory_params_.ParseFromArray(params_ + 5, data_size); 8 | 9 | if(parsed_params){ 10 | run_time_ = joint_velocity_trajectory_params_.run_time(); 11 | for(int i = 0; i < 7; i++) { 12 | goal_joint_velocities_[i] = joint_velocity_trajectory_params_.joint_velocities(i); 13 | joint_accelerations_[i] = joint_velocity_trajectory_params_.joint_accelerations(i); 14 | } 15 | } else { 16 | std::cout << "Parsing JointVelocityTrajectoryGenerator params failed. Data size = " << data_size << std::endl; 17 | } 18 | } 19 | 20 | void JointVelocityTrajectoryGenerator::initialize_trajectory(const franka::RobotState &robot_state, 21 | SkillType skill_type) { 22 | initialize_initial_joint_velocities(robot_state, skill_type); 23 | } 24 | 25 | void JointVelocityTrajectoryGenerator::initialize_initial_joint_velocities(const franka::RobotState &robot_state, 26 | SkillType skill_type) { 27 | switch(skill_type) { 28 | case SkillType::JointVelocitySkill: 29 | initial_joint_velocities_ = robot_state.dq_d; 30 | desired_joint_velocities_ = robot_state.dq_d; 31 | break; 32 | default: 33 | initial_joint_velocities_ = robot_state.dq_d; 34 | desired_joint_velocities_ = robot_state.dq_d; 35 | } 36 | } 37 | 38 | const std::array& JointVelocityTrajectoryGenerator::get_desired_joint_velocities() const { 39 | return desired_joint_velocities_; 40 | } 41 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/linear_force_position_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jacky on 4/7/20. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/linear_force_position_trajectory_generator.h" 6 | #include 7 | 8 | void LinearForcePositionTrajectoryGenerator::parse_parameters() { 9 | // First parameter is reserved for the type 10 | 11 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 12 | 13 | bool parsed_params = run_time_params_.ParseFromArray(params_ + 5, data_size); 14 | 15 | if (parsed_params){ 16 | run_time_ = run_time_params_.run_time(); 17 | } else { 18 | std::cout << "Parsing PoseTrajectoryGenerator params failed. Data size = " << data_size << std::endl; 19 | } 20 | } 21 | 22 | void LinearForcePositionTrajectoryGenerator::initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type) { 23 | initial_pose_ = robot_state.O_T_EE; 24 | initial_transform_ = Eigen::Affine3d(Eigen::Matrix4d::Map(initial_pose_.data())); 25 | initial_position_ = Eigen::Vector3d(initial_transform_.translation()); 26 | initial_orientation_ = Eigen::Quaterniond(initial_transform_.linear()); 27 | 28 | goal_pose_ = robot_state.O_T_EE; 29 | goal_transform_ = Eigen::Affine3d(Eigen::Matrix4d::Map(goal_pose_.data())); 30 | goal_position_ = Eigen::Vector3d(goal_transform_.translation()); 31 | goal_orientation_ = Eigen::Quaterniond(goal_transform_.linear()); 32 | 33 | fix_goal_quaternion(); 34 | 35 | for (int i = 0; i < 6; i++) { 36 | desired_force_[i] = 0.; 37 | } 38 | 39 | seg_run_time_ = run_time_; 40 | seg_start_time_ = time_; 41 | } 42 | 43 | void LinearForcePositionTrajectoryGenerator::parse_sensor_data(const franka::RobotState &robot_state) { 44 | SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readTrajectoryGeneratorSensorMessage(force_position_sensor_msg_); 45 | if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) { 46 | initial_pose_ = robot_state.O_T_EE; 47 | initial_transform_ = Eigen::Affine3d(Eigen::Matrix4d::Map(initial_pose_.data())); 48 | initial_position_ = Eigen::Vector3d(initial_transform_.translation()); 49 | initial_orientation_ = Eigen::Quaterniond(initial_transform_.linear()); 50 | 51 | for (size_t i = 0; i < 16; i++) { 52 | goal_pose_[i] = force_position_sensor_msg_.pose(i); 53 | } 54 | goal_transform_ = Eigen::Affine3d(Eigen::Matrix4d::Map(goal_pose_.data())); 55 | goal_position_ = Eigen::Vector3d(goal_transform_.translation()); 56 | goal_orientation_ = Eigen::Quaterniond(goal_transform_.linear()); 57 | fix_goal_quaternion(); 58 | 59 | for (int i = 0; i < 6; i++) { 60 | desired_force_[i] = force_position_sensor_msg_.force(i); 61 | } 62 | 63 | seg_run_time_ = force_position_sensor_msg_.seg_run_time(); 64 | seg_start_time_ = time_; 65 | } 66 | } 67 | 68 | const std::array& LinearForcePositionTrajectoryGenerator::get_desired_pose() { 69 | double t = std::min(std::max((time_ - seg_start_time_) / seg_run_time_, 0.0), 1.0); 70 | desired_position_ = initial_position_ + (goal_position_ - initial_position_) * t; 71 | desired_orientation_ = initial_orientation_.slerp(t, goal_orientation_); 72 | calculate_desired_pose(); 73 | 74 | return desired_pose_; 75 | } 76 | 77 | const std::array& LinearForcePositionTrajectoryGenerator::get_desired_force() { 78 | return desired_force_; 79 | } -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/linear_joint_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 11/30/18. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/linear_joint_trajectory_generator.h" 6 | 7 | void LinearJointTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 8 | t_ = std::min(std::max(time_ / run_time_, 0.0), 1.0); 9 | 10 | for (size_t i = 0; i < desired_joints_.size(); i++) { 11 | desired_joints_[i] = initial_joints_[i] * (1 - t_) + goal_joints_[i] * t_; 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/linear_pose_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kevin on 11/29/18. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/linear_pose_trajectory_generator.h" 6 | 7 | void LinearPoseTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 8 | t_ = std::min(std::max(time_ / run_time_, 0.0), 1.0); 9 | 10 | desired_position_ = initial_position_ + (goal_position_ - initial_position_) * t_; 11 | desired_orientation_ = initial_orientation_.slerp(t_, goal_orientation_); 12 | 13 | calculate_desired_pose(); 14 | } 15 | 16 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/min_jerk_joint_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jacky on 2/11/19. 3 | // From https://mika-s.github.io/python/control-theory/trajectory-generation/2017/12/06/trajectory-generation-with-a-minimum-jerk-trajectory.html 4 | // 5 | 6 | #include "franka-interface/trajectory_generator/min_jerk_joint_trajectory_generator.h" 7 | 8 | #include 9 | 10 | void MinJerkJointTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 11 | t_ = std::min(std::max(time_ / run_time_, 0.0), 1.0); 12 | 13 | for (size_t i = 0; i < desired_joints_.size(); i++) { 14 | desired_joints_[i] = initial_joints_[i] + (goal_joints_[i] - initial_joints_[i]) * ( 15 | 10 * std::pow(t_, 3) - 15 * std::pow(t_, 4) + 6 * std::pow(t_, 5) 16 | ); 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/min_jerk_pose_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kevin on 3/25/19. 3 | // From https://mika-s.github.io/python/control-theory/trajectory-generation/2017/12/06/trajectory-generation-with-a-minimum-jerk-trajectory.html 4 | // 5 | 6 | #include "franka-interface/trajectory_generator/min_jerk_pose_trajectory_generator.h" 7 | 8 | void MinJerkPoseTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 9 | t_ = std::min(std::max(time_ / run_time_, 0.0), 1.0); 10 | slerp_t_ = (10 * std::pow(t_, 3) - 15 * std::pow(t_, 4) + 6 * std::pow(t_, 5)); 11 | 12 | for (int i = 0; i < desired_position_.size(); i++) { 13 | desired_position_[i] = initial_position_[i] + 14 | (goal_position_[i] - initial_position_[i]) * slerp_t_; 15 | } 16 | 17 | desired_orientation_ = initial_orientation_.slerp(slerp_t_, goal_orientation_); 18 | 19 | calculate_desired_pose(); 20 | } 21 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/pass_through_cartesian_velocity_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kevin on 11/1/22. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/pass_through_cartesian_velocity_trajectory_generator.h" 6 | 7 | #include 8 | 9 | void PassThroughCartesianVelocityTrajectoryGenerator::parse_sensor_data(const franka::RobotState &robot_state) { 10 | SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readTrajectoryGeneratorSensorMessage(cartesian_velocity_sensor_msg_); 11 | if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) { 12 | for (int i = 0; i < 6; i++) { 13 | desired_cartesian_velocities_[i] = cartesian_velocity_sensor_msg_.cartesian_velocities(i); 14 | } 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/pass_through_force_position_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jacky on 4/3/20. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/pass_through_force_position_trajectory_generator.h" 6 | #include 7 | 8 | void PassThroughForcePositionTrajectoryGenerator::parse_parameters() { 9 | // First parameter is reserved for the type 10 | 11 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 12 | 13 | bool parsed_params = run_time_params_.ParseFromArray(params_ + 5, data_size); 14 | 15 | if (parsed_params){ 16 | run_time_ = run_time_params_.run_time(); 17 | } else { 18 | std::cout << "Parsing PoseTrajectoryGenerator params failed. Data size = " << data_size << std::endl; 19 | } 20 | } 21 | 22 | void PassThroughForcePositionTrajectoryGenerator::parse_sensor_data(const franka::RobotState &robot_state) { 23 | SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readTrajectoryGeneratorSensorMessage(force_position_sensor_msg_); 24 | if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) { 25 | 26 | for (size_t i = 0; i < 16; i++) { 27 | desired_pose_[i] = force_position_sensor_msg_.pose(i); 28 | } 29 | for (int i = 0; i < 6; i++) { 30 | desired_force_[i] = force_position_sensor_msg_.force(i); 31 | } 32 | } 33 | } 34 | 35 | void PassThroughForcePositionTrajectoryGenerator::initialize_trajectory(const franka::RobotState &robot_state, SkillType skill_type) { 36 | for (int i = 0; i < 16; i++) { 37 | desired_pose_[i] = robot_state.O_T_EE[i]; 38 | } 39 | for (int i = 0; i < 6; i++) { 40 | desired_force_[i] = 0.; 41 | } 42 | } 43 | 44 | const std::array& PassThroughForcePositionTrajectoryGenerator::get_desired_pose() { 45 | return desired_pose_; 46 | } 47 | 48 | const std::array& PassThroughForcePositionTrajectoryGenerator::get_desired_force() { 49 | return desired_force_; 50 | } -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/pass_through_joint_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jacky on 4/1/20. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/pass_through_joint_trajectory_generator.h" 6 | 7 | #include 8 | 9 | void PassThroughJointTrajectoryGenerator::parse_sensor_data(const franka::RobotState &robot_state) { 10 | SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readTrajectoryGeneratorSensorMessage(joint_sensor_msg_); 11 | if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) { 12 | for (int i = 0; i < 7; i++) { 13 | desired_joints_[i] = joint_sensor_msg_.joints(i); 14 | } 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/pass_through_joint_velocity_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kevin on 11/1/22. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/pass_through_joint_velocity_trajectory_generator.h" 6 | 7 | #include 8 | 9 | void PassThroughJointVelocityTrajectoryGenerator::parse_sensor_data(const franka::RobotState &robot_state) { 10 | SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readTrajectoryGeneratorSensorMessage(joint_velocity_sensor_msg_); 11 | if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) { 12 | for (int i = 0; i < 7; i++) { 13 | desired_joint_velocities_[i] = joint_velocity_sensor_msg_.joint_velocities(i); 14 | } 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/pass_through_pose_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jacky on 4/1/20. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/pass_through_pose_trajectory_generator.h" 6 | #include 7 | 8 | void PassThroughPoseTrajectoryGenerator::parse_sensor_data(const franka::RobotState &robot_state) { 9 | SensorDataManagerReadStatus sensor_msg_status = sensor_data_manager_->readTrajectoryGeneratorSensorMessage(pose_sensor_msg_); 10 | if (sensor_msg_status == SensorDataManagerReadStatus::SUCCESS) { 11 | for (int i = 0; i < 3; i++) { 12 | desired_position_[i] = pose_sensor_msg_.position(i); 13 | } 14 | 15 | desired_orientation_.w() = pose_sensor_msg_.quaternion(0); 16 | desired_orientation_.x() = pose_sensor_msg_.quaternion(1); 17 | desired_orientation_.y() = pose_sensor_msg_.quaternion(2); 18 | desired_orientation_.z() = pose_sensor_msg_.quaternion(3); 19 | } 20 | } 21 | 22 | void PassThroughPoseTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 23 | calculate_desired_pose(); 24 | } 25 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/relative_linear_pose_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kevin on 11/29/18. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/relative_linear_pose_trajectory_generator.h" 6 | 7 | void RelativeLinearPoseTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 8 | t_ = std::min(std::max(time_ / run_time_, 0.0), 1.0); 9 | 10 | desired_position_ = initial_position_ + (goal_position_ - initial_position_) * t_; 11 | desired_orientation_ = initial_orientation_.slerp(t_, goal_orientation_); 12 | 13 | calculate_desired_pose(); 14 | } 15 | 16 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/relative_min_jerk_pose_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kevin on 3/25/19. 3 | // From https://mika-s.github.io/python/control-theory/trajectory-generation/2017/12/06/trajectory-generation-with-a-minimum-jerk-trajectory.html 4 | // 5 | 6 | #include "franka-interface/trajectory_generator/relative_min_jerk_pose_trajectory_generator.h" 7 | 8 | void RelativeMinJerkPoseTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 9 | t_ = std::min(std::max(time_ / run_time_, 0.0), 1.0); 10 | slerp_t_ = (10 * std::pow(t_, 3) - 15 * std::pow(t_, 4) + 6 * std::pow(t_, 5)); 11 | 12 | for (int i = 0; i < desired_position_.size(); i++) { 13 | desired_position_[i] = initial_position_[i] + 14 | (goal_position_[i] - initial_position_[i]) * slerp_t_; 15 | } 16 | 17 | desired_orientation_ = initial_orientation_.slerp(slerp_t_, goal_orientation_); 18 | 19 | calculate_desired_pose(); 20 | } 21 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/relative_pose_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | #include "franka-interface/trajectory_generator/relative_pose_trajectory_generator.h" 2 | 3 | #include 4 | 5 | void RelativePoseTrajectoryGenerator::parse_parameters() { 6 | // First parameter is reserved for the type 7 | 8 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 9 | 10 | bool parsed_params = pose_trajectory_params_.ParseFromArray(params_ + 5, data_size); 11 | 12 | if(parsed_params){ 13 | run_time_ = pose_trajectory_params_.run_time(); 14 | 15 | if(pose_trajectory_params_.pose_size() == 16){ 16 | for(size_t i = 0; i < relative_pose_.size(); i++) { 17 | relative_pose_[i] = pose_trajectory_params_.pose(i); 18 | } 19 | 20 | Eigen::Affine3d goal_transform(Eigen::Matrix4d::Map(relative_pose_.data())); 21 | relative_position_ = Eigen::Vector3d(goal_transform.translation()); 22 | relative_orientation_ = Eigen::Quaterniond(goal_transform.linear()); 23 | } else { 24 | 25 | for(int i = 0; i < 3; i++) { 26 | relative_position_[i] = pose_trajectory_params_.position(i); 27 | } 28 | 29 | relative_orientation_.w() = pose_trajectory_params_.quaternion(0); 30 | relative_orientation_.x() = pose_trajectory_params_.quaternion(1); 31 | relative_orientation_.y() = pose_trajectory_params_.quaternion(2); 32 | relative_orientation_.z() = pose_trajectory_params_.quaternion(3); 33 | } 34 | } else { 35 | std::cout << "Parsing RelativePoseTrajectoryGenerator params failed. Data size = " << data_size << std::endl; 36 | } 37 | } 38 | 39 | void RelativePoseTrajectoryGenerator::initialize_trajectory(const franka::RobotState &robot_state, 40 | SkillType skill_type) { 41 | initialize_initial_and_desired_poses(robot_state, skill_type); 42 | goal_position_ = initial_position_ + relative_position_; 43 | goal_orientation_ = initial_orientation_ * relative_orientation_; 44 | 45 | fix_goal_quaternion(); 46 | } -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/sine_joint_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by kevin on 3/26/19. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/sine_joint_trajectory_generator.h" 6 | 7 | #include 8 | 9 | void SineJointTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 10 | t_ = std::min(std::max(time_ / run_time_, 0.0), 1.0); 11 | sine_t_ = ((std::sin((t_ * M_PI) - (M_PI / 2)) + 1) / 2); 12 | 13 | for (size_t i = 0; i < desired_joints_.size(); i++) { 14 | desired_joints_[i] = initial_joints_[i] + (goal_joints_[i] - initial_joints_[i]) * sine_t_; 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/sine_pose_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kevin on 11/29/18. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/sine_pose_trajectory_generator.h" 6 | 7 | void SinePoseTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 8 | t_ = std::min(std::max(time_ / run_time_, 0.0), 1.0); 9 | sine_t_ = ((std::sin((t_ * M_PI) - (M_PI / 2)) + 1) / 2); 10 | 11 | desired_position_ = initial_position_ + (goal_position_ - initial_position_) * sine_t_; 12 | desired_orientation_ = initial_orientation_.slerp(sine_t_, goal_orientation_); 13 | 14 | calculate_desired_pose(); 15 | } 16 | 17 | -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/stay_in_initial_joints_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kevin on 4/1/19. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/stay_in_initial_joints_trajectory_generator.h" 6 | 7 | #include 8 | 9 | void StayInInitialJointsTrajectoryGenerator::parse_parameters() { 10 | // First parameter is reserved for the type 11 | 12 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 13 | 14 | bool parsed_params = run_time_msg_.ParseFromArray(params_ + 5, data_size); 15 | 16 | if(parsed_params){ 17 | run_time_ = run_time_msg_.run_time(); 18 | 19 | } else { 20 | std::cout << "Parsing StayInInitialJointsTrajectoryGenerator params failed. Data size = " << data_size << std::endl; 21 | } 22 | } 23 | 24 | void StayInInitialJointsTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 25 | desired_joints_ = initial_joints_; 26 | } -------------------------------------------------------------------------------- /franka-interface/src/trajectory_generator/stay_in_initial_pose_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Kevin on 11/29/18. 3 | // 4 | 5 | #include "franka-interface/trajectory_generator/stay_in_initial_pose_trajectory_generator.h" 6 | 7 | #include 8 | 9 | void StayInInitialPoseTrajectoryGenerator::parse_parameters() { 10 | // First parameter is reserved for the type 11 | 12 | int data_size = (params_[1] + (params_[2] << 8) + (params_[3] << 16) + (params_[4] << 24)); 13 | 14 | bool parsed_params = run_time_msg_.ParseFromArray(params_ + 5, data_size); 15 | 16 | if(parsed_params){ 17 | run_time_ = run_time_msg_.run_time(); 18 | 19 | } else { 20 | std::cout << "Parsing StayInInitialPoseTrajectoryGenerator params failed. Data size = " << data_size << std::endl; 21 | } 22 | } 23 | 24 | void StayInInitialPoseTrajectoryGenerator::get_next_step(const franka::RobotState &robot_state) { 25 | desired_position_ = initial_position_; 26 | desired_orientation_ = initial_orientation_; 27 | } -------------------------------------------------------------------------------- /franka-interface/src/utils/logger_utils.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by mohit on 3/23/19. 3 | // 4 | 5 | #include "franka-interface/utils/logger_utils.h" 6 | 7 | #include 8 | 9 | 10 | std::vector LoggerUtils::all_logger_files(std::string logdir) { 11 | std::string prefix = "robot_state_data_"; 12 | std::vector all_log_files; 13 | for (boost::filesystem::directory_entry& f : boost::filesystem::directory_iterator(logdir)) { 14 | std::string filename = f.path().filename().string(); 15 | if (filename.find(prefix) != std::string::npos) { 16 | all_log_files.push_back(filename); 17 | } 18 | } 19 | return all_log_files; 20 | } 21 | 22 | int LoggerUtils::integer_suffix_for_new_log_file(std::string logdir) { 23 | std::string prefix = "robot_state_data_"; 24 | int curr_suffix = 0; 25 | for (boost::filesystem::directory_entry& f : boost::filesystem::directory_iterator(logdir)) { 26 | std::string filename = f.path().filename().string(); 27 | if (filename.find(prefix) != std::string::npos) { 28 | int int_suffix = std::stoi(filename.substr(prefix.length(), filename.find(".txt"))); 29 | if (int_suffix >= curr_suffix) { 30 | curr_suffix = int_suffix + 1; 31 | } 32 | } 33 | } 34 | return curr_suffix; 35 | } 36 | -------------------------------------------------------------------------------- /old_docs/franka_control_pc_setup_guide.md: -------------------------------------------------------------------------------- 1 | # Franka Control PC Setup Guide 2 | 3 | This guide describes how to setup the software for the computer to specifically control the Franka Robot Arm. It assumes that the [control pc ubuntu setup guide](./control_pc_ubuntu_setup_guide.md) was successfully completed. 4 | 5 | ## Setting Up the Robot 6 | If this is the first time you are using the robot, please follow the Franka Robot Setup instructions located in the booklet that accompanies the robot and set the username and password for the robot. 7 | 8 | ## Editing the Ethernet Network Configuration 9 | 1. Insert the ethernet cable from the Franka Control box to the Control PC. 10 | 2. Turn on the Franka Control box. 11 | 3. Go to Edit Connections in the Ubuntu Network Connections Menu. 12 | 4. Select the Ethernet connection that corresponds to the port that you plugged the ethernet cable into and then click edit. 13 | 5. Go to the IPv4 Settings Tab and switch from Automatic (DHCP) to Manual. 14 | 6. Add an address of 172.16.0.1 with netmask 24 and then click save. 15 | 7. Check to see if you can ping 172.16.0.2 from any terminal. 16 | 8. If any issues arise, refer to here: https://frankaemika.github.io/docs/troubleshooting.html#robot-is-not-reachable 17 | 18 | ## Entering Franka Desk 19 | 1. Go to the web browser and type in https://172.16.0.2/ 20 | 2. Add an exception if there is a security certificate error. 21 | 3. Login to the robot with the correct credentials or complete setup with the robot. 22 | You should now be able to see the Franka desk GUI in the web browser. 23 | 4. If the username and password do not work, there is a method of factory resetting the Franka located here: https://www.franka-community.de/t/reset-admin-password-or-reset-to-factory-settings/184 24 | 25 | ## Using the Franka With Desk 26 | 1. Unlock the physical user stop. 27 | 2. Open the Robot brakes near the bottom of the Franka Desk GUI 28 | 3. (Optional) Program the Franka Robot in Desk by creating a new program and clicking and dragging blocks and moving the robot by pressing on the two buttons near the Franka’s end effectors. 29 | 30 | ## Franka ROS Installation 31 | 1. Run the following terminal command to install the Franka ROS packages on Ubuntu 18.04: `sudo apt install ros-melodic-libfranka ros-melodic-franka-ros` 32 | 33 | Now you are ready to return back to the main [README.md](../README.md) to continue the installation. 34 | -------------------------------------------------------------------------------- /src/franka_interface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace po = boost::program_options; 10 | 11 | int main(int argc, char *argv[]) { 12 | 13 | try { 14 | bool stop_franka_interface_on_error; 15 | bool reset_skill_numbering_on_error; 16 | bool use_new_filestream_on_error; 17 | bool with_gripper; 18 | bool log; 19 | std::string logdir; 20 | std::string robot_ip; 21 | po::options_description desc("Allowed options"); 22 | desc.add_options() 23 | ("help", "Produce help message") 24 | ("robot_ip,ip_addr,ip", po::value(&robot_ip)->default_value("172.16.0.2"), 25 | "Robot's ip address") 26 | ("stop_on_error", po::value(&stop_franka_interface_on_error)->default_value(false), 27 | "Stop robo-lib on error, i.e. any exception thrown by libfranka.") 28 | ("reset_skill_numbering_on_error", po::value(&reset_skill_numbering_on_error)->default_value(false), 29 | "Reset skill numbering on error, i.e. any exception thrown by libfranka.") 30 | ("use_new_filestream_on_error", po::value(&use_new_filestream_on_error)->default_value(false), 31 | "Use a new filestream on error, i.e. any exception thrown by libfranka.") 32 | ("log", po::value(&log)->default_value(false), "Log at 1kHz in Franka-Interface") 33 | ("logdir", po::value(&logdir)->default_value("logs"), "Directory to save robot_state_data") 34 | ("with_gripper", po::value(&with_gripper)->default_value(true), "Robot has gripper attached") 35 | ; 36 | 37 | po::positional_options_description p; 38 | p.add("robot_ip", -1); 39 | 40 | po::variables_map vm; 41 | po::store(po::command_line_parser(argc, argv). 42 | options(desc).positional(p).run(), vm); 43 | po::notify(vm); 44 | 45 | if (vm.count("help")) { 46 | std::cout << "Usage: options_description [options]\n"; 47 | std::cout << desc; 48 | return 0; 49 | } 50 | 51 | std::cout << "IAM FrankaInterface\n"; 52 | std::mutex m; 53 | std::mutex robot_loop_data_mutex; 54 | run_loop rl = run_loop(std::ref(m), std::ref(robot_loop_data_mutex), robot_ip, 55 | stop_franka_interface_on_error, reset_skill_numbering_on_error, use_new_filestream_on_error, log, logdir, with_gripper); 56 | std::cout << "Will start run loop.\n"; 57 | 58 | rl.start(); 59 | std::cout << "Did start run loop.\n"; 60 | std::cout << "Will run..\n"; 61 | rl.run_on_franka(); 62 | 63 | } 64 | catch(std::exception& e) { 65 | std::cout << e.what() << "\n"; 66 | return 1; 67 | } 68 | 69 | return 0; 70 | } 71 | --------------------------------------------------------------------------------