├── config ├── gripper_settings.yaml ├── robot_settings.yaml ├── control_gains.yaml └── franka_setup.sh ├── launch ├── gravity_mode.launch ├── move_to_joint_config.launch ├── command_mode.launch ├── joint_pos_goal_publisher.launch ├── monitor_mode.launch └── tracking_controller_node.launch ├── include ├── joint_position_controller.h ├── joint_velocity_controller.h ├── franka_controller.h ├── examples_common.h └── tracking_controller.h ├── LICENSE ├── src ├── move_to_joint_config.cpp ├── tracking_controller_node.cpp ├── joint_position_control_node.cpp ├── joint_velocity_controller_node.cpp ├── joint_goal_publisher.py ├── joint_position_controller.cpp ├── joint_velocity_controller.cpp ├── tracking_controller.cpp ├── examples_common.cpp └── franka_controller.cpp ├── README.md ├── package.xml ├── .gitignore └── CMakeLists.txt /config/gripper_settings.yaml: -------------------------------------------------------------------------------- 1 | joint_names: 2 | - panda_finger_joint1 3 | - panda_finger_joint2 4 | -------------------------------------------------------------------------------- /config/robot_settings.yaml: -------------------------------------------------------------------------------- 1 | joint_names: 2 | - panda_joint1 3 | - panda_joint2 4 | - panda_joint3 5 | - panda_joint4 6 | - panda_joint5 7 | - panda_joint6 8 | - panda_joint7 9 | -------------------------------------------------------------------------------- /config/control_gains.yaml: -------------------------------------------------------------------------------- 1 | # Kp: [650.0, 650.0, 650.0, 650.0, 100.0, 100.0, 50.0] 2 | # Kp: [350.0, 350.0, 350.0, 350.0, 100.0, 100.0, 50.0] 3 | # "joint_stiffness: [200.0, 200.0, 150.0, 150.0, 4 | # 100.0, 90.0, 50.0]" 5 | # Kp: [250.0, 250.0, 150.0, 150.0, 100.0, 90.0, 50.0] 6 | # Kd: [30.0, 30.0, 30.0, 30.0, 5.0, 5.0, 2.0] 7 | Kp: [100.0, 100.0, 100.0, 100.0, 75.0, 40.0, 30.0] 8 | Kd: [20.0, 20.0, 20.0, 20.0, 7.5, 5.0, 3.0] 9 | Km: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 10 | alpha_q: 0.8 11 | alpha_dq: 0.1 12 | -------------------------------------------------------------------------------- /config/franka_setup.sh: -------------------------------------------------------------------------------- 1 | export robot_ip="172.16.0.2" 2 | #### Home configs #### 3 | HOME_CONFIG_1="[0.0, -0.7853, 0.0, -2.3561, 0.0, 1.5707, 0.7853]" 4 | HOME_CONFIG_2="[-0.207, -0.494, -0.398, -2.239, 1.289, 1.194, 0.462]" 5 | HOME_CONFIG_3="[1.745, 0.914, -2.254, -2.306, 0.784, 1.617, -0.109]" 6 | HOME_CONFIG_4="[-0.9, -0.9, 1.2, -1.9, -0.2, 1.57, 0.78]" 7 | 8 | 9 | export home_1=${HOME_CONFIG_1} 10 | export home_2=${HOME_CONFIG_2} 11 | export home_3=${HOME_CONFIG_3} 12 | export home_4=${HOME_CONFIG_4} 13 | 14 | 15 | #######Aliases for commands######## 16 | -------------------------------------------------------------------------------- /launch/gravity_mode.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/move_to_joint_config.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | $(arg goal_config) 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/command_mode.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/joint_pos_goal_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | --> 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /launch/monitor_mode.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /include/joint_position_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "franka_controller.h" 16 | 17 | 18 | class JointPositionController : public FrankaController 19 | { 20 | public: 21 | JointPositionController(ros::NodeHandle* nh, ros::NodeHandle* pnh, std::string robot_ip); 22 | void command_loop(); 23 | // void initialize_control_gains(); 24 | 25 | 26 | private: 27 | franka::JointPositions joint_position_controller_callback(const franka::RobotState& robot_state, franka::Duration period); 28 | 29 | // Vector7d tau_d_error_, tau_d_coriolis_, tau_d_inertia_, tau_d_calculated_; 30 | // std::array tau_d_calculated_arr_; 31 | 32 | double alpha_q_, alpha_dq_; 33 | Vector7d Km_, Kp_, Kd_; 34 | // bool gains_set_; 35 | }; 36 | 37 | -------------------------------------------------------------------------------- /include/joint_velocity_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "franka_controller.h" 16 | 17 | 18 | class JointVelocityController : public FrankaController 19 | { 20 | public: 21 | JointVelocityController(ros::NodeHandle* nh, ros::NodeHandle* pnh, std::string robot_ip); 22 | void command_loop(); 23 | // void initialize_control_gains(); 24 | 25 | 26 | private: 27 | franka::JointVelocities joint_velocity_controller_callback(const franka::RobotState& robot_state, franka::Duration period); 28 | 29 | // Vector7d tau_d_error_, tau_d_coriolis_, tau_d_inertia_, tau_d_calculated_; 30 | // std::array tau_d_calculated_arr_; 31 | 32 | double alpha_q_, alpha_dq_; 33 | Vector7d Km_, Kp_, Kd_; 34 | // bool gains_set_; 35 | }; 36 | 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Mohak Bhardwaj 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 | -------------------------------------------------------------------------------- /src/move_to_joint_config.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "examples_common.h" 13 | 14 | 15 | int main(int argc, char** argv) { 16 | ros::init(argc, argv, "move_to_joint_config"); 17 | ros::NodeHandle nh("~"); 18 | 19 | std::string robot_ip; 20 | nh.getParam("robot_ip", robot_ip); 21 | std::vector goal_config; 22 | nh.getParam("goal_config", goal_config); 23 | double speed_factor; 24 | nh.getParam("speed_factor", speed_factor); 25 | 26 | try { 27 | franka::Robot robot(robot_ip); 28 | setDefaultBehavior(robot); 29 | std::array goal_config_arr; 30 | std::copy(goal_config.begin(), goal_config.begin()+7, goal_config_arr.begin()); 31 | 32 | MotionGenerator motion_generator(speed_factor, goal_config_arr); 33 | std::cout << "WARNING: This example will move the robot! " 34 | << "Please make sure to have the user stop button at hand!" << std::endl 35 | << "Press Enter to continue..." << std::endl; 36 | std::cin.ignore(); 37 | robot.control(motion_generator); 38 | std::cout << "Finished moving to joint configuration. Exiting..." << std::endl; 39 | 40 | } catch (const franka::Exception& e) { 41 | std::cout << e.what() << std::endl; 42 | return -1; 43 | } 44 | 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /src/tracking_controller_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "tracking_controller.h" 14 | 15 | int main(int argc, char** argv) 16 | { 17 | ros::init(argc, argv, "FrankaTrackingController"); 18 | 19 | ros::NodeHandle n; 20 | ros::NodeHandle nh("~"); 21 | 22 | std::string robot_ip; 23 | n.getParam("robot_ip", robot_ip); 24 | ROS_INFO("robot_ip: %s", robot_ip.c_str()); 25 | 26 | std::string mode; 27 | n.getParam("mode", mode); 28 | ROS_INFO("Mode: %s", mode.c_str()); 29 | 30 | try { 31 | TrackingController controller(&n, &nh, robot_ip); 32 | 33 | if (mode == "monitor"){ 34 | //read robot state and publish 35 | controller.monitor_loop(); 36 | } 37 | else if (mode == "gravity"){ 38 | //puts arm in gravity compensation mode 39 | controller.gravity_command_loop(); 40 | } 41 | else if (mode == "command"){ 42 | //control the robot using published joint commands 43 | controller.initialize_control_gains(); 44 | ROS_INFO("WARNING: This example will move the robot! \n" 45 | "Please make sure to have the user stop button at hand! \n" 46 | "Press Enter to continue... \n"); 47 | std::cin.ignore(); 48 | controller.command_loop(); 49 | } 50 | else{ 51 | throw std::runtime_error("Invalid operation mode"); 52 | } 53 | 54 | 55 | } catch (const franka::Exception& e) { 56 | std::cout << e.what() << std::endl; 57 | return -1; 58 | } 59 | 60 | return 0; 61 | } -------------------------------------------------------------------------------- /src/joint_position_control_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "joint_position_controller.h" 14 | 15 | int main(int argc, char** argv) 16 | { 17 | ros::init(argc, argv, "FrankaJointPositionController"); 18 | 19 | ros::NodeHandle n; 20 | ros::NodeHandle nh("~"); 21 | 22 | std::string robot_ip; 23 | n.getParam("robot_ip", robot_ip); 24 | ROS_INFO("robot_ip: %s", robot_ip.c_str()); 25 | 26 | std::string mode; 27 | n.getParam("mode", mode); 28 | ROS_INFO("Mode: %s", mode.c_str()); 29 | 30 | try { 31 | JointPositionController controller(&n, &nh, robot_ip); 32 | 33 | if (mode == "monitor"){ 34 | //read robot state and publish 35 | controller.monitor_loop(); 36 | } 37 | else if (mode == "gravity"){ 38 | //puts arm in gravity compensation mode 39 | controller.gravity_command_loop(); 40 | } 41 | else if (mode == "command"){ 42 | //control the robot using published joint commands 43 | // controller.initialize_control_gains(); 44 | ROS_INFO("WARNING: This example will move the robot! \n" 45 | "Please make sure to have the user stop button at hand! \n" 46 | "Press Enter to continue... \n"); 47 | std::cin.ignore(); 48 | controller.command_loop(); 49 | } 50 | else{ 51 | throw std::runtime_error("Invalid operation mode"); 52 | } 53 | 54 | 55 | } catch (const franka::Exception& e) { 56 | std::cout << e.what() << std::endl; 57 | return -1; 58 | } 59 | 60 | return 0; 61 | } -------------------------------------------------------------------------------- /src/joint_velocity_controller_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "joint_velocity_controller.h" 14 | 15 | int main(int argc, char** argv) 16 | { 17 | ros::init(argc, argv, "FrankaJointVelocityController"); 18 | 19 | ros::NodeHandle n; 20 | ros::NodeHandle nh("~"); 21 | 22 | std::string robot_ip; 23 | n.getParam("robot_ip", robot_ip); 24 | ROS_INFO("robot_ip: %s", robot_ip.c_str()); 25 | 26 | std::string mode; 27 | n.getParam("mode", mode); 28 | ROS_INFO("Mode: %s", mode.c_str()); 29 | 30 | try { 31 | JointVelocityController controller(&n, &nh, robot_ip); 32 | 33 | if (mode == "monitor"){ 34 | //read robot state and publish 35 | controller.monitor_loop(); 36 | } 37 | else if (mode == "gravity"){ 38 | //puts arm in gravity compensation mode 39 | controller.gravity_command_loop(); 40 | } 41 | else if (mode == "command"){ 42 | //control the robot using published joint commands 43 | // controller.initialize_control_gains(); 44 | ROS_INFO("WARNING: This example will move the robot! \n" 45 | "Please make sure to have the user stop button at hand! \n" 46 | "Press Enter to continue... \n"); 47 | std::cin.ignore(); 48 | controller.command_loop(); 49 | } 50 | else{ 51 | throw std::runtime_error("Invalid operation mode"); 52 | } 53 | 54 | 55 | } catch (const franka::Exception& e) { 56 | std::cout << e.what() << std::endl; 57 | return -1; 58 | } 59 | 60 | return 0; 61 | } -------------------------------------------------------------------------------- /launch/tracking_controller_node.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 | [franka_motion_control/joint_states, franka_gripper/joint_states] 28 | [franka_motion_control/joint_states] 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /include/franka_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // #include 4 | // #include 5 | // #include 6 | // #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | 22 | 23 | 24 | 25 | class FrankaController 26 | { 27 | public: 28 | FrankaController(ros::NodeHandle* nh, ros::NodeHandle* pnh, std::string robot_ip); 29 | void setDefaultBehavior(franka::Robot& robot); 30 | void monitor_loop(); 31 | virtual void command_loop()=0; 32 | void gravity_command_loop(); 33 | 34 | protected: 35 | using Vector7d = Eigen::Matrix; 36 | using Mat7d = Eigen::Matrix; 37 | using Vector7i = Eigen::Matrix; 38 | using Vector7f = Eigen::Matrix; 39 | using Mat7f = Eigen::Matrix; 40 | 41 | 42 | std::string robot_ip_; 43 | std::vector joint_names_; 44 | std::string prefix_; 45 | std::string joint_states_topic_; 46 | std::string joint_command_topic_; 47 | std::string robot_joint_command_topic_; 48 | 49 | ros::NodeHandle nh_, pnh_; 50 | ros::Subscriber goal_subscriber_; 51 | ros::Publisher state_publisher_, command_publisher_; 52 | 53 | franka::Robot robot_; 54 | franka::Model model_; 55 | 56 | sensor_msgs::JointState curr_robot_state_, curr_joint_command_, curr_robot_joint_command_; 57 | // franka::RobotState curr_robot_state_; 58 | 59 | Vector7d curr_q_des_, curr_dq_des_, curr_ddq_des_; 60 | Vector7d curr_q_, curr_dq_; 61 | Vector7d delta_q_; 62 | Vector7d curr_q_bel_, curr_dq_bel_, prev_q_bel_; //filtered belief over state 63 | Vector7d q_des_cmd_, dq_des_cmd_, ddq_des_cmd_; 64 | std::array tau_z_; //zero torque for gravity mode 65 | 66 | double time_ = 0.0; 67 | bool command_pub_started_; 68 | double prev_time_ = 0.0; 69 | 70 | void initializeSubscribers(); 71 | void initializePublishers(); 72 | void jointCommandCallback(const sensor_msgs::JointState& msg); 73 | bool publishRobotState(const franka::RobotState& robot_state); 74 | bool publishRobotState(const Vector7d& q, const Vector7d& dq); 75 | bool publishRobotCommand(const franka::RobotState& robot_command); 76 | 77 | }; 78 | 79 | -------------------------------------------------------------------------------- /include/examples_common.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017 Franka Emika GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | /** 15 | * @file examples_common.h 16 | * Contains common types and functions for the examples. 17 | */ 18 | 19 | /** 20 | * Sets a default collision behavior, joint impedance, Cartesian impedance, and filter frequency. 21 | * 22 | * @param[in] robot Robot instance to set behavior on. 23 | */ 24 | void setDefaultBehavior(franka::Robot& robot); 25 | 26 | /** 27 | * An example showing how to generate a joint pose motion to a goal position. Adapted from: 28 | * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots 29 | * (Kogan Page Science Paper edition). 30 | */ 31 | class MotionGenerator { 32 | public: 33 | /** 34 | * Creates a new MotionGenerator instance for a target q. 35 | * 36 | * @param[in] speed_factor General speed factor in range [0, 1]. 37 | * @param[in] q_goal Target joint positions. 38 | */ 39 | MotionGenerator(double speed_factor, const std::array q_goal); 40 | // MotionGenerator(double speed_factor, std::array q_goal); 41 | // MotionGenerator(double speed_factor); 42 | 43 | 44 | /** 45 | * Sends joint position calculations 46 | * 47 | * @param[in] robot_state Current state of the robot. 48 | * @param[in] period Duration of execution. 49 | * 50 | * @return Joint positions for use inside a control loop. 51 | */ 52 | franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period); 53 | 54 | void UpdateGoal(const std::array& q_goal); 55 | 56 | private: 57 | using Vector7d = Eigen::Matrix; 58 | using Vector7i = Eigen::Matrix; 59 | 60 | bool calculateDesiredValues(double t, Vector7d* delta_q_d) const; 61 | void calculateSynchronizedValues(); 62 | 63 | static constexpr double kDeltaQMotionFinished = 1e-5; 64 | // const Vector7d q_goal_; 65 | Vector7d q_goal_; 66 | Vector7d q_start_; 67 | Vector7d delta_q_; 68 | 69 | Vector7d dq_max_sync_; 70 | Vector7d t_1_sync_; 71 | Vector7d t_2_sync_; 72 | Vector7d t_f_sync_; 73 | Vector7d q_1_; 74 | 75 | double time_ = 0.0; 76 | 77 | Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished(); 78 | Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); 79 | Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); 80 | }; 81 | -------------------------------------------------------------------------------- /src/joint_goal_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from copy import deepcopy 3 | import numpy as np 4 | import rospy 5 | from sensor_msgs.msg import JointState 6 | import matplotlib.pyplot as plt 7 | 8 | M_PI = np.pi 9 | M_PI_2 = np.pi / 2.0 10 | M_PI_4 = np.pi / 4.0 11 | M_PI_8 = np.pi/8.0 12 | colors = ['#1b9e77', '#d95f02', '#7570b3', '#e7298a', '#66a61e', '#e6ab02', '#a6761d'] 13 | 14 | joint_to_increment = 2 15 | delta_increment = 0.01 16 | 17 | q_list = [] 18 | qd_list = [] 19 | tstep_list = [] 20 | tstep = 0 21 | start_t = 0 22 | start_q = None 23 | goal_q = None 24 | goal_qd = np.zeros(7) 25 | 26 | def state_sub(msg): 27 | global tstep, start_t, start_q, goal_q 28 | q_list.append(msg.position) 29 | qd_list.append(msg.velocity) 30 | if tstep == 0: 31 | start_q = deepcopy(np.array(msg.position)) 32 | start_t = rospy.get_time() 33 | goal_q = start_q 34 | print(goal_q) 35 | goal_q[joint_to_increment] += delta_increment 36 | 37 | tstep = rospy.get_time() - start_t 38 | tstep_list.append(tstep) 39 | 40 | 41 | def goal_pub(): 42 | global start_q, goal_q, goal_qd, joint_to_increment, delta_increment 43 | pub = rospy.Publisher('franka_motion_control/joint_command', JointState, queue_size=1) 44 | rate = rospy.Rate(100) 45 | 46 | while not rospy.is_shutdown(): 47 | if start_q is not None: 48 | goal_state = JointState() 49 | 50 | goal_state.position = goal_q 51 | goal_state.velocity = goal_qd 52 | print("Goal pos: {}, Goal vel: {}".format(goal_q, goal_qd)) 53 | pub.publish(goal_state) 54 | else: 55 | print('Waiting for state....') 56 | 57 | rate.sleep() 58 | plot() 59 | 60 | def plot(): 61 | global goal_q, goal_qd, q_list, qd_list, joint_to_increment, tstep_list 62 | fig, ax = plt.subplots(2,1) 63 | num_pts = len(tstep_list) 64 | q_list = np.array(q_list) 65 | qd_list = np.array(qd_list) 66 | if num_pts > 1: 67 | # for i in range(7): 68 | i = joint_to_increment 69 | ax[0].plot(tstep_list, [goal_q[i]]*num_pts, linestyle='dashed', color=colors[i], label='joint_{}_des'.format(i)) 70 | ax[0].plot(tstep_list, q_list[:,i], color=colors[i]) 71 | ax[1].plot(tstep_list, [goal_qd[i]]*num_pts, linestyle='dashed', color=colors[i]) 72 | ax[1].plot(tstep_list, qd_list[:,i], color=colors[i]) 73 | ax[0].set_title('Joint Position') 74 | ax[1].set_title('Joint Velocity') 75 | ax[0].legend() 76 | plt.show() 77 | 78 | 79 | 80 | if __name__ == '__main__': 81 | rospy.init_node('joint_pos_goal_publisher', anonymous=True) 82 | state_sub = rospy.Subscriber('franka_motion_control/joint_states', JointState, state_sub) 83 | goal_pub() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Lower level ROS controllers for following joint commands with Franka Emika robot. This code runs on computer attached to the robot with real-time kernel. 2 | 3 | ## System Requirements 4 | 5 | [1] Ubuntu 18.04 6 | 7 | [2] ROS Melodic 8 | 9 | [3] `libfranka` v0.7 10 | 11 | ## Setup 12 | 13 | We assume `libfranka` has been setup using the instructions [here](https://frankaemika.github.io/docs/installation_linux.html). 14 | 15 | ### ROS Package Dependencies 16 | 17 | [1] We use [`franka_panda_description`](https://github.com/justagist/franka_panda_description.git) for URDFs. However, you can use another package using `$robot_urdf` parameter in our launch files. 18 | 19 | [2] [`franka_gripper`](https://github.com/frankaemika/franka_ros/tree/kinetic-devel/franka_gripper) (part of `franka_ros`) is required for parallel jaw gripper support. 20 | 21 | ### Building 22 | 23 | [1] ```mkdir -p ~/catkin_ws/src && cd ~/catkin_ws/src``` 24 | 25 | [2] ```git clonehttps://github.com/mohakbhardwaj/franka_motion_control.git``` 26 | 27 | [3] ```git clone https://github.com/frankaemika/franka_ros.git``` 28 | 29 | [4] ```git clone https://github.com/justagist/franka_panda_description.git``` 30 | 31 | [5] ```catkin_make -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=/path/to/libfranka/build``` 32 | 33 | 34 | (Optional) ``source ~/catkin_ws/src/franka_motion_control/config/franka_setup.sh``. This adds a few home configs as environment variables. 35 | 36 | ## Examples 37 | 38 | [1] Moving to Joint Configurations 39 | 40 | ```roslaunch franka_motion_control move_to_joint_config.launch goal_config:="" robot_ip:="" speed_factor=""``` 41 | 42 | If `franka_setup.sh` was sourced, we can use 43 | ```roslaunch franka_motion_control move_to_joint_config.launch goal_config:="${home_1}" robot_ip:="172.16.0.2" speed_factor:="0.2"``` 44 | 45 | 46 | [2] Monitor Mode 47 | 48 | Runs robot in monitor mode where it publishes robot state to a ROS topic. 49 | 50 | ```roslaunch franka_motion_control monitor_mode.launch robot_ip:="" load_gripper:="" robot_urdf:="" ``` 51 | 52 | Example usage: 53 | 54 | ```roslaunch franka_motion_control monitor_mode.launch robot_ip:="172.16.0.2" load_gripper:="true" robot_urdf:="$(rospack find franka_panda_description)/robots/panda_arm_hand.urdf.xacro" ``` 55 | 56 | You can see published joint states using (arm only) 57 | 58 | ```rostopic echo /franka_motion_control/joint_states``` 59 | 60 | The node also launches `joint_state_publisher` and `robot_state_publisher`. Use following to see published joint states for gripper and arm 61 | 62 | ```rostopic echo joint_states``` 63 | 64 | You can also view RobotModel by running RVIZ 65 | 66 | 67 | [3] Gravity Compensation Mode 68 | 69 | Runs robot in gravity compensation mode 70 | 71 | ```roslaunch franka_motion_control gravity_mode.launch ``` 72 | 73 | Example usage same as Monitor Mode 74 | 75 | [4] Tracking Controller 76 | 77 | Runs a torque controller that tracks joint position, velocity and acceleration commands sent as sensor_msgs/JointState message. 78 | 79 | ```roslaunch franka_motion_control command_mode.launch``` 80 | 81 | Example usage same as Monitor Mode 82 | 83 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | franka_motion_control 4 | 0.0.0 5 | The franka_motion_control package 6 | 7 | 8 | 9 | 10 | mohak 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 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | sensor_msgs 56 | roscpp 57 | rospy 58 | std_msgs 59 | sensor_msgs 60 | roscpp 61 | rospy 62 | std_msgs 63 | sensor_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | 53 | 54 | # Byte-compiled / optimized / DLL files 55 | __pycache__/ 56 | *.py[cod] 57 | *$py.class 58 | 59 | # C extensions 60 | *.so 61 | 62 | # Distribution / packaging 63 | .Python 64 | build/ 65 | develop-eggs/ 66 | dist/ 67 | downloads/ 68 | eggs/ 69 | .eggs/ 70 | lib/ 71 | lib64/ 72 | parts/ 73 | sdist/ 74 | var/ 75 | wheels/ 76 | share/python-wheels/ 77 | *.egg-info/ 78 | .installed.cfg 79 | *.egg 80 | MANIFEST 81 | 82 | # PyInstaller 83 | # Usually these files are written by a python script from a template 84 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 85 | *.manifest 86 | *.spec 87 | 88 | # Installer logs 89 | pip-log.txt 90 | pip-delete-this-directory.txt 91 | 92 | # Unit test / coverage reports 93 | htmlcov/ 94 | .tox/ 95 | .nox/ 96 | .coverage 97 | .coverage.* 98 | .cache 99 | nosetests.xml 100 | coverage.xml 101 | *.cover 102 | *.py,cover 103 | .hypothesis/ 104 | .pytest_cache/ 105 | cover/ 106 | 107 | # Translations 108 | *.mo 109 | *.pot 110 | 111 | # Django stuff: 112 | *.log 113 | local_settings.py 114 | db.sqlite3 115 | db.sqlite3-journal 116 | 117 | # Flask stuff: 118 | instance/ 119 | .webassets-cache 120 | 121 | # Scrapy stuff: 122 | .scrapy 123 | 124 | # Sphinx documentation 125 | docs/_build/ 126 | 127 | # PyBuilder 128 | .pybuilder/ 129 | target/ 130 | 131 | # Jupyter Notebook 132 | .ipynb_checkpoints 133 | 134 | # IPython 135 | profile_default/ 136 | ipython_config.py 137 | 138 | # pyenv 139 | # For a library or package, you might want to ignore these files since the code is 140 | # intended to run in multiple environments; otherwise, check them in: 141 | # .python-version 142 | 143 | # pipenv 144 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 145 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 146 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 147 | # install all needed dependencies. 148 | #Pipfile.lock 149 | 150 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 151 | __pypackages__/ 152 | 153 | # Celery stuff 154 | celerybeat-schedule 155 | celerybeat.pid 156 | 157 | # SageMath parsed files 158 | *.sage.py 159 | 160 | # Environments 161 | .env 162 | .venv 163 | env/ 164 | venv/ 165 | ENV/ 166 | env.bak/ 167 | venv.bak/ 168 | 169 | # Spyder project settings 170 | .spyderproject 171 | .spyproject 172 | 173 | # Rope project settings 174 | .ropeproject 175 | 176 | # mkdocs documentation 177 | /site 178 | 179 | # mypy 180 | .mypy_cache/ 181 | .dmypy.json 182 | dmypy.json 183 | 184 | # Pyre type checker 185 | .pyre/ 186 | 187 | # pytype static type analyzer 188 | .pytype/ 189 | 190 | # Cython debug symbols 191 | cython_debug/ 192 | 193 | 194 | # Prerequisites 195 | *.d 196 | 197 | # Compiled Object files 198 | *.slo 199 | *.lo 200 | *.o 201 | *.obj 202 | 203 | # Precompiled Headers 204 | *.gch 205 | *.pch 206 | 207 | # Compiled Dynamic libraries 208 | *.so 209 | *.dylib 210 | *.dll 211 | 212 | # Fortran module files 213 | *.mod 214 | *.smod 215 | 216 | # Compiled Static libraries 217 | *.lai 218 | *.la 219 | *.a 220 | *.lib 221 | 222 | # Executables 223 | *.exe 224 | *.out 225 | *.app 226 | -------------------------------------------------------------------------------- /src/joint_position_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "joint_position_controller.h" 3 | 4 | 5 | JointPositionController::JointPositionController(ros::NodeHandle* nh, ros::NodeHandle* pnh, std::string robot_ip): 6 | FrankaController(nh, pnh, robot_ip){ 7 | 8 | // tau_d_error_.setZero(); 9 | // tau_d_coriolis_.setZero(); 10 | // tau_d_inertia_.setZero(); 11 | // tau_d_calculated_.setZero(); 12 | 13 | // Km_.setZero(); 14 | // Kp_.setZero(); 15 | // Kd_.setZero(); 16 | // gains_set_ = false; 17 | 18 | } 19 | 20 | // void TrackingController::initialize_control_gains(){ 21 | // std::vector Kp, Kd, Km; 22 | // pnh_.getParam("Kp", Kp); 23 | // pnh_.getParam("Kd", Kd); 24 | // pnh_.getParam("Km", Km); 25 | // pnh_.getParam("alpha_q", alpha_q_); 26 | // pnh_.getParam("alpha_dq", alpha_dq_); 27 | // for(size_t i = 0; i < 7; ++i){ 28 | // Kp_[i] = Kp[i]; 29 | // Kd_[i] = Kd[i]; 30 | // Km_[i] = Km[i]; 31 | // } 32 | // gains_set_ = true; 33 | // } 34 | 35 | void JointPositionController::command_loop(){ 36 | // if (!gains_set_){ 37 | // throw std::runtime_error("Control gains not set!!"); 38 | // } 39 | while(ros::ok()){ 40 | robot_.control([&](const franka::RobotState& robot_state, franka::Duration period) 41 | -> franka::JointPositions { 42 | return joint_position_controller_callback(robot_state, period); 43 | } 44 | ); 45 | } 46 | 47 | } 48 | 49 | franka::JointPositions JointPositionController::joint_position_controller_callback(const franka::RobotState& robot_state, franka::Duration period){ 50 | time_ += period.toSec(); 51 | // publishRobotState(robot_state); 52 | curr_q_ = Vector7d(robot_state.q.data()); 53 | // curr_dq_ = Vector7d(robot_state.dq.data()); 54 | 55 | if(time_ == 0.0){ 56 | //Initial state to hold while waiting for command 57 | q_des_cmd_ = curr_q_; 58 | // dq_des_cmd_.setZero(); // = curr_dq_; 59 | // ddq_des_cmd_.setZero(); 60 | curr_q_bel_= curr_q_; 61 | prev_q_bel_ = curr_q_; 62 | curr_dq_bel_ = curr_dq_; 63 | } 64 | else{ 65 | //filter state 66 | curr_q_bel_ = alpha_q_ * curr_q_ + (1.0 - alpha_q_) * curr_q_bel_; 67 | 68 | curr_dq_ = (curr_q_bel_ - prev_q_bel_) / 0.001; 69 | for(size_t i=0; i < 7; ++i){ 70 | if(std::abs(curr_dq_[i]) <= 0.05){ 71 | curr_dq_[i] = 0.0; 72 | } 73 | } 74 | 75 | curr_dq_bel_ = alpha_dq_ * curr_dq_ + (1.0 - alpha_dq_) * curr_dq_bel_; 76 | 77 | for(size_t i=0; i < 7; ++i){ 78 | if(std::abs(curr_dq_bel_[i]) <= 0.001){ 79 | curr_dq_bel_[i] = 0.0; 80 | } 81 | } 82 | 83 | } 84 | 85 | publishRobotState(curr_q_bel_, curr_dq_bel_); 86 | prev_q_bel_ = curr_q_bel_; 87 | 88 | if(command_pub_started_){ 89 | 90 | //update desired command 91 | q_des_cmd_ = curr_q_des_; 92 | // dq_des_cmd_ = curr_dq_des_; 93 | // ddq_des_cmd_ = curr_ddq_des_; 94 | 95 | } 96 | 97 | 98 | // tau_d_inertia_ = inertia_matrix * ddq_des_cmd_; 99 | // tau_d_inertia_ = Km_.cwiseProduct(tau_d_inertia_); 100 | 101 | // tau_d_error_ = Kp_.cwiseProduct(q_des_cmd_ - curr_q_bel_) + Kd_.cwiseProduct(dq_des_cmd_ - curr_dq_bel_); 102 | // tau_d_calculated_ = tau_d_inertia_ + tau_d_error_ + tau_d_coriolis_; 103 | 104 | // Eigen::VectorXd::Map(&tau_d_calculated_arr_[0], 7) = tau_d_calculated_; 105 | 106 | // std::array tau_d_rate_limited = 107 | // franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated_arr, robot_state.tau_J_d); 108 | 109 | // if(!ros::ok()){ 110 | // ROS_INFO("Ros shutdown"); 111 | // } 112 | std::array joint_positions; 113 | Eigen::VectorXd::Map(&joint_positions[0], 7) = q_des_cmd_; 114 | franka::JointPositions output(joint_positions); 115 | output.motion_finished = false; 116 | ros::spinOnce(); 117 | return output; 118 | } 119 | 120 | -------------------------------------------------------------------------------- /src/joint_velocity_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "joint_velocity_controller.h" 3 | 4 | 5 | JointVelocityController::JointVelocityController(ros::NodeHandle* nh, ros::NodeHandle* pnh, std::string robot_ip): 6 | FrankaController(nh, pnh, robot_ip){ 7 | 8 | // tau_d_error_.setZero(); 9 | // tau_d_coriolis_.setZero(); 10 | // tau_d_inertia_.setZero(); 11 | // tau_d_calculated_.setZero(); 12 | 13 | // Km_.setZero(); 14 | // Kp_.setZero(); 15 | // Kd_.setZero(); 16 | // gains_set_ = false; 17 | 18 | } 19 | 20 | // void TrackingController::initialize_control_gains(){ 21 | // std::vector Kp, Kd, Km; 22 | // pnh_.getParam("Kp", Kp); 23 | // pnh_.getParam("Kd", Kd); 24 | // pnh_.getParam("Km", Km); 25 | // pnh_.getParam("alpha_q", alpha_q_); 26 | // pnh_.getParam("alpha_dq", alpha_dq_); 27 | // for(size_t i = 0; i < 7; ++i){ 28 | // Kp_[i] = Kp[i]; 29 | // Kd_[i] = Kd[i]; 30 | // Km_[i] = Km[i]; 31 | // } 32 | // gains_set_ = true; 33 | // } 34 | 35 | void JointVelocityController::command_loop(){ 36 | // if (!gains_set_){ 37 | // throw std::runtime_error("Control gains not set!!"); 38 | // } 39 | while(ros::ok()){ 40 | robot_.control([&](const franka::RobotState& robot_state, franka::Duration period) 41 | -> franka::JointVelocities { 42 | return joint_velocity_controller_callback(robot_state, period); 43 | } 44 | ); 45 | } 46 | 47 | } 48 | 49 | franka::JointVelocities JointVelocityController::joint_velocity_controller_callback(const franka::RobotState& robot_state, franka::Duration period){ 50 | time_ += period.toSec(); 51 | // publishRobotState(robot_state); 52 | curr_q_ = Vector7d(robot_state.q.data()); 53 | // curr_dq_ = Vector7d(robot_state.dq.data()); 54 | 55 | if(time_ == 0.0){ 56 | //Initial state to hold while waiting for command 57 | // q_des_cmd_ = curr_q_; 58 | dq_des_cmd_.setZero(); // = curr_dq_; 59 | // ddq_des_cmd_.setZero(); 60 | curr_q_bel_= curr_q_; 61 | prev_q_bel_ = curr_q_; 62 | curr_dq_bel_ = curr_dq_; 63 | } 64 | else{ 65 | //filter state 66 | curr_q_bel_ = alpha_q_ * curr_q_ + (1.0 - alpha_q_) * curr_q_bel_; 67 | 68 | curr_dq_ = (curr_q_bel_ - prev_q_bel_) / 0.001; 69 | for(size_t i=0; i < 7; ++i){ 70 | if(std::abs(curr_dq_[i]) <= 0.05){ 71 | curr_dq_[i] = 0.0; 72 | } 73 | } 74 | 75 | curr_dq_bel_ = alpha_dq_ * curr_dq_ + (1.0 - alpha_dq_) * curr_dq_bel_; 76 | 77 | for(size_t i=0; i < 7; ++i){ 78 | if(std::abs(curr_dq_bel_[i]) <= 0.001){ 79 | curr_dq_bel_[i] = 0.0; 80 | } 81 | } 82 | 83 | } 84 | 85 | publishRobotState(curr_q_bel_, curr_dq_bel_); 86 | prev_q_bel_ = curr_q_bel_; 87 | 88 | if(command_pub_started_){ 89 | 90 | //update desired command 91 | // q_des_cmd_ = curr_q_des_; 92 | dq_des_cmd_ = curr_dq_des_; 93 | // ddq_des_cmd_ = curr_ddq_des_; 94 | 95 | } 96 | 97 | 98 | // tau_d_inertia_ = inertia_matrix * ddq_des_cmd_; 99 | // tau_d_inertia_ = Km_.cwiseProduct(tau_d_inertia_); 100 | 101 | // tau_d_error_ = Kp_.cwiseProduct(q_des_cmd_ - curr_q_bel_) + Kd_.cwiseProduct(dq_des_cmd_ - curr_dq_bel_); 102 | // tau_d_calculated_ = tau_d_inertia_ + tau_d_error_ + tau_d_coriolis_; 103 | 104 | // Eigen::VectorXd::Map(&tau_d_calculated_arr_[0], 7) = tau_d_calculated_; 105 | 106 | // std::array tau_d_rate_limited = 107 | // franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated_arr, robot_state.tau_J_d); 108 | 109 | // if(!ros::ok()){ 110 | // ROS_INFO("Ros shutdown"); 111 | // } 112 | std::array joint_velocities; 113 | Eigen::VectorXd::Map(&joint_velocities[0], 7) = dq_des_cmd_; 114 | franka::JointVelocities output(joint_velocities); 115 | output.motion_finished = false; 116 | ros::spinOnce(); 117 | return output; 118 | } 119 | 120 | -------------------------------------------------------------------------------- /include/tracking_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "franka_controller.h" 16 | 17 | 18 | class TrackingController : public FrankaController 19 | { 20 | public: 21 | TrackingController(ros::NodeHandle* nh, ros::NodeHandle* pnh, std::string robot_ip); 22 | void command_loop(); 23 | void initialize_control_gains(); 24 | 25 | 26 | private: 27 | franka::Torques torque_controller_callback(const franka::RobotState& robot_state, franka::Duration period); 28 | 29 | Vector7d tau_d_error_, tau_d_coriolis_, tau_d_inertia_, tau_d_calculated_; 30 | std::array tau_d_calculated_arr_; 31 | 32 | // Vector7f dq_max_ = (Vector7f() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished(); 33 | // Vector7f ddq_max_start_ = (Vector7f() << 5, 5, 5, 5, 5, 5, 5).finished(); 34 | // Vector7f ddq_max_goal_ = (Vector7f() << 5, 5, 5, 5, 5, 5, 5).finished(); 35 | 36 | // Vector7f P_ = (Vector7f() << 6.0, 6.0, 6.0, 6.0, 2.5, 4.0, 4.0).finished(); 37 | 38 | // Vector7f P_ = (Vector7f() << 7.0, 5.0, 5.0, 7.0, 5.0, 6.0, 7.0).finished(); 39 | // Vector7f D_ = (Vector7f() << 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.5).finished(); 40 | double alpha_q_, alpha_dq_; 41 | Vector7d Km_, Kp_, Kd_; 42 | bool gains_set_; 43 | ////Use these 44 | // double alpha_q_ = 0.8; //1.0; 45 | // double alpha_dq_ = 0.01; //0.05; //1.0; 46 | // Vector7d Pf_ = (Vector7d() << 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01).finished(); 47 | //// 48 | 49 | // Vector7d P_ = (Vector7d() << 210.0, 200.0, 5.0, 7.0, 5.0, 6.0, 7.0).finished(); 50 | // Vector7d P_ = (Vector7d() << 100.0, 100.0, 100.0, 100.0, 50.0, 50.0, 50.0).finished(); 51 | // Vector7d D_ = (Vector7d() << 12.0, 12.0, 12.0, 12.0, 2.0, 2.0, 1.0).finished(); 52 | 53 | //star 54 | // Vector7d P_ = (Vector7d() << 90.0, 90.0, 90.0, 90.0, 50.0, 50.0, 50.0).finished(); 55 | // Vector7d D_ = (Vector7d() << 16.0, 16.0, 16.0, 16.0, 3.0, 3.0, 3.0).finished(); 56 | 57 | // Vector7d P_ = (Vector7d() << 80.0, 80.0, 80.0, 80.0, 50.0, 50.0, 50.0).finished(); 58 | // Vector7d D_ = (Vector7d() << 16.0, 16.0, 16.0, 16.0, 3.0, 3.0, 3.0).finished(); 59 | 60 | 61 | // Vector7d P_ = (Vector7d() << 70.0, 70.0, 70.0, 70.0, 40.0, 40.0, 40.0).finished(); 62 | // Vector7d D_ = (Vector7d() << 16.0, 16.0, 16.0, 16.0, 3.0, 3.0, 3.0).finished(); 63 | 64 | 65 | // Vector7d P_ = (Vector7d() << 300.0, 300.0, 300.0, 300.0, 100.0, 100.0, 100.0).finished(); 66 | // Vector7d D_ = (Vector7d() << 20.0, 20.0, 20.0, 25.0, 2.0, 2.0, 2.0).finished(); 67 | 68 | // Vector7d P_ = (Vector7d() << 500.0, 500.0, 500.0, 500.0, 100.0, 100.0, 50.0).finished(); 69 | // Vector7d P_ = (Vector7d() << 300.0, 300.0, 300.0, 300.0, 100.0, 100.0, 50.0).finished(); 70 | // Vector7d D_ = (Vector7d() << 20.0, 20.0, 20.0, 25.0, 2.0, 2.0, 2.0).finished(); 71 | 72 | // Vector7d D_ = (Vector7d() << 25.0, 25.0, 25.0, 25.0, 2.0, 2.0, 2.0).finished(); 73 | // Vector7d D_ = (Vector7d() << 30.0, 30.0, 30.0, 30.0, 2.0, 2.0, 2.0).finished(); 74 | 75 | 76 | // Vector7d P_ = (Vector7d() << 600.0, 600.0, 600.0, 600.0, 100.0, 100.0, 50.0).finished(); 77 | // // Vector7d D_ = (Vector7d() << 50.0, 50.0, 50.0, 50.0, 5.0, 5.0, 5.0).finished(); 78 | // Vector7d D_ = (Vector7d() << 25.0, 25.0, 25.0, 25.0, 2.0, 2.0, 2.0).finished(); 79 | // // Vector7d D_ = (Vector7d() << 20.0, 20.0, 20.0, 20.0, 2.0, 2.0, 2.0).finished(); 80 | // Vector7d D_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 0.2, 0.2, 2.0).finished(); 81 | 82 | 83 | // Vector7d P_ = (Vector7d() << 650.0, 650.0, 650.0, 650.0, 200.0, 100.0, 50.0).finished(); 84 | ///Use this 85 | // Vector7d P_ = (Vector7d() << 650.0, 650.0, 650.0, 650.0, 100.0, 100.0, 50.0).finished(); 86 | ////// 87 | // Vector7d D_ = (Vector7d() << 25.0, 25.0, 25.0, 25.0, 2.0, 2.0, 2.0).finished(); 88 | 89 | //Use this 90 | // Vector7d D_ = (Vector7d() << 30.0, 30.0, 30.0, 30.0, 5.0, 5.0, 2.0).finished(); 91 | ///// 92 | }; 93 | 94 | -------------------------------------------------------------------------------- /src/tracking_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "tracking_controller.h" 3 | 4 | 5 | TrackingController::TrackingController(ros::NodeHandle* nh, ros::NodeHandle* pnh, std::string robot_ip): 6 | FrankaController(nh, pnh, robot_ip){ 7 | 8 | tau_d_error_.setZero(); 9 | tau_d_coriolis_.setZero(); 10 | tau_d_inertia_.setZero(); 11 | tau_d_calculated_.setZero(); 12 | 13 | Km_.setZero(); 14 | Kp_.setZero(); 15 | Kd_.setZero(); 16 | gains_set_ = false; 17 | 18 | } 19 | 20 | void TrackingController::initialize_control_gains(){ 21 | std::vector Kp, Kd, Km; 22 | pnh_.getParam("Kp", Kp); 23 | pnh_.getParam("Kd", Kd); 24 | pnh_.getParam("Km", Km); 25 | pnh_.getParam("alpha_q", alpha_q_); 26 | pnh_.getParam("alpha_dq", alpha_dq_); 27 | for(size_t i = 0; i < 7; ++i){ 28 | Kp_[i] = Kp[i]; 29 | Kd_[i] = Kd[i]; 30 | Km_[i] = Km[i]; 31 | } 32 | gains_set_ = true; 33 | } 34 | 35 | void TrackingController::command_loop(){ 36 | if (!gains_set_){ 37 | throw std::runtime_error("Control gains not set!!"); 38 | } 39 | while(ros::ok()){ 40 | robot_.control([&](const franka::RobotState& robot_state, franka::Duration period) 41 | -> franka::Torques { 42 | return torque_controller_callback(robot_state, period); 43 | } 44 | ); 45 | } 46 | 47 | } 48 | 49 | franka::Torques TrackingController::torque_controller_callback(const franka::RobotState& robot_state, franka::Duration period){ 50 | time_ += period.toSec(); 51 | // publishRobotState(robot_state); 52 | curr_q_ = Vector7d(robot_state.q.data()); 53 | // curr_dq_ = Vector7d(robot_state.dq.data()); 54 | 55 | if(time_ == 0.0){ 56 | //Initial state to hold while waiting for command 57 | q_des_cmd_ = curr_q_; 58 | dq_des_cmd_ = curr_dq_; 59 | ddq_des_cmd_.setZero(); 60 | curr_q_bel_= curr_q_; 61 | prev_q_bel_ = curr_q_; 62 | curr_dq_bel_ = curr_dq_; 63 | } 64 | else{ 65 | //filter state 66 | curr_q_bel_ = alpha_q_ * curr_q_ + (1.0 - alpha_q_) * curr_q_bel_; 67 | 68 | curr_dq_ = (curr_q_bel_ - prev_q_bel_) / 0.001; 69 | for(size_t i=0; i < 7; ++i){ 70 | if(std::abs(curr_dq_[i]) <= 0.05){ 71 | curr_dq_[i] = 0.0; 72 | } 73 | } 74 | 75 | curr_dq_bel_ = alpha_dq_ * curr_dq_ + (1.0 - alpha_dq_) * curr_dq_bel_; 76 | 77 | for(size_t i=0; i < 7; ++i){ 78 | if(std::abs(curr_dq_bel_[i]) <= 0.001){ 79 | curr_dq_bel_[i] = 0.0; 80 | } 81 | } 82 | 83 | } 84 | 85 | publishRobotState(curr_q_bel_, curr_dq_bel_); 86 | prev_q_bel_ = curr_q_bel_; 87 | 88 | 89 | Mat7d inertia_matrix = Mat7d(model_.mass(robot_state).data()); 90 | tau_d_coriolis_ = Vector7d(model_.coriolis(robot_state).data()); //coriolis torque from current state 91 | 92 | if(command_pub_started_){ 93 | 94 | // //Filter state 95 | // curr_q_bel_ = alpha_q_ * curr_q_ + (1.0 - alpha_q_) * curr_q_bel_; 96 | 97 | // // curr_dq_bel_ = clip 98 | // curr_dq_bel_ = alpha_dq_ * curr_dq_ + (1.0 - alpha_dq_) * curr_dq_bel_; 99 | 100 | //update desired command 101 | q_des_cmd_ = curr_q_des_; 102 | dq_des_cmd_ = curr_dq_des_; 103 | ddq_des_cmd_ = curr_ddq_des_; 104 | 105 | } 106 | 107 | 108 | tau_d_inertia_ = inertia_matrix * ddq_des_cmd_; 109 | tau_d_inertia_ = Km_.cwiseProduct(tau_d_inertia_); 110 | 111 | tau_d_error_ = Kp_.cwiseProduct(q_des_cmd_ - curr_q_bel_) + Kd_.cwiseProduct(dq_des_cmd_ - curr_dq_bel_); 112 | tau_d_calculated_ = tau_d_inertia_ + tau_d_error_ + tau_d_coriolis_; 113 | 114 | Eigen::VectorXd::Map(&tau_d_calculated_arr_[0], 7) = tau_d_calculated_; 115 | 116 | // std::array tau_d_rate_limited = 117 | // franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated_arr, robot_state.tau_J_d); 118 | 119 | // if(!ros::ok()){ 120 | // ROS_INFO("Ros shutdown"); 121 | // } 122 | ros::spinOnce(); 123 | return tau_d_calculated_arr_; //tau_d_rate_limited; 124 | } 125 | 126 | -------------------------------------------------------------------------------- /src/examples_common.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 "examples_common.h" 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | void setDefaultBehavior(franka::Robot& robot) { 13 | robot.setCollisionBehavior( 14 | {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, 15 | {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, 16 | {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, 17 | {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); 18 | robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); 19 | robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}}); 20 | } 21 | 22 | MotionGenerator::MotionGenerator(double speed_factor, const std::array q_goal) 23 | : q_goal_(q_goal.data()) { 24 | dq_max_ *= speed_factor; 25 | ddq_max_start_ *= speed_factor; 26 | ddq_max_goal_ *= speed_factor; 27 | dq_max_sync_.setZero(); 28 | q_start_.setZero(); 29 | delta_q_.setZero(); 30 | t_1_sync_.setZero(); 31 | t_2_sync_.setZero(); 32 | t_f_sync_.setZero(); 33 | q_1_.setZero(); 34 | } 35 | 36 | bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const { 37 | Vector7i sign_delta_q; 38 | sign_delta_q << delta_q_.cwiseSign().cast(); 39 | Vector7d t_d = t_2_sync_ - t_1_sync_; 40 | Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_; 41 | std::array joint_motion_finished{}; 42 | 43 | for (size_t i = 0; i < 7; i++) { 44 | if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) { 45 | (*delta_q_d)[i] = 0; 46 | joint_motion_finished[i] = true; 47 | } else { 48 | if (t < t_1_sync_[i]) { 49 | (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] * 50 | (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0); 51 | } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) { 52 | (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i]; 53 | } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) { 54 | (*delta_q_d)[i] = 55 | delta_q_[i] + 0.5 * 56 | (1.0 / std::pow(delta_t_2_sync[i], 3.0) * 57 | (t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) * 58 | std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) + 59 | (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) * 60 | dq_max_sync_[i] * sign_delta_q[i]; 61 | } else { 62 | (*delta_q_d)[i] = delta_q_[i]; 63 | joint_motion_finished[i] = true; 64 | } 65 | } 66 | } 67 | return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(), 68 | [](bool x) { return x; }); 69 | } 70 | 71 | void MotionGenerator::calculateSynchronizedValues() { 72 | Vector7d dq_max_reach(dq_max_); 73 | Vector7d t_f = Vector7d::Zero(); 74 | Vector7d delta_t_2 = Vector7d::Zero(); 75 | Vector7d t_1 = Vector7d::Zero(); 76 | Vector7d delta_t_2_sync = Vector7d::Zero(); 77 | Vector7i sign_delta_q; 78 | sign_delta_q << delta_q_.cwiseSign().cast(); 79 | 80 | for (size_t i = 0; i < 7; i++) { 81 | if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) { 82 | if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) + 83 | 3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) { 84 | dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] * 85 | (ddq_max_start_[i] * ddq_max_goal_[i]) / 86 | (ddq_max_start_[i] + ddq_max_goal_[i])); 87 | } 88 | t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i]; 89 | delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i]; 90 | t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i]; 91 | } 92 | } 93 | double max_t_f = t_f.maxCoeff(); 94 | for (size_t i = 0; i < 7; i++) { 95 | if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) { 96 | double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]); 97 | double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i]; 98 | double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i]; 99 | double delta = b * b - 4.0 * a * c; 100 | if (delta < 0.0) { 101 | delta = 0.0; 102 | } 103 | dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a); 104 | t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i]; 105 | delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i]; 106 | t_f_sync_[i] = 107 | (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]); 108 | t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i]; 109 | q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]); 110 | } 111 | } 112 | } 113 | 114 | franka::JointPositions MotionGenerator::operator()(const franka::RobotState& robot_state, 115 | franka::Duration period) { 116 | time_ += period.toSec(); 117 | 118 | if (time_ == 0.0) { 119 | q_start_ = Vector7d(robot_state.q_d.data()); 120 | delta_q_ = q_goal_ - q_start_; 121 | calculateSynchronizedValues(); 122 | } 123 | 124 | Vector7d delta_q_d; 125 | bool motion_finished = calculateDesiredValues(time_, &delta_q_d); 126 | 127 | std::array joint_positions; 128 | Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d); 129 | franka::JointPositions output(joint_positions); 130 | output.motion_finished = motion_finished; 131 | return output; 132 | } 133 | 134 | void MotionGenerator::UpdateGoal(const std::array& q_goal){ 135 | time_ = 0.0; 136 | q_goal_ = Vector7d(q_goal.data()); 137 | } -------------------------------------------------------------------------------- /src/franka_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "franka_controller.h" 2 | 3 | FrankaController::FrankaController(ros::NodeHandle* nh, ros::NodeHandle* pnh, std::string robot_ip): 4 | nh_(*nh), pnh_(*pnh), robot_(robot_ip), model_(robot_.loadModel()){ 5 | 6 | 7 | curr_q_.setZero(); 8 | curr_dq_.setZero(); 9 | curr_q_des_.setZero(); 10 | curr_dq_des_.setZero(); 11 | curr_ddq_des_.setZero(); 12 | curr_q_bel_.setZero(); 13 | curr_dq_bel_.setZero(); 14 | tau_z_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; 15 | 16 | setDefaultBehavior(robot_); 17 | 18 | command_pub_started_ = false; 19 | nh_.getParam("joint_states_topic", joint_states_topic_); 20 | nh_.getParam("joint_command_topic", joint_command_topic_); 21 | nh_.getParam("robot_joint_command_topic", robot_joint_command_topic_); 22 | nh_.getParam("prefix", prefix_); 23 | pnh_.getParam("joint_names", joint_names_); 24 | 25 | joint_states_topic_ = prefix_ + "/" + joint_states_topic_; 26 | joint_command_topic_ = prefix_ + "/" + joint_command_topic_; 27 | robot_joint_command_topic_ = prefix_ + "/" + robot_joint_command_topic_; 28 | 29 | initializeSubscribers(); 30 | initializePublishers(); 31 | 32 | curr_joint_command_.name.resize(7); 33 | curr_joint_command_.position.resize(7); 34 | curr_joint_command_.velocity.resize(7); 35 | curr_joint_command_.effort.resize(7); 36 | 37 | curr_robot_state_.name.resize(7); 38 | curr_robot_state_.position.resize(7); 39 | curr_robot_state_.velocity.resize(7); 40 | curr_robot_state_.effort.resize(7); 41 | 42 | curr_robot_joint_command_.name.resize(7); 43 | curr_robot_joint_command_.position.resize(7); 44 | curr_robot_joint_command_.velocity.resize(7); 45 | curr_robot_joint_command_.effort.resize(7); 46 | 47 | curr_joint_command_.name = joint_names_; 48 | curr_robot_state_.name = joint_names_; 49 | curr_robot_joint_command_.name = joint_names_; 50 | } 51 | 52 | void FrankaController::initializeSubscribers(){ 53 | ROS_INFO("Initializing Subscriber"); 54 | goal_subscriber_ = nh_.subscribe(joint_command_topic_, 1, &FrankaController::jointCommandCallback, this); 55 | } 56 | 57 | void FrankaController::initializePublishers(){ 58 | ROS_INFO("Initializing Publisher"); 59 | state_publisher_ = nh_.advertise(joint_states_topic_, 1, false); 60 | command_publisher_ = nh_.advertise(robot_joint_command_topic_, 1, false); 61 | } 62 | 63 | void FrankaController::jointCommandCallback(const sensor_msgs::JointState& msg) { 64 | curr_joint_command_ = msg; 65 | curr_q_des_ = Eigen::VectorXd::Map(&curr_joint_command_.position[0], 7); 66 | curr_dq_des_ = Eigen::VectorXd::Map(&curr_joint_command_.velocity[0], 7); 67 | curr_ddq_des_ = Eigen::VectorXd::Map(&curr_joint_command_.effort[0], 7); 68 | command_pub_started_ = true; 69 | } 70 | 71 | bool FrankaController::publishRobotState(const franka::RobotState& robot_state){ 72 | curr_robot_state_.header.stamp = ros::Time::now(); 73 | for (size_t i = 0; i < curr_robot_state_.position.size(); i++) { 74 | curr_robot_state_.position[i] = robot_state.q[i]; 75 | curr_robot_state_.velocity[i] = robot_state.dq[i]; 76 | curr_robot_state_.effort[i] = 0.0; //robot_state.dq_d[i]; 77 | } 78 | state_publisher_.publish(curr_robot_state_); 79 | return true; 80 | } 81 | 82 | bool FrankaController::publishRobotState(const Vector7d& q, const Vector7d& dq){ 83 | curr_robot_state_.header.stamp = ros::Time::now(); 84 | for (size_t i = 0; i < curr_robot_state_.position.size(); i++) { 85 | curr_robot_state_.position[i] = q[i]; 86 | curr_robot_state_.velocity[i] = dq[i]; 87 | curr_robot_state_.effort[i] = 0.0; 88 | } 89 | state_publisher_.publish(curr_robot_state_); 90 | return true; 91 | } 92 | 93 | bool FrankaController::publishRobotCommand(const franka::RobotState& robot_command){ 94 | curr_robot_joint_command_.header.stamp = ros::Time::now(); 95 | for (size_t i = 0; i < curr_robot_joint_command_.position.size(); i++) { 96 | curr_robot_joint_command_.position[i] = robot_command.q[i]; 97 | curr_robot_joint_command_.velocity[i] = robot_command.dq[i]; 98 | curr_robot_joint_command_.effort[i] = robot_command.tau_J[i]; 99 | } 100 | command_publisher_.publish(curr_robot_joint_command_); 101 | return true; 102 | } 103 | 104 | void FrankaController::monitor_loop(){ 105 | // double start_nsecs =ros::Time::now().toNSec(); 106 | // franka::Duration period; 107 | 108 | robot_.read([&](const franka::RobotState& robot_state) { 109 | // bool ros_ok = read_state_callback(robot_state, period); 110 | // double nsecs_elapsed = ros::Time::now().toNSec() - start_nsecs; 111 | 112 | // period = franka::Duration(nsecs_elapsed/1000000.0); 113 | publishRobotState(robot_state); 114 | return ros::ok(); 115 | } 116 | ); 117 | } 118 | 119 | void FrankaController::gravity_command_loop(){ 120 | 121 | while(ros::ok()){ 122 | robot_.control([&](const franka::RobotState& robot_state, franka::Duration period) 123 | -> franka::Torques { 124 | publishRobotState(robot_state); 125 | return tau_z_; 126 | } 127 | ); 128 | } 129 | 130 | } 131 | 132 | // bool FrankaController::read_state_callback(const franka::RobotState& robot_state, franka::Duration period){ 133 | // // motion_generator_callback_integrator(robot_state, period); 134 | // torque_controller_callback(robot_state, period); 135 | // return ros::ok(); 136 | 137 | // } 138 | 139 | 140 | void FrankaController::setDefaultBehavior(franka::Robot& robot) { 141 | // robot.setCollisionBehavior( 142 | // {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, 143 | // {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, 144 | // {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, 145 | // {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); 146 | robot.setCollisionBehavior( 147 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 148 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 149 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, 150 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); 151 | 152 | // robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); 153 | // robot.setJointImpedance({{300, 300, 300, 250, 250, 200, 200}}); 154 | // robot.setJointImpedance({{30, 30, 30, 25, 25, 20, 20}}); 155 | 156 | // robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}}); 157 | } 158 | 159 | 160 | 161 | 162 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(franka_motion_control) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | set(CMAKE_CXX_STANDARD 14) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | 10 | ## Find catkin macros and libraries 11 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 12 | ## is used, also find other catkin packages 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | rospy 16 | std_msgs 17 | sensor_msgs 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | find_package(Franka REQUIRED) 22 | find_package(Eigen3 REQUIRED) 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | set(THREADS_PREFER_PTHREAD_FLAG ON) 25 | find_package(Threads REQUIRED) 26 | 27 | ## Uncomment this if the package has a setup.py. This macro ensures 28 | ## modules and global scripts declared therein get installed 29 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 30 | # catkin_python_setup() 31 | 32 | ################################################ 33 | ## Declare ROS messages, services and actions ## 34 | ################################################ 35 | 36 | ## To declare and build messages, services or actions from within this 37 | ## package, follow these steps: 38 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 39 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 40 | ## * In the file package.xml: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 43 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 44 | ## but can be declared for certainty nonetheless: 45 | ## * add a exec_depend tag for "message_runtime" 46 | ## * In this file (CMakeLists.txt): 47 | ## * add "message_generation" and every package in MSG_DEP_SET to 48 | ## find_package(catkin REQUIRED COMPONENTS ...) 49 | ## * add "message_runtime" and every package in MSG_DEP_SET to 50 | ## catkin_package(CATKIN_DEPENDS ...) 51 | ## * uncomment the add_*_files sections below as needed 52 | ## and list every .msg/.srv/.action file to be processed 53 | ## * uncomment the generate_messages entry below 54 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 55 | 56 | ## Generate messages in the 'msg' folder 57 | # add_message_files( 58 | # FILES 59 | # Message1.msg 60 | # Message2.msg 61 | # ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.srv 68 | # ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | # generate_messages( 79 | # DEPENDENCIES 80 | # std_msgs 81 | # ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if your package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | INCLUDE_DIRS include 114 | LIBRARIES franka_motion_control 115 | CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs 116 | DEPENDS Franka 117 | ) 118 | 119 | ########### 120 | ## Build ## 121 | ########### 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | include_directories( 126 | include 127 | ${catkin_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | # add_library(${PROJECT_NAME} 132 | # src/${PROJECT_NAME}/franka_motion_control.cpp 133 | # ) 134 | 135 | 136 | add_library(franka_controller STATIC 137 | src/franka_controller.cpp 138 | ) 139 | 140 | target_link_libraries(franka_controller PUBLIC 141 | ${Franka_LIBRARIES} 142 | ${catkin_LIBRARIES} 143 | ) 144 | 145 | target_include_directories(franka_controller SYSTEM PUBLIC 146 | ${Franka_INCLUDE_DIRS} 147 | ${EIGEN3_INCLUDE_DIRS} 148 | ${catkin_INCLUDE_DIRS} 149 | ) 150 | target_include_directories(franka_controller PUBLIC 151 | include 152 | ) 153 | 154 | add_library(tracking_controller STATIC 155 | src/tracking_controller.cpp 156 | ) 157 | 158 | target_link_libraries(tracking_controller PUBLIC 159 | franka_controller 160 | ${Franka_LIBRARIES} 161 | ${catkin_LIBRARIES} 162 | ) 163 | 164 | target_include_directories(tracking_controller SYSTEM PUBLIC 165 | ${Franka_INCLUDE_DIRS} 166 | ${EIGEN3_INCLUDE_DIRS} 167 | ${catkin_INCLUDE_DIRS} 168 | ) 169 | target_include_directories(tracking_controller PUBLIC 170 | include 171 | ) 172 | 173 | 174 | add_library(joint_velocity_controller STATIC 175 | src/joint_velocity_controller.cpp 176 | ) 177 | 178 | target_link_libraries(joint_velocity_controller PUBLIC 179 | franka_controller 180 | ${Franka_LIBRARIES} 181 | ${catkin_LIBRARIES} 182 | ) 183 | 184 | target_include_directories(joint_velocity_controller SYSTEM PUBLIC 185 | ${Franka_INCLUDE_DIRS} 186 | ${EIGEN3_INCLUDE_DIRS} 187 | ${catkin_INCLUDE_DIRS} 188 | ) 189 | target_include_directories(joint_velocity_controller PUBLIC 190 | include 191 | ) 192 | 193 | add_library(joint_position_controller STATIC 194 | src/joint_position_controller.cpp 195 | ) 196 | 197 | target_link_libraries(joint_position_controller PUBLIC 198 | franka_controller 199 | ${Franka_LIBRARIES} 200 | ${catkin_LIBRARIES} 201 | ) 202 | 203 | target_include_directories(joint_position_controller SYSTEM PUBLIC 204 | ${Franka_INCLUDE_DIRS} 205 | ${EIGEN3_INCLUDE_DIRS} 206 | ${catkin_INCLUDE_DIRS} 207 | ) 208 | target_include_directories(joint_position_controller PUBLIC 209 | include 210 | ) 211 | 212 | 213 | add_executable(tracking_controller_node src/tracking_controller_node.cpp) 214 | target_include_directories(tracking_controller_node SYSTEM PUBLIC 215 | ${Franka_INCLUDE_DIRS} 216 | ${EIGEN3_INCLUDE_DIRS} 217 | ${catkin_INCLUDE_DIRS} 218 | ) 219 | target_link_libraries(tracking_controller_node tracking_controller franka_controller Franka::Franka) 220 | 221 | add_executable(joint_velocity_controller_node src/joint_velocity_controller_node.cpp) 222 | target_include_directories(joint_velocity_controller_node SYSTEM PUBLIC 223 | ${Franka_INCLUDE_DIRS} 224 | ${EIGEN3_INCLUDE_DIRS} 225 | ${catkin_INCLUDE_DIRS} 226 | ) 227 | target_link_libraries(joint_velocity_controller_node joint_velocity_controller franka_controller Franka::Franka) 228 | 229 | add_executable(joint_position_control_node src/joint_position_control_node.cpp) 230 | target_include_directories(joint_position_control_node SYSTEM PUBLIC 231 | ${Franka_INCLUDE_DIRS} 232 | ${EIGEN3_INCLUDE_DIRS} 233 | ${catkin_INCLUDE_DIRS} 234 | ) 235 | target_link_libraries(joint_position_control_node joint_position_controller franka_controller Franka::Franka) 236 | 237 | #set(NODES 238 | # tracking_controller_node 239 | #) 240 | 241 | 242 | #foreach(node ${NODES}) 243 | # add_executable(${node} src/${node}.cpp) 244 | # target_include_directories(${node} SYSTEM PUBLIC 245 | # ${Franka_INCLUDE_DIRS} 246 | # ${EIGEN3_INCLUDE_DIRS} 247 | # ${catkin_INCLUDE_DIRS} 248 | # ) 249 | # target_link_libraries(${node} franka_controller Franka::Franka) 250 | #endforeach() 251 | 252 | 253 | 254 | add_library(examples_common STATIC 255 | src/examples_common.cpp) 256 | 257 | target_link_libraries(examples_common PUBLIC 258 | ${Franka_LIBRARIES} 259 | ${catkin_LIBRARIES} 260 | ) 261 | 262 | target_include_directories(examples_common SYSTEM PUBLIC 263 | ${Franka_INCLUDE_DIRS} 264 | ${EIGEN3_INCLUDE_DIRS} 265 | ${catkin_INCLUDE_DIRS} 266 | ) 267 | 268 | add_executable(move_to_joint_config src/move_to_joint_config.cpp) 269 | target_include_directories(move_to_joint_config SYSTEM PUBLIC 270 | ${Franka_INCLUDE_DIRS} 271 | ${EIGEN3_INCLUDE_DIRS} 272 | ${catkin_INCLUDE_DIRS} 273 | ) 274 | target_link_libraries(move_to_joint_config examples_common Franka::Franka) 275 | 276 | 277 | ## Add cmake target dependencies of the library 278 | ## as an example, code may need to be generated before libraries 279 | ## either from message generation or dynamic reconfigure 280 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 281 | 282 | ## Declare a C++ executable 283 | ## With catkin_make all packages are built within a single CMake context 284 | ## The recommended prefix ensures that target names across packages don't collide 285 | # add_executable(${PROJECT_NAME}_node src/franka_motion_control_node.cpp) 286 | 287 | ## Rename C++ executable without prefix 288 | ## The above recommended prefix causes long target names, the following renames the 289 | ## target back to the shorter version for ease of user use 290 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 291 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 292 | 293 | ## Add cmake target dependencies of the executable 294 | ## same as for the library above 295 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 296 | 297 | ## Specify libraries to link a library or executable target against 298 | # target_link_libraries(${PROJECT_NAME}_node 299 | # ${catkin_LIBRARIES} 300 | # ) 301 | 302 | ############# 303 | ## Install ## 304 | ############# 305 | 306 | # all install targets should use catkin DESTINATION variables 307 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 308 | 309 | ## Mark executable scripts (Python etc.) for installation 310 | ## in contrast to setup.py, you can choose the destination 311 | # install(PROGRAMS 312 | # scripts/my_python_script 313 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 314 | # ) 315 | 316 | ## Mark executables for installation 317 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 318 | # install(TARGETS ${PROJECT_NAME}_node 319 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 320 | # ) 321 | 322 | ## Mark libraries for installation 323 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 324 | # install(TARGETS ${PROJECT_NAME} 325 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 326 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 327 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 328 | # ) 329 | 330 | ## Mark cpp header files for installation 331 | # install(DIRECTORY include/${PROJECT_NAME}/ 332 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 333 | # FILES_MATCHING PATTERN "*.h" 334 | # PATTERN ".svn" EXCLUDE 335 | # ) 336 | 337 | ## Mark other files for installation (e.g. launch and bag files, etc.) 338 | # install(FILES 339 | # # myfile1 340 | # # myfile2 341 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 342 | # ) 343 | 344 | ############# 345 | ## Testing ## 346 | ############# 347 | 348 | ## Add gtest based cpp test target and link libraries 349 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_franka_motion_control.cpp) 350 | # if(TARGET ${PROJECT_NAME}-test) 351 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 352 | # endif() 353 | 354 | ## Add folders to be run by python nosetests 355 | # catkin_add_nosetests(test) 356 | 357 | 358 | 359 | 360 | 361 | --------------------------------------------------------------------------------