├── .gitignore ├── msg └── EffortCommand.msg ├── setup_local_coc_nvim.sh ├── kortex_api └── README.txt ├── config ├── joint_limits.yaml └── kinova_gen3_controllers.yaml ├── include ├── mock_position_joint_hardware_interface │ └── mocked_position_joint_hardware_interface.h └── kinova_gen3_control │ ├── fake_kinova_network_connection.h │ ├── kinova_network_connection.h │ ├── kinova_gen3_network_connection.h │ ├── gazebo_kinova_network_connection.h │ └── kinova_gen3_hardware_interface.h ├── LICENSE ├── src ├── mock_position_joint_hardware_interface_node.cpp ├── mock_position_joint_hardware_interface │ └── mocked_position_joint_hardware_interface.cpp ├── kinova_gen3_control │ ├── fake_kinova_network_connection.cpp │ ├── gazebo_kinova_network_connection.cpp │ ├── kinova_gen3_network_connection.cpp │ └── kinova_gen3_hardware_interface.cpp └── kinova_gen3_hardware_interface_node.cpp ├── package.xml ├── launch ├── last_joint.launch └── complete.launch ├── README.md └── CMakeLists.txt /.gitignore: -------------------------------------------------------------------------------- 1 | kortex_api/include 2 | kortex_api/lib 3 | env.sh 4 | compile_commands.json 5 | -------------------------------------------------------------------------------- /msg/EffortCommand.msg: -------------------------------------------------------------------------------- 1 | time stamp 2 | float32[] command_before_limiting 3 | float32[] command 4 | float32[] read_position 5 | float32[] read_velocity 6 | float32[] read_effort 7 | float32[] read_current 8 | 9 | 10 | -------------------------------------------------------------------------------- /setup_local_coc_nvim.sh: -------------------------------------------------------------------------------- 1 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS=1 2 | catkin build 3 | ln -s ../../build/inverse_kinematics_trajectory/compile_commands.json compile_commands.json 4 | -------------------------------------------------------------------------------- /kortex_api/README.txt: -------------------------------------------------------------------------------- 1 | Please download the kortex_api zip folder from https://artifactory.kinovaapps.com/artifactory/generic-local-public/kortex/API/2.6.0/kortex_api_2.6.0.zip 2 | 3 | Then unzip that file, 4 | 5 | then run something like the below to copy the needed files into a new directory called kinova_gen3_control/kortex_api/linux_gcc_x86-64 6 | 7 | cd ~/Downloads 8 | rm -rf kortex_api 9 | wget https://artifactory.kinovaapps.com/artifactory/generic-local-public/kortex/API/2.6.0/linux_x86-64_x86_gcc.zip 10 | unzip linux_x86-64_x86_gcc.zip -d kortex_api 11 | cp -r ~/Downloads/kortex_api/* ~/custom_trajectory_ws/src/kinova_gen3_control/kortex_api/ 12 | -------------------------------------------------------------------------------- /config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | joint_limits: 2 | joint_1: 3 | has_position_limits: false # Continuous joint 4 | has_effort_limits: true 5 | max_effort: 32.0 6 | joint_2: 7 | has_position_limits: true 8 | min_position: -2.24 9 | max_position: 2.24 10 | has_effort_limits: true 11 | max_effort: 32.0 12 | joint_3: 13 | has_position_limits: false # Continuous joint 14 | has_effort_limits: true 15 | max_effort: 32.0 16 | joint_4: 17 | has_position_limits: true 18 | min_position: -2.57 19 | max_position: 2.57 20 | has_effort_limits: true 21 | max_effort: 32.0 22 | joint_5: 23 | has_position_limits: false # Continuous joint 24 | has_effort_limits: true 25 | max_effort: 13.0 26 | joint_6: 27 | has_position_limits: true 28 | min_position: -2.09 29 | max_position: 2.09 30 | has_effort_limits: true 31 | max_effort: 13.0 32 | joint_7: 33 | has_position_limits: false # Continuous joint 34 | has_effort_limits: true 35 | max_effort: 13.0 36 | -------------------------------------------------------------------------------- /include/mock_position_joint_hardware_interface/mocked_position_joint_hardware_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef MOCKED_POSITION_HARDWARE_INTERFACE_H 2 | #define MOCKED_POSITION_HARDWARE_INTERFACE_H 3 | #include 4 | #include 5 | #include 6 | 7 | // http://wiki.ros.org/ros_control/Tutorials/Create%20your%20own%20hardware%20interface 8 | 9 | class MockedPositionInterface : public hardware_interface::RobotHW 10 | { 11 | public: 12 | MockedPositionInterface(); 13 | void read(); 14 | void write(const ros::Duration& period); 15 | 16 | private: 17 | std::vector joint_states_; 18 | std::vector pos_handles_; 19 | hardware_interface::JointStateInterface jnt_state_interface_; 20 | hardware_interface::PositionJointInterface jnt_pos_interface_; 21 | double cmd[7]; 22 | double pos[7]; 23 | double vel[7]; 24 | double eff[7]; 25 | }; 26 | #endif // MOCKED_POSITION_HARDWARE_INTERFACE_H 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Travers Rhodes 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 | -------------------------------------------------------------------------------- /include/kinova_gen3_control/fake_kinova_network_connection.h: -------------------------------------------------------------------------------- 1 | #ifndef FAKE_KINOVA_NETWORK_CONNECTION_H 2 | #define FAKE_KINOVA_NETWORK_CONNECTION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // This object, when constructed, creates fake network connections. 12 | // This is a mock of a KinovaNetworkConnection 13 | class FakeKinovaNetworkConnection : public KinovaNetworkConnection 14 | { 15 | public: 16 | FakeKinovaNetworkConnection(); 17 | ~FakeKinovaNetworkConnection(); 18 | // you must construct these methods in your implementation 19 | virtual void BaseSetServoingMode(const Kinova::Api::Base::ServoingModeInformation& servoing_mode); 20 | virtual void ActuatorSetControlMode(const Kinova::Api::ActuatorConfig::ControlModeInformation& servoing_mode, int actuator_device_id); 21 | virtual void CyclicRefresh(const Kinova::Api::BaseCyclic::Command& base_comand); 22 | virtual Kinova::Api::BaseCyclic::Feedback CyclicRefreshFeedback(); 23 | }; 24 | 25 | #endif // FAKE_KINOVA_NETWORK_CONNECTION_H 26 | -------------------------------------------------------------------------------- /include/kinova_gen3_control/kinova_network_connection.h: -------------------------------------------------------------------------------- 1 | #ifndef KINOVA_NETWORK_CONNECTION_H 2 | #define KINOVA_NETWORK_CONNECTION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // This object, when constructed, creates network connections to the KinovaGen3. 13 | // And deletes those network connections when this object is destructed. 14 | // We wrap all these connection functions in their own class so we can easily mock these connections 15 | // when we want to test our controller code without a robot. 16 | class KinovaNetworkConnection 17 | { 18 | public: 19 | virtual ~KinovaNetworkConnection() {}; 20 | // you must construct these methods in your implementation 21 | virtual void BaseSetServoingMode(const Kinova::Api::Base::ServoingModeInformation& servoing_mode) = 0; 22 | virtual void ActuatorSetControlMode(const Kinova::Api::ActuatorConfig::ControlModeInformation& servoing_mode, int actuator_device_id) = 0; 23 | virtual void CyclicRefresh(const Kinova::Api::BaseCyclic::Command& base_comand) = 0; 24 | virtual Kinova::Api::BaseCyclic::Feedback CyclicRefreshFeedback() = 0; 25 | }; 26 | 27 | #endif // KINOVA_NETWORK_CONNECTION_H 28 | -------------------------------------------------------------------------------- /src/mock_position_joint_hardware_interface_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "mock_position_joint_hardware_interface/mocked_position_joint_hardware_interface.h" 4 | 5 | 6 | //https://slaterobots.com/blog/5abd8a1ed4442a651de5cb5b/how-to-implement-ros_control-on-a-custom-robot 7 | int main(int argc, char** argv) 8 | { 9 | ros::init(argc, argv, "mocked_gen3_hardware_interface"); 10 | ros::NodeHandle nh; 11 | // need this in order to have a thread for controller_manager to run on 12 | ros::AsyncSpinner spinner(1); 13 | spinner.start(); 14 | 15 | MockedPositionInterface robot; 16 | 17 | ROS_INFO("Starting controller manager"); 18 | controller_manager::ControllerManager cm(&robot, nh); 19 | ROS_INFO("Controller manager started"); 20 | 21 | ROS_INFO("Entering ros_control loop"); 22 | ros::Rate loop_rate(100); 23 | ros::Time previous = ros::Time::now(); 24 | ros::Time current = ros::Time::now(); 25 | ros::Duration controller_manager_loop_duration; 26 | while (ros::ok()) 27 | { 28 | robot.read(); 29 | current = ros::Time::now(); 30 | controller_manager_loop_duration = current-previous; 31 | cm.update(current, controller_manager_loop_duration); 32 | robot.write(controller_manager_loop_duration); 33 | previous = current; 34 | loop_rate.sleep(); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /src/mock_position_joint_hardware_interface/mocked_position_joint_hardware_interface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // http://wiki.ros.org/ros_control/Tutorials/Create%20your%20own%20hardware%20interface 4 | MockedPositionInterface::MockedPositionInterface() 5 | { 6 | std::vector joint_names = {"joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7"}; 7 | for (int i = 0; i < 7; i++) { 8 | pos[i] = vel[i] = eff[i] = 0; 9 | // connect and register the joint state interface 10 | hardware_interface::JointStateHandle state_handle(joint_names[i], &pos[i], &vel[i], &eff[i]); 11 | joint_states_.push_back(state_handle); 12 | jnt_state_interface_.registerHandle(state_handle); 13 | } 14 | 15 | registerInterface(&jnt_state_interface_); 16 | 17 | for (int i = 0; i < 7; i++) { 18 | // connect and register the joint position interface 19 | cmd[i] = 0; 20 | hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(joint_names[i]), &cmd[i]); 21 | pos_handles_.push_back(pos_handle); 22 | jnt_pos_interface_.registerHandle(pos_handle); 23 | } 24 | 25 | registerInterface(&jnt_pos_interface_); 26 | } 27 | 28 | void MockedPositionInterface::read() { 29 | // do nothing 30 | } 31 | 32 | void MockedPositionInterface::write(const ros::Duration& period) { 33 | for (int i = 0; i < 7; i++) { 34 | pos[i] = cmd[i]; 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /src/kinova_gen3_control/fake_kinova_network_connection.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | FakeKinovaNetworkConnection::FakeKinovaNetworkConnection() 4 | { 5 | ROS_DEBUG("FakeNetwork: Creating connection"); 6 | std::cout << "FakeNetwork: Creating connection" << std::endl; 7 | } 8 | 9 | FakeKinovaNetworkConnection::~FakeKinovaNetworkConnection() 10 | { 11 | ROS_DEBUG("FakeNetwork: Destroying connection"); 12 | std::cout << "FakeNetwork: Destroying connection" << std::endl; 13 | } 14 | 15 | void FakeKinovaNetworkConnection::BaseSetServoingMode(const Kinova::Api::Base::ServoingModeInformation& servoing_mode) 16 | { 17 | ROS_DEBUG("FakeNetwork: Setting Servoing Mode"); 18 | } 19 | 20 | void FakeKinovaNetworkConnection::ActuatorSetControlMode(const Kinova::Api::ActuatorConfig::ControlModeInformation& servoing_mode, int actuator_device_id) 21 | { 22 | ROS_DEBUG("FakeNetwork: Setting Control Mode"); 23 | } 24 | 25 | void FakeKinovaNetworkConnection::CyclicRefresh(const Kinova::Api::BaseCyclic::Command& base_comand) 26 | { 27 | ROS_DEBUG_THROTTLE(1, "FakeNetwork (throttled): Writing Joints"); 28 | } 29 | 30 | Kinova::Api::BaseCyclic::Feedback FakeKinovaNetworkConnection::CyclicRefreshFeedback() 31 | { 32 | ROS_DEBUG_THROTTLE(1, "FakeNetwork (throttled): Reading Joints"); 33 | Kinova::Api::BaseCyclic::Feedback feedback; 34 | for (int i = 0; i < 7; i++) 35 | { 36 | Kinova::Api::BaseCyclic::ActuatorFeedback* actuators = feedback.add_actuators(); 37 | } 38 | return feedback; 39 | } 40 | -------------------------------------------------------------------------------- /include/kinova_gen3_control/kinova_gen3_network_connection.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // This object, when constructed, creates useable network connections to the KinovaGen3. 11 | // And deletes those network connections when this object is destructed. 12 | class KinovaGen3NetworkConnection : public KinovaNetworkConnection 13 | { 14 | public: 15 | KinovaGen3NetworkConnection(); 16 | ~KinovaGen3NetworkConnection(); 17 | 18 | void BaseSetServoingMode(const Kinova::Api::Base::ServoingModeInformation& servoing_mode); 19 | void ActuatorSetControlMode(const Kinova::Api::ActuatorConfig::ControlModeInformation& control_mode, int actuator_device_id); 20 | void CyclicRefresh(const Kinova::Api::BaseCyclic::Command& base_comand); 21 | Kinova::Api::BaseCyclic::Feedback CyclicRefreshFeedback(); 22 | 23 | private: 24 | Kinova::Api::ActuatorConfig::ActuatorConfigClient *actuator_config_; 25 | Kinova::Api::Base::BaseClient *base_; 26 | Kinova::Api::BaseCyclic::BaseCyclicClient *base_cyclic_; 27 | 28 | Kinova::Api::TransportClientTcp *transport_; 29 | Kinova::Api::TransportClientUdp *transport_cyclic_; 30 | Kinova::Api::RouterClient *router_; 31 | Kinova::Api::RouterClient *router_cyclic_; 32 | Kinova::Api::SessionManager *session_manager_; 33 | Kinova::Api::SessionManager *session_manager_cyclic_; 34 | }; 35 | -------------------------------------------------------------------------------- /include/kinova_gen3_control/gazebo_kinova_network_connection.h: -------------------------------------------------------------------------------- 1 | #ifndef GAZEBO_KINOVA_NETWORK_CONNECTION_H 2 | #define GAZEBO_KINOVA_NETWORK_CONNECTION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define GAZEBO_NUMBER_OF_JOINTS 7 14 | 15 | // This object, when constructed, passes through all commands to gazebo joint effort interfaces 16 | // This allows us to test much of our control logic with Gazebo 17 | class GazeboKinovaNetworkConnection : public KinovaNetworkConnection 18 | { 19 | public: 20 | GazeboKinovaNetworkConnection(ros::NodeHandle &nh); 21 | ~GazeboKinovaNetworkConnection(); 22 | // you must construct these methods in your implementation 23 | virtual void BaseSetServoingMode(const Kinova::Api::Base::ServoingModeInformation& servoing_mode); 24 | virtual void ActuatorSetControlMode(const Kinova::Api::ActuatorConfig::ControlModeInformation& servoing_mode, int actuator_device_id); 25 | virtual void CyclicRefresh(const Kinova::Api::BaseCyclic::Command& base_comand); 26 | virtual Kinova::Api::BaseCyclic::Feedback CyclicRefreshFeedback(); 27 | void joint_state_callback(const sensor_msgs::JointState &message); 28 | 29 | private: 30 | ros::Subscriber sub_; 31 | std::vector pub_; 32 | double cmd_[GAZEBO_NUMBER_OF_JOINTS]; 33 | double pos_[GAZEBO_NUMBER_OF_JOINTS]; 34 | double vel_[GAZEBO_NUMBER_OF_JOINTS]; 35 | double eff_[GAZEBO_NUMBER_OF_JOINTS]; 36 | }; 37 | 38 | #endif // GAZEBO_KINOVA_NETWORK_CONNECTION_H 39 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kinova_gen3_control 4 | 0.0.0 5 | The kinova_gen3_control package 6 | 7 | Travers Rhodes 8 | MIT 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | hardware_interface 29 | controller_manager 30 | joint_limits_interface 31 | urdf 32 | roscpp 33 | xacro 34 | ros_controllers 35 | kortex_description 36 | catkin 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /launch/last_joint.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /launch/complete.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | 38 | 39 | 40 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /include/kinova_gen3_control/kinova_gen3_hardware_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef KINOVA_GEN3_HARDWARE_INTERFACE_H 2 | #define KINOVA_GEN3_HARDWARE_INTERFACE_H 3 | 4 | #include 5 | #include "realtime_tools/realtime_publisher.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | 19 | // 1-indexed first and last joint indices 20 | // When not testing individual joints, this should be 1 21 | #define FIRST_JOINT_INDEX 1 22 | // When not testing individual joints, this should be 7 23 | #define NUMBER_OF_JOINTS 7 24 | 25 | class KinovaGen3HardwareInterface : public hardware_interface::RobotHW 26 | { 27 | public: 28 | KinovaGen3HardwareInterface( 29 | std::vector joint_names, 30 | std::vector limits, 31 | std::shared_ptr network_connection, 32 | ros::NodeHandle &nh); 33 | ~KinovaGen3HardwareInterface(); 34 | void write(const ros::Duration& period); 35 | void read(); 36 | 37 | private: 38 | void publish_throttled_debug_message(int severity, std::vector before_limiting); 39 | std::shared_ptr network_connection_; 40 | std::vector joint_names_; 41 | std::vector limits_; 42 | // explicitly save pointers to these effort handles in a vector 43 | // since I think this will prevent some valgrind errors I was seeing in testing 44 | std::vector jnt_state_handles_; 45 | std::vector eff_handles_; 46 | hardware_interface::JointStateInterface jnt_state_interface_; 47 | hardware_interface::EffortJointInterface jnt_eff_interface_; 48 | joint_limits_interface::EffortJointSaturationInterface jnt_eff_limit_interface_; 49 | // no need to re-create this object every time it's used 50 | Kinova::Api::BaseCyclic::Feedback base_feedback_; 51 | double cmd_[NUMBER_OF_JOINTS]; 52 | double pos_[NUMBER_OF_JOINTS]; 53 | double vel_[NUMBER_OF_JOINTS]; 54 | double eff_[NUMBER_OF_JOINTS]; 55 | double curr_[NUMBER_OF_JOINTS]; 56 | // https://github.com/Kinovarobotics/kortex/issues/165#issuecomment-1563424647 57 | // Divide desired torque by this number to get a current to send to motor. 58 | float curr_to_torque_factor_[7] = { 59 | 7, 60 | 7, 61 | 7, 62 | 7, 63 | 7, 64 | 7, 65 | 7 66 | }; 67 | // For debugging, periodically log the actual commands we send to the kinova arm 68 | boost::shared_ptr> realtime_pub_; 69 | // send a debugging message every write 10 loops. 70 | // use this counter to count how many since last publication. 71 | // if last publish was NOT because of an error/warning 72 | // and the new message is because of an error/warning 73 | // then you can publish the new message regardless of counter 74 | int debug_msg_counter_ = 0; 75 | int last_publish_severity_ = 0; 76 | }; 77 | #endif // KINOVA_GEN3_HARDWARE_INTERFACE_H 78 | -------------------------------------------------------------------------------- /src/kinova_gen3_control/gazebo_kinova_network_connection.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "angles/angles.h" 3 | 4 | void GazeboKinovaNetworkConnection::joint_state_callback(const sensor_msgs::JointState& message) 5 | { 6 | // In Gazebo, the first joint is the finger joint 7 | int has_gripper = 0; 8 | if (message.name[0] != "joint_1") { 9 | has_gripper = 1; 10 | } 11 | // and we want to mimic the weird negative effort that kinova does, too (sigh) 12 | // and we want to mimic the fact that kinova does degrees too. (DOUBLE sigh) 13 | for (int i = 0; i < GAZEBO_NUMBER_OF_JOINTS; i++) { 14 | pos_[i] = angles::to_degrees(message.position[i+has_gripper]); 15 | vel_[i] = angles::to_degrees(message.velocity[i+has_gripper]); 16 | eff_[i] = -message.effort[i+has_gripper]; 17 | } 18 | } 19 | 20 | GazeboKinovaNetworkConnection::GazeboKinovaNetworkConnection(ros::NodeHandle &nh) 21 | { 22 | ROS_INFO("GazeboNetwork: Creating connection"); 23 | sub_ = nh.subscribe("/my_gen3/joint_states", 1, &GazeboKinovaNetworkConnection::joint_state_callback, this); 24 | for (int i = 0; i < GAZEBO_NUMBER_OF_JOINTS; i++) { 25 | std::string joint_topic = "/my_gen3/joint_" + std::to_string(i+1) + "_effort_controller/command"; 26 | uint32_t queue_size = 1; 27 | bool latch = true; 28 | pub_.push_back(nh.advertise(joint_topic, queue_size, latch)); 29 | } 30 | // before joint_states publish, be sure to initialize eff_ to 0's 31 | for (int i = 0; i < GAZEBO_NUMBER_OF_JOINTS; i++) { 32 | eff_[i] = 0; 33 | } 34 | // wait until joint_states is publishing before allowing the rest of the code to continue 35 | ros::topic::waitForMessage("/my_gen3/joint_states"); 36 | ros::topic::waitForMessage("/my_gen3/joint_states"); 37 | } 38 | 39 | GazeboKinovaNetworkConnection::~GazeboKinovaNetworkConnection() 40 | { 41 | ROS_INFO("GazeboNetwork: Destroying connection"); 42 | } 43 | 44 | void GazeboKinovaNetworkConnection::BaseSetServoingMode(const Kinova::Api::Base::ServoingModeInformation& servoing_mode) 45 | { 46 | ROS_INFO("GazeboNetwork: Setting Servoing Mode"); 47 | } 48 | 49 | void GazeboKinovaNetworkConnection::ActuatorSetControlMode(const Kinova::Api::ActuatorConfig::ControlModeInformation& servoing_mode, int actuator_device_id) 50 | { 51 | ROS_INFO("GazeboNetwork: Setting Control Mode"); 52 | } 53 | 54 | void GazeboKinovaNetworkConnection::CyclicRefresh(const Kinova::Api::BaseCyclic::Command& base_command) 55 | { 56 | ROS_DEBUG_THROTTLE(1, "GazeboNetwork (throttled): Writing Effort Commands"); 57 | for (int i = 0; i < GAZEBO_NUMBER_OF_JOINTS; i++) { 58 | std_msgs::Float64 eff_cmd; 59 | eff_cmd.data = base_command.actuators()[i].torque_joint(); 60 | pub_[i].publish(eff_cmd); 61 | } 62 | } 63 | 64 | Kinova::Api::BaseCyclic::Feedback GazeboKinovaNetworkConnection::CyclicRefreshFeedback() 65 | { 66 | ROS_DEBUG_THROTTLE(1,"GazeboNetwork (throttled): Reading Joints"); 67 | Kinova::Api::BaseCyclic::Feedback feedback; 68 | for (int i = 0; i < 7; i++) 69 | { 70 | Kinova::Api::BaseCyclic::ActuatorFeedback* actuator = feedback.add_actuators(); 71 | } 72 | for (int i = 0; i < 7; i++) 73 | { 74 | feedback.mutable_actuators(i)->set_position(pos_[i]); 75 | feedback.mutable_actuators(i)->set_velocity(vel_[i]); 76 | feedback.mutable_actuators(i)->set_torque(eff_[i]); 77 | } 78 | ROS_DEBUG_THROTTLE(1,"GazeboNetwork (throttled): Done reading Joints"); 79 | return feedback; 80 | } 81 | -------------------------------------------------------------------------------- /src/kinova_gen3_control/kinova_gen3_network_connection.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define IP_ADDRESS "192.168.1.10" 4 | #define PORT 10000 5 | #define CYCLIC_PORT 10001 6 | 7 | KinovaGen3NetworkConnection::KinovaGen3NetworkConnection() 8 | { 9 | auto error_callback = [](Kinova::Api::KError err){ cout << "_________ callback error _________" << err.toString(); }; 10 | transport_ = new Kinova::Api::TransportClientTcp(); 11 | transport_cyclic_ = new Kinova::Api::TransportClientUdp(); 12 | router_ = new Kinova::Api::RouterClient(transport_, error_callback); 13 | router_cyclic_ = new Kinova::Api::RouterClient(transport_cyclic_, error_callback); 14 | session_manager_ = new Kinova::Api::SessionManager(router_); 15 | session_manager_cyclic_ = new Kinova::Api::SessionManager(router_cyclic_); 16 | 17 | 18 | // Set session data connection information 19 | auto create_session_info = Kinova::Api::Session::CreateSessionInfo(); 20 | create_session_info.set_username("admin"); 21 | create_session_info.set_password("admin"); 22 | create_session_info.set_session_inactivity_timeout(60000); // (milliseconds) 23 | create_session_info.set_connection_inactivity_timeout(2000); // (milliseconds) 24 | 25 | 26 | std::cout << "Connectng TCP and UDP clients to " << IP_ADDRESS << std::endl; 27 | transport_->connect(IP_ADDRESS, PORT); 28 | transport_cyclic_->connect(IP_ADDRESS, CYCLIC_PORT); 29 | 30 | // Session manager service wrapper 31 | std::cout << "Creating session for communication" << std::endl; 32 | session_manager_->CreateSession(create_session_info); 33 | session_manager_cyclic_->CreateSession(create_session_info); 34 | std::cout << "Sessions created" << std::endl; 35 | 36 | // create these objects after session initialized 37 | actuator_config_ = new Kinova::Api::ActuatorConfig::ActuatorConfigClient(router_); 38 | base_ = new Kinova::Api::Base::BaseClient(router_); 39 | base_cyclic_ = new Kinova::Api::BaseCyclic::BaseCyclicClient(router_cyclic_); 40 | } 41 | 42 | KinovaGen3NetworkConnection::~KinovaGen3NetworkConnection() 43 | { 44 | std::cout << "Closing sessions" << std::endl; 45 | // Close API session 46 | session_manager_->CloseSession(); 47 | session_manager_cyclic_->CloseSession(); 48 | 49 | std::cout << "Deactivating router" << std::endl; 50 | // Deactivate the router and cleanly disconnect from the transport object 51 | router_->SetActivationStatus(false); 52 | transport_->disconnect(); 53 | router_cyclic_->SetActivationStatus(false); 54 | transport_cyclic_->disconnect(); 55 | 56 | delete transport_; 57 | delete transport_cyclic_; 58 | delete router_; 59 | delete router_cyclic_; 60 | delete session_manager_; 61 | delete session_manager_cyclic_; 62 | delete actuator_config_; 63 | delete base_; 64 | delete base_cyclic_; 65 | 66 | std::cout << "Done cleaning up network connection to Kinova" << std::endl; 67 | } 68 | 69 | void KinovaGen3NetworkConnection::BaseSetServoingMode(const Kinova::Api::Base::ServoingModeInformation& servoing_mode) 70 | { 71 | base_->SetServoingMode(servoing_mode); 72 | } 73 | 74 | void KinovaGen3NetworkConnection::ActuatorSetControlMode(const Kinova::Api::ActuatorConfig::ControlModeInformation& servoing_mode, int actuator_device_id) 75 | { 76 | actuator_config_->SetControlMode(servoing_mode, actuator_device_id); 77 | } 78 | 79 | void KinovaGen3NetworkConnection::CyclicRefresh(const Kinova::Api::BaseCyclic::Command& base_command) 80 | { 81 | base_cyclic_->Refresh(base_command); 82 | } 83 | 84 | Kinova::Api::BaseCyclic::Feedback KinovaGen3NetworkConnection::CyclicRefreshFeedback() 85 | { 86 | return(base_cyclic_->RefreshFeedback()); 87 | } 88 | -------------------------------------------------------------------------------- /config/kinova_gen3_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Publish all joint states ----------------------------------- 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 50 5 | 6 | # publish them super rapidly if you're using gazebo simulator 7 | high_freq_joint_state_controller: 8 | type: joint_state_controller/JointStateController 9 | publish_rate: 2000 10 | 11 | mocked_joint_trajectory_controller: 12 | type: position_controllers/JointTrajectoryController 13 | joints: 14 | - joint_1 15 | - joint_2 16 | - joint_3 17 | - joint_4 18 | - joint_5 19 | - joint_6 20 | - joint_7 21 | 22 | gen3_joint_trajectory_controller: 23 | type: effort_controllers/JointTrajectoryController 24 | joints: 25 | - joint_1 26 | - joint_2 27 | - joint_3 28 | - joint_4 29 | - joint_5 30 | - joint_6 31 | - joint_7 32 | constraints: 33 | goal_time: 1.0 34 | stopped_velocity_tolerance: 0.5 35 | stop_trajectory_duration: 1.0 36 | state_publish_rate: 25 37 | action_monitor_rate: 25 38 | gains: 39 | joint_1: {p: 800.0, i: 0.0, d: 20.0, i_clamp_min: -15.0, i_clamp_max: 15.0, antiwindup: true} 40 | joint_2: {p: 2000.0, i: 0.0, d: 40.0, i_clamp_min: -30.0, i_clamp_max: 30.0, antiwindup: true} 41 | joint_3: {p: 800.0, i: 0.0, d: 20.0, i_clamp_min: -15.0, i_clamp_max: 15.0, antiwindup: true} 42 | joint_4: {p: 800.0, i: 0.0, d: 0.0, i_clamp_min: -15.0, i_clamp_max: 15.0, antiwindup: true} 43 | joint_5: {p: 320.0, i: 0.0, d: 0.0, i_clamp_min: -15.0, i_clamp_max: 15.0, antiwindup: true} 44 | joint_6: {p: 320.0, i: 0.0, d: 0.0, i_clamp_min: -5.0, i_clamp_max: 5.0, antiwindup: true} 45 | joint_7: {p: 320.0, i: 0.0, d: 0.0, i_clamp_min: -1.0, i_clamp_max: 1.0, antiwindup: true} 46 | 47 | joint_1_effort_controller: 48 | joint: joint_1 49 | type: effort_controllers/JointEffortController 50 | joint_2_effort_controller: 51 | joint: joint_2 52 | type: effort_controllers/JointEffortController 53 | joint_3_effort_controller: 54 | joint: joint_3 55 | type: effort_controllers/JointEffortController 56 | joint_4_effort_controller: 57 | joint: joint_4 58 | type: effort_controllers/JointEffortController 59 | joint_5_effort_controller: 60 | joint: joint_5 61 | type: effort_controllers/JointEffortController 62 | joint_6_effort_controller: 63 | joint: joint_6 64 | type: effort_controllers/JointEffortController 65 | joint_7_effort_controller: 66 | joint: joint_7 67 | type: effort_controllers/JointEffortController 68 | 69 | joint_1_position_controller: 70 | joint: joint_1 71 | pid: 72 | p: 20.0 73 | i: 0.0 74 | d: 1.0 75 | type: effort_controllers/JointPositionController 76 | 77 | joint_2_position_controller: 78 | joint: joint_2 79 | pid: 80 | p: 100.0 81 | i: 0.0 82 | d: 4.0 83 | type: effort_controllers/JointPositionController 84 | 85 | joint_3_position_controller: 86 | joint: joint_3 87 | pid: 88 | p: 20.0 89 | i: 0.0 90 | d: 1.0 91 | type: effort_controllers/JointPositionController 92 | 93 | joint_4_position_controller: 94 | joint: joint_4 95 | pid: 96 | p: 20.0 97 | i: 0.0 98 | d: 0.0 99 | type: effort_controllers/JointPositionController 100 | 101 | joint_5_position_controller: 102 | joint: joint_5 103 | pid: 104 | p: 4.0 105 | i: 0.0 106 | d: 0.0 107 | i_clamp: 200 108 | type: effort_controllers/JointPositionController 109 | 110 | joint_6_position_controller: 111 | joint: joint_6 112 | pid: 113 | p: 4.0 114 | i: 0.0 115 | d: 0.0 116 | i_clamp: 200 117 | type: effort_controllers/JointPositionController 118 | 119 | joint_7_position_controller: 120 | joint: joint_7 121 | pid: 122 | p: 4.0 123 | i: 0.0 124 | d: 0.0 125 | i_clamp: 200 126 | type: effort_controllers/JointPositionController 127 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # kinova_gen3_control 2 | Open-source ros_control hardware_interface::RobotHW (hardware_interface::EffortJointInterface and hardware_interface::JointStateInterface) implementation for the Kinova Gen3 arm 3 | 4 | ## Installation 5 | ### Clone this repository to your catkin workspace. 6 | ``` 7 | cd ~ 8 | mkdir -p my_catkin_ws/src 9 | cd my_catkin_ws/src 10 | git clone https://github.com/travers-rhodes/kinova_gen3_control.git 11 | ``` 12 | ### Clone the `kortex_description` package 13 | You can clone this from `https://github.com/Kinovarobotics/ros_kortex.git` (and just ignore the rest of the packages that are bundled with that). 14 | 15 | ``` 16 | cd ~/my_catkin_ws/src 17 | git clone https://github.com/Kinovarobotics/ros_kortex.git 18 | ``` 19 | 20 | 21 | ### Make sure you have all required dependencies installed 22 | ``` 23 | cd ~/my_catkin_ws/src 24 | source /opt/ros/melodic/setup.bash 25 | rosdep init 26 | rosdep update 27 | rosdep install --from-paths . --ignore-src -r 28 | ``` 29 | 30 | ### Include the Kinova API 31 | Please download the latest kortex_api zip folder listed at https://github.com/Kinovarobotics/kortex 32 | 33 | Then unzip that file, then run something like the below to copy the needed lib and include files into new directories called kinova_gen3_control/kortex_api/linux_gcc_x86-64/lib and kinova_gen3_control/kortex_api/linux_gcc_x86-64/include 34 | ``` 35 | cd ~/Downloads 36 | wget https://artifactory.kinovaapps.com:443/artifactory/generic-public/kortex/API/2.3.0/linux_x86-64_x86_gcc.zip 37 | unzip linux_x86-64_x86_gcc.zip -d linux_gcc_x86-64 38 | cp -r ~/Downloads/linux_gcc_x86-64/* ~/my_catkin_ws/src/kinova_gen3_control/kortex_api/ 39 | ``` 40 | 41 | ### Build the packages we care about (blacklist all the extra packages bundled with ros_kortex) 42 | ``` 43 | cd ~/my_catkin_ws 44 | catkin config --blacklist kortex_control kortex_driver kortex_examples gazebo_version_helpers gazebo_grasp_plugin roboticsgroup_gazebo_plugins gen3_lite_gen3_lite_2f_move_it_config gen3_move_it_config gen3_robotiq_2f_140_move_it_config roboticsgroup_upatras_gazebo_plugins 45 | source /opt/ros/melodic/setup.bash 46 | catkin build 47 | ``` 48 | 49 | ## TL;DR Installation 50 | Together, all the install instructions above are 51 | ``` 52 | cd ~ 53 | mkdir -p my_catkin_ws/src 54 | cd ~/my_catkin_ws/src 55 | git clone https://github.com/travers-rhodes/kinova_gen3_control.git 56 | git clone https://github.com/Kinovarobotics/ros_kortex.git 57 | source /opt/ros/melodic/setup.bash 58 | rosdep init 59 | rosdep update 60 | rosdep install --from-paths . --ignore-src -r 61 | cd ~/Downloads 62 | rm -f linux_x86-64_x86_gcc.zip 63 | rm -rf linux_gcc_x86-64 64 | wget https://artifactory.kinovaapps.com:443/artifactory/generic-public/kortex/API/2.3.0/linux_x86-64_x86_gcc.zip 65 | unzip linux_x86-64_x86_gcc.zip -d linux_gcc_x86-64 66 | cp -r ~/Downloads/linux_gcc_x86-64/* ~/my_catkin_ws/src/kinova_gen3_control/kortex_api/ 67 | cd ~/my_catkin_ws 68 | catkin config --blacklist kortex_control kortex_driver kortex_examples kortex_gazebo gazebo_version_helpers gazebo_grasp_plugin roboticsgroup_gazebo_plugins gen3_lite_gen3_lite_2f_move_it_config gen3_move_it_config gen3_robotiq_2f_140_move_it_config roboticsgroup_upatras_gazebo_plugins 69 | source /opt/ros/melodic/setup.bash 70 | catkin build 71 | source devel/setup.bash 72 | ``` 73 | 74 | 75 | ## Usage 76 | `roslaunch kinova_gen3_control default.launch` will 77 | 78 | 1. Connect to the Kinova Gen3 arm (assumed to be at IP address `192.168.1.10`) using a TCP Transport Client on port 10000 79 | 1. Connect to the Kinova Gen3 arm using a UDP Transport Client on port 10001 80 | 1. Put the Robot in low-level control loop and load and start a `hardware_interface::EffortJointInterface` for each joint to send effort commands to each joint. 81 | 1. Load and start a `effort_controllers::JointPositionController` for each joint, which provides a ROS topic `/joint_X_position_controller/command` for each joint. 82 | 1. Start a `controller_manager::ControllerManager` that can be used to switch between high-level `ros_control::Controller`s. 83 | For example, to switch to a joint_trajectory_controller (the information for which is already loaded by `default.launch`) you can run the following from the command line: 84 | ``` 85 | rosservice call controller_manager/switch_controller "start_controllers: ['gen3_joint_trajectory_controller'] 86 | stop_controllers: ['joint_1_position_controller', 87 | 'joint_2_position_controller', 88 | 'joint_3_position_controller', 89 | 'joint_4_position_controller', 90 | 'joint_5_position_controller', 91 | 'joint_6_position_controller', 92 | 'joint_7_position_controller' 93 | ] 94 | strictness: 1 95 | start_asap: false 96 | timeout: 0.0" 97 | ok: True 98 | ``` 99 | 100 | ## Debugging 101 | 102 | If you want to test this package without a network connection to a robot, you can run 103 | `roslaunch kinova_gen3_control default.launch fake_connection:=true` 104 | 105 | ## Realtime 106 | 107 | Ideally, this node should be run on a realtime operating system, to maintain consistent timings between commands to the robot. 108 | That's why the launch file runs this node on the `machine="control-computer"`. By default the `control-computer` machine is defined to be `localhost`, 109 | but you can alternatively have this launch file run the node on a different machine by follwing the instructions at http://wiki.ros.org/roslaunch/XML/machine. 110 | 111 | If you want the `control-computer` to be a different machine from `localhost`, you should set the arg `use_local_control_computer` to `false`. 112 | 113 | 114 | -------------------------------------------------------------------------------- /src/kinova_gen3_hardware_interface_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include "std_msgs/Float32.h" 10 | 11 | #include "kinova_gen3_control/kinova_gen3_hardware_interface.h" 12 | #include "kinova_gen3_control/kinova_gen3_network_connection.h" 13 | #include "kinova_gen3_control/fake_kinova_network_connection.h" 14 | #include "kinova_gen3_control/gazebo_kinova_network_connection.h" 15 | 16 | //https://slaterobots.com/blog/5abd8a1ed4442a651de5cb5b/how-to-implement-ros_control-on-a-custom-robot 17 | int main(int argc, char** argv) 18 | { 19 | ros::init(argc, argv, "kinova_gen3_hardware_interface"); 20 | ros::NodeHandle private_nh("~"); 21 | ros::NodeHandle nh; 22 | ros::AsyncSpinner spinner(1); 23 | spinner.start(); 24 | realtime_tools::RealtimePublisher *realtime_pub = new 25 | realtime_tools::RealtimePublisher(nh, "realtime_loop_rate", 4); 26 | 27 | std::cout << "Parsing URDF" << std::endl; 28 | urdf::Model urdf_robot; 29 | // load urdf from "robot_description" 30 | // TODO don't hard-code this string 31 | urdf_robot.initParam("robot_description"); 32 | 33 | bool fake_connection; 34 | private_nh.getParam("fake_connection", fake_connection); 35 | bool use_gazebo; 36 | private_nh.getParam("use_gazebo", use_gazebo); 37 | 38 | std::shared_ptr network_connection; 39 | if (fake_connection) 40 | { 41 | ROS_INFO("Creating fake network connection"); 42 | network_connection = std::make_shared(); 43 | } 44 | else if (use_gazebo) { 45 | ROS_INFO("Creating Gazebo network connection"); 46 | network_connection = std::make_shared(nh); 47 | } 48 | else 49 | { 50 | ROS_INFO("Creating real network connection"); 51 | network_connection = std::make_shared(); 52 | } 53 | 54 | ROS_INFO("Creating hardware interface"); 55 | std::vector joint_names = { 56 | "joint_1", 57 | "joint_2", 58 | "joint_3", 59 | "joint_4", 60 | "joint_5", 61 | "joint_6", 62 | "joint_7" 63 | }; 64 | std::vector limits_list; 65 | for (int i = 0; i < joint_names.size(); i++) 66 | { 67 | joint_limits_interface::JointLimits limits; 68 | std::shared_ptr urdf_joint = urdf_robot.getJoint(joint_names[i]); 69 | const bool urdf_limits_ok = joint_limits_interface::getJointLimits(urdf_joint, limits); 70 | // Populate joint limits from the ros parameter server 71 | // Limits specified in the parameter server overwrite existing values in 'limits' 72 | // Limits not specified in the parameter server preserve their existing values 73 | const bool rosparam_limits_ok = joint_limits_interface::getJointLimits(joint_names[i], nh, limits); 74 | limits_list.push_back(limits); 75 | } 76 | 77 | KinovaGen3HardwareInterface robot( 78 | joint_names, 79 | limits_list, 80 | network_connection, 81 | nh); 82 | 83 | ROS_INFO("Starting controller manager"); 84 | controller_manager::ControllerManager controller_manager(&robot, nh); 85 | ROS_INFO("Controller manager started"); 86 | 87 | double control_loop_hz; 88 | if (!private_nh.getParam("control_loop_hz", control_loop_hz)) 89 | { 90 | double default_control_loop_hz = 500; 91 | ROS_WARN("No control_loop_hz given in namespace: %s. Defaulting to %f.", 92 | private_nh.getNamespace().c_str(), default_control_loop_hz); 93 | control_loop_hz = default_control_loop_hz; 94 | } 95 | 96 | ros::Duration update_freq = ros::Duration(1.0/control_loop_hz); 97 | ros::Time previous = ros::Time::now(); 98 | ros::Time current = ros::Time::now(); 99 | 100 | while (ros::Time::now().toSec() == 0) 101 | { 102 | ROS_INFO_THROTTLE(0.5, "Waiting for clock to publish"); 103 | update_freq.sleep(); 104 | } 105 | 106 | ROS_INFO("Entering ros_control loop"); 107 | double loop_time_secs = 0; 108 | double read_time_secs = 0; 109 | double update_time_secs = 0; 110 | double write_time_secs = 0; 111 | double sleep_time_secs = 0; 112 | double loop_count_mod_thousand = 0; 113 | ros::Time thousand_loops_start = ros::Time::now(); 114 | ros::Time thousand_loops_end = ros::Time::now(); 115 | ros::Duration controller_manager_loop_duration; 116 | ros::Time start = ros::Time::now(); 117 | ros::Time stop = ros::Time::now(); 118 | current = ros::Time::now(); 119 | previous = current; 120 | // use a Rate loop to only sleep if we're faster than 1kHz (we generally aren't) 121 | ros::Rate loop_rate(update_freq); 122 | loop_rate.sleep(); 123 | while (ros::ok()) 124 | { 125 | start = stop; 126 | robot.read(); 127 | stop = ros::Time::now(); 128 | read_time_secs = (stop-start).toSec(); 129 | 130 | start = stop; 131 | current = ros::Time::now(); 132 | controller_manager_loop_duration = current-previous; 133 | if (controller_manager_loop_duration.toSec() == 0.00) 134 | { 135 | //ROS_ERROR("KinovaGen3HardwareInterfaceNode: The loop duration was zero. I assume this was in simulation and that there's a bug. Total: %f sec. Read: %f; Update: %f; Write: %f; Sleep: %f", 136 | // controller_manager_loop_duration.toSec(), read_time_secs, update_time_secs, write_time_secs, sleep_time_secs); 137 | continue; 138 | } 139 | controller_manager.update(current, controller_manager_loop_duration); 140 | // Warn the user if a loop ever takes 2 milliseconds or more 141 | if (controller_manager_loop_duration.toSec() > 2.0/control_loop_hz) 142 | { 143 | //ROS_WARN_THROTTLE(0.1, "KinovaGen3HardwareInterfaceNode: The update loop took more than twice as long as expected. Total: %f sec. Read: %f; Update: %f; Write: %f; Sleep: %f", 144 | // controller_manager_loop_duration.toSec(), read_time_secs, update_time_secs, write_time_secs, sleep_time_secs); 145 | } 146 | previous = current; 147 | stop = ros::Time::now(); 148 | update_time_secs = (stop-start).toSec(); 149 | 150 | start = stop; 151 | robot.write(controller_manager_loop_duration); 152 | stop = ros::Time::now(); 153 | write_time_secs = (stop-start).toSec(); 154 | 155 | start = stop; 156 | loop_rate.sleep(); 157 | stop = ros::Time::now(); 158 | sleep_time_secs = (stop-start).toSec(); 159 | loop_count_mod_thousand++; 160 | if (loop_count_mod_thousand == 1000) { 161 | loop_count_mod_thousand = 0; 162 | thousand_loops_end = ros::Time::now(); 163 | if (realtime_pub->trylock()){ 164 | realtime_pub->msg_.data = 1000/(thousand_loops_end-thousand_loops_start).toSec(); 165 | realtime_pub->unlockAndPublish(); 166 | } 167 | thousand_loops_start = ros::Time::now(); 168 | } 169 | } 170 | } 171 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | # We don't use the VERSION variables for our package... 4 | # https://github.com/google/googletest/pull/2264/commits/97d8c47df92b7514dc33d361c21a4d8965f10954 5 | if (POLICY CMP0048) 6 | cmake_policy(SET CMP0048 NEW) 7 | endif (POLICY CMP0048) 8 | 9 | project(kinova_gen3_control) 10 | 11 | ## Compile as C++11, supported in ROS Kinetic and newer 12 | set (CMAKE_CXX_STANDARD 11) 13 | 14 | ## Find catkin macros and libraries 15 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 16 | ## is used, also find other catkin packages 17 | find_package(catkin REQUIRED 18 | urdf 19 | hardware_interface 20 | joint_limits_interface 21 | controller_manager 22 | angles 23 | message_generation 24 | roscpp) 25 | 26 | set(KORTEX_DIR "${PROJECT_SOURCE_DIR}/kortex_api/") 27 | if(NOT EXISTS ${KORTEX_DIR}/lib) 28 | message(FATAL_ERROR "Please download the kortex_api and copy include and lib files to: ${KORTEX_DIR} More information is available in the kinova_gen3_control/kortex_api/README.txt file") 29 | endif() 30 | # in order to compile kortex headers, you need to set the architecture through this variable 31 | add_definitions(-D_OS_UNIX) 32 | include_directories(${KORTEX_DIR}include) 33 | include_directories(${KORTEX_DIR}include/client) 34 | include_directories(${KORTEX_DIR}include/common) 35 | include_directories(${KORTEX_DIR}include/messages) 36 | include_directories(${KORTEX_DIR}include/client_stubs) 37 | 38 | 39 | 40 | ## Uncomment this if the package has a setup.py. This macro ensures 41 | ## modules and global scripts declared therein get installed 42 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 43 | # catkin_python_setup() 44 | 45 | ################################################ 46 | ## Declare ROS messages, services and actions ## 47 | ################################################ 48 | 49 | ## To declare and build messages, services or actions from within this 50 | ## package, follow these steps: 51 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 52 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 53 | ## * In the file package.xml: 54 | ## * add a build_depend tag for "message_generation" 55 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 56 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 57 | ## but can be declared for certainty nonetheless: 58 | ## * add a exec_depend tag for "message_runtime" 59 | ## * In this file (CMakeLists.txt): 60 | ## * add "message_generation" and every package in MSG_DEP_SET to 61 | ## find_package(catkin REQUIRED COMPONENTS ...) 62 | ## * add "message_runtime" and every package in MSG_DEP_SET to 63 | ## catkin_package(CATKIN_DEPENDS ...) 64 | ## * uncomment the add_*_files sections below as needed 65 | ## and list every .msg/.srv/.action file to be processed 66 | ## * uncomment the generate_messages entry below 67 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 68 | 69 | ## Generate messages in the 'msg' folder 70 | add_message_files( 71 | FILES 72 | EffortCommand.msg 73 | ) 74 | 75 | ## Generate services in the 'srv' folder 76 | # add_service_files( 77 | # FILES 78 | # Service1.srv 79 | # Service2.srv 80 | # ) 81 | 82 | ## Generate actions in the 'action' folder 83 | # add_action_files( 84 | # FILES 85 | # Action1.action 86 | # Action2.action 87 | # ) 88 | 89 | # Generate added messages and services with any dependencies listed here 90 | generate_messages( 91 | DEPENDENCIES 92 | ) 93 | 94 | ################################################ 95 | ## Declare ROS dynamic reconfigure parameters ## 96 | ################################################ 97 | 98 | ## To declare and build dynamic reconfigure parameters within this 99 | ## package, follow these steps: 100 | ## * In the file package.xml: 101 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 102 | ## * In this file (CMakeLists.txt): 103 | ## * add "dynamic_reconfigure" to 104 | ## find_package(catkin REQUIRED COMPONENTS ...) 105 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 106 | ## and list every .cfg file to be processed 107 | 108 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 109 | # generate_dynamic_reconfigure_options( 110 | # cfg/DynReconf1.cfg 111 | # cfg/DynReconf2.cfg 112 | # ) 113 | 114 | ################################### 115 | ## catkin specific configuration ## 116 | ################################### 117 | ## The catkin_package macro generates cmake config files for your package 118 | ## Declare things to be passed to dependent projects 119 | ## INCLUDE_DIRS: uncomment this if your package contains header files 120 | ## LIBRARIES: libraries you create in this project that dependent projects also need 121 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 122 | ## DEPENDS: system dependencies of this project that dependent projects also need 123 | catkin_package( 124 | INCLUDE_DIRS include 125 | ${KORTEX_DIR}include 126 | ${KORTEX_DIR}include/client 127 | ${KORTEX_DIR}include/common 128 | ${KORTEX_DIR}include/messages 129 | ${KORTEX_DIR}include/client_stubs 130 | LIBRARIES kinova_gen3_control mock_position_joint_hardware_interface 131 | ${KORTEX_DIR}lib/release/libKortexApiCpp.a 132 | # CATKIN_DEPENDS other_catkin_pkg 133 | # DEPENDS system_lib 134 | ) 135 | 136 | ########### 137 | ## Build ## 138 | ########### 139 | 140 | ## Specify additional locations of header files 141 | ## Your package locations should be listed before other locations 142 | include_directories( 143 | include 144 | ${catkin_INCLUDE_DIRS} 145 | ) 146 | 147 | ## Declare a C++ library 148 | add_library(${PROJECT_NAME} 149 | src/${PROJECT_NAME}/kinova_gen3_hardware_interface.cpp 150 | src/${PROJECT_NAME}/kinova_gen3_network_connection.cpp 151 | src/${PROJECT_NAME}/fake_kinova_network_connection.cpp 152 | src/${PROJECT_NAME}/gazebo_kinova_network_connection.cpp 153 | ) 154 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 155 | 156 | # Declare a C++ library 157 | add_library(mock_position_joint_hardware_interface 158 | src/mock_position_joint_hardware_interface/mocked_position_joint_hardware_interface.cpp 159 | ) 160 | add_dependencies(mock_position_joint_hardware_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 161 | 162 | add_executable(mock_position_joint_hardware_interface_node src/mock_position_joint_hardware_interface_node.cpp) 163 | add_dependencies(mock_position_joint_hardware_interface_node mock_position_joint_hardware_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 164 | target_link_libraries(mock_position_joint_hardware_interface_node 165 | mock_position_joint_hardware_interface 166 | ${catkin_LIBRARIES} 167 | ) 168 | 169 | ## Declare a C++ executable 170 | ## With catkin_make all packages are built within a single CMake context 171 | ## The recommended prefix ensures that target names across packages don't collide 172 | add_executable(${PROJECT_NAME}_node src/kinova_gen3_hardware_interface_node.cpp) 173 | 174 | ## Add cmake target dependencies of the executable 175 | ## same as for the library above 176 | add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 177 | 178 | # Specify libraries to link a library or executable target against 179 | target_link_libraries(${PROJECT_NAME}_node 180 | ${PROJECT_NAME} 181 | ${KORTEX_DIR}lib/release/libKortexApiCpp.a 182 | ${catkin_LIBRARIES} 183 | ) 184 | 185 | ############# 186 | ## Install ## 187 | ############# 188 | 189 | # all install targets should use catkin DESTINATION variables 190 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 191 | 192 | ## Mark executable scripts (Python etc.) for installation 193 | ## in contrast to setup.py, you can choose the destination 194 | # install(PROGRAMS 195 | # scripts/my_python_script 196 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 197 | # ) 198 | 199 | ## Mark executables for installation 200 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 201 | # install(TARGETS ${PROJECT_NAME}_node 202 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 203 | # ) 204 | 205 | ## Mark libraries for installation 206 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 207 | # install(TARGETS ${PROJECT_NAME} 208 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 209 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 210 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 211 | # ) 212 | 213 | ## Mark cpp header files for installation 214 | # install(DIRECTORY include/${PROJECT_NAME}/ 215 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 216 | # FILES_MATCHING PATTERN "*.h" 217 | # PATTERN ".svn" EXCLUDE 218 | # ) 219 | 220 | ## Mark other files for installation (e.g. launch and bag files, etc.) 221 | # install(FILES 222 | # # myfile1 223 | # # myfile2 224 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 225 | # ) 226 | 227 | ############# 228 | ## Testing ## 229 | ############# 230 | 231 | ## Add gtest based cpp test target and link libraries 232 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_kinova_gen3_control.cpp) 233 | # if(TARGET ${PROJECT_NAME}-test) 234 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 235 | # endif() 236 | 237 | ## Add folders to be run by python nosetests 238 | # catkin_add_nosetests(test) 239 | -------------------------------------------------------------------------------- /src/kinova_gen3_control/kinova_gen3_hardware_interface.cpp: -------------------------------------------------------------------------------- 1 | #include "kinova_gen3_control/kinova_gen3_hardware_interface.h" 2 | #include "angles/angles.h" 3 | 4 | void 5 | InitializeLowLevelControl(std::shared_ptr network_connection) 6 | { 7 | std::cout << "Initialize low-level control" << std::endl; 8 | 9 | auto servoing_mode = Kinova::Api::Base::ServoingModeInformation(); 10 | servoing_mode.set_servoing_mode(Kinova::Api::Base::ServoingMode::LOW_LEVEL_SERVOING); 11 | auto control_mode_message = Kinova::Api::ActuatorConfig::ControlModeInformation(); 12 | control_mode_message.set_control_mode(Kinova::Api::ActuatorConfig::ControlMode::CURRENT); 13 | try 14 | { 15 | ROS_INFO("Set the servoing mode"); 16 | // Set the base in low-level servoing mode 17 | network_connection->BaseSetServoingMode(servoing_mode); 18 | // Set last actuator in torque mode now that the command is equal to measure 19 | for (int i = 0; i < NUMBER_OF_JOINTS; i++) 20 | { 21 | // actuator_device_id is one-indexed 22 | int actuator_device_id_offset = FIRST_JOINT_INDEX; 23 | ROS_INFO("Set actuator %d control mode to low-level servoing", actuator_device_id_offset + i); 24 | network_connection->ActuatorSetControlMode(control_mode_message, actuator_device_id_offset + i); 25 | } 26 | } 27 | catch (Kinova::Api::KDetailedException& ex) 28 | { 29 | std::cout << "API error: " << ex.what() << std::endl; 30 | throw; 31 | } 32 | catch (std::runtime_error& ex2) 33 | { 34 | std::cout << "Error: " << ex2.what() << std::endl; 35 | throw; 36 | } 37 | ROS_INFO("Low-level control initialized"); 38 | } 39 | 40 | void 41 | EndLowLevelControl(std::shared_ptr network_connection) 42 | { 43 | ROS_INFO("End low-level control"); 44 | std::cout << "End low-level control" << std::endl; 45 | auto servoing_mode = Kinova::Api::Base::ServoingModeInformation(); 46 | servoing_mode.set_servoing_mode(Kinova::Api::Base::ServoingMode::SINGLE_LEVEL_SERVOING); 47 | auto control_mode_message = Kinova::Api::ActuatorConfig::ControlModeInformation(); 48 | control_mode_message.set_control_mode(Kinova::Api::ActuatorConfig::ControlMode::POSITION); 49 | try 50 | { 51 | // Set the base back to regular servoing mode 52 | network_connection->BaseSetServoingMode(servoing_mode); 53 | // Set the actuators to position mode 54 | for (int i = 0; i < NUMBER_OF_JOINTS; i++) 55 | { 56 | // actuator_device_id is one-indexed 57 | int actuator_device_id_offset = FIRST_JOINT_INDEX; 58 | ROS_INFO("Set actuator %d control mode to position control", actuator_device_id_offset + i); 59 | network_connection->ActuatorSetControlMode(control_mode_message, actuator_device_id_offset + i); 60 | } 61 | } 62 | catch (Kinova::Api::KDetailedException& ex) 63 | { 64 | std::cout << "API error: " << ex.what() << std::endl; 65 | throw; 66 | } 67 | catch (std::runtime_error& ex2) 68 | { 69 | std::cout << "Error: " << ex2.what() << std::endl; 70 | throw; 71 | } 72 | ROS_INFO("Low-level control ended"); 73 | } 74 | 75 | KinovaGen3HardwareInterface::KinovaGen3HardwareInterface( 76 | std::vector joint_names, 77 | std::vector limits, 78 | std::shared_ptr network_connection, 79 | ros::NodeHandle &nh) : joint_names_(joint_names), limits_(limits) 80 | { 81 | realtime_pub_.reset(new realtime_tools::RealtimePublisher(nh, "low_level_effort_commands", 1)); 82 | 83 | network_connection_ = network_connection; 84 | 85 | InitializeLowLevelControl(network_connection_); 86 | 87 | ROS_INFO("Register hardware interface"); 88 | 89 | jnt_state_handles_.resize(NUMBER_OF_JOINTS); 90 | int array_index_offset = FIRST_JOINT_INDEX - 1; 91 | // connect and register the joint state interfaces 92 | for (int i = 0; i < NUMBER_OF_JOINTS; i++) 93 | { 94 | hardware_interface::JointStateHandle state_handle(joint_names_[i + array_index_offset], &pos_[i], &vel_[i], &eff_[i]); 95 | jnt_state_handles_[i] = state_handle; 96 | jnt_state_interface_.registerHandle(state_handle); 97 | } 98 | 99 | registerInterface(&jnt_state_interface_); 100 | 101 | eff_handles_.resize(NUMBER_OF_JOINTS); 102 | // connect and register the joint position interfaces 103 | for (int i = 0; i < NUMBER_OF_JOINTS; i++) 104 | { 105 | // note how a hardware_interface::JointHandle requires a hardware_interface::JointStateHandle in its constructor 106 | // in case you're wondering how this for loop relates to the for loop above 107 | hardware_interface::JointHandle eff_handle(jnt_state_interface_.getHandle(joint_names_[i + array_index_offset]), &cmd_[i]); 108 | eff_handles_[i] = eff_handle; 109 | jnt_eff_interface_.registerHandle(eff_handle); 110 | } 111 | 112 | 113 | // set up the joint limiting interface so we can use it in "write" 114 | for (int i = 0; i < NUMBER_OF_JOINTS; i++) 115 | { 116 | // Register handle in joint limits interface 117 | joint_limits_interface::EffortJointSaturationHandle eff_limit_handle(jnt_eff_interface_.getHandle(joint_names_[i + array_index_offset]), // We read the state and read/write the command 118 | limits_[i + array_index_offset]); // Limits struct, copy constructor copies this 119 | jnt_eff_limit_interface_.registerHandle(eff_limit_handle); 120 | } 121 | 122 | registerInterface(&jnt_eff_interface_); 123 | ROS_INFO("Hardware interfaces registered"); 124 | } 125 | 126 | KinovaGen3HardwareInterface::~KinovaGen3HardwareInterface() 127 | { 128 | EndLowLevelControl(network_connection_); 129 | } 130 | 131 | void KinovaGen3HardwareInterface::publish_throttled_debug_message(int severity, std::vector before_limiting) { 132 | // add to throttle counter 133 | debug_msg_counter_ += 1; 134 | 135 | // publish if throttle counter high enough 136 | if (severity > last_publish_severity_ || debug_msg_counter_ >= 10) { 137 | if (realtime_pub_->trylock()){ 138 | realtime_pub_->msg_.stamp = ros::Time::now(); 139 | std::vector command(std::begin(cmd_), std::end(cmd_)); 140 | std::vector read_position(std::begin(pos_), std::end(pos_)); 141 | std::vector read_velocity(std::begin(vel_), std::end(vel_)); 142 | std::vector read_effort(std::begin(eff_), std::end(eff_)); 143 | std::vector read_current(std::begin(curr_), std::end(curr_)); 144 | realtime_pub_->msg_.command_before_limiting = before_limiting; 145 | realtime_pub_->msg_.command = command; 146 | realtime_pub_->msg_.read_position = read_position; 147 | realtime_pub_->msg_.read_velocity = read_velocity; 148 | realtime_pub_->msg_.read_effort = read_effort; 149 | realtime_pub_->msg_.read_current = read_current; 150 | // save last published severity and reset counter 151 | last_publish_severity_ = severity; 152 | debug_msg_counter_ = 0; 153 | realtime_pub_->unlockAndPublish(); 154 | } 155 | } 156 | } 157 | 158 | void KinovaGen3HardwareInterface::write(const ros::Duration& period) 159 | { 160 | int severity=0; // how important a debugging message of the current state would be 161 | 162 | std::vector before_limiting(NUMBER_OF_JOINTS); 163 | for (int i=0; i 0.00001; 174 | joints_set_to_zero = joints_set_to_zero || abs(cmd_[i]) < 0.00001; 175 | } 176 | if (joints_limited){ 177 | severity = 1; 178 | if (joints_set_to_zero) { 179 | severity = 2; 180 | std::stringstream errormsg; 181 | errormsg << "JointTorqueSetToZero: "; 182 | for (int i=0; iset_position(base_feedback_.actuators(i).position()); 206 | } 207 | int array_index_offset = FIRST_JOINT_INDEX - 1; 208 | for (int i = 0; i < NUMBER_OF_JOINTS; i++) 209 | { 210 | // Note that mutable_actuators is a 0-indexed array 211 | //base_command.mutable_actuators(i + array_index_offset)->set_torque_joint(cmd_[i]); 212 | base_command.mutable_actuators(i + array_index_offset)->set_current_motor(cmd_[i]/curr_to_torque_factor_[i + array_index_offset]); 213 | 214 | } 215 | 216 | try 217 | { 218 | network_connection_->CyclicRefresh(base_command); 219 | } 220 | catch (Kinova::Api::KDetailedException& ex) 221 | { 222 | std::cout << "API error: " << ex.what() << std::endl; 223 | throw; 224 | } 225 | catch (std::runtime_error& ex2) 226 | { 227 | std::cout << "Error: " << ex2.what() << std::endl; 228 | throw; 229 | } 230 | } 231 | 232 | void KinovaGen3HardwareInterface::read() 233 | { 234 | base_feedback_ = network_connection_->CyclicRefreshFeedback(); 235 | 236 | int array_index_offset = FIRST_JOINT_INDEX - 1; 237 | for (int i = 0; i < NUMBER_OF_JOINTS; i++) 238 | { 239 | // Note that actuators is a 0-indexed array 240 | pos_[i] = angles::normalize_angle(angles::from_degrees(base_feedback_.actuators(i + array_index_offset).position())); // originally degrees 241 | vel_[i] = angles::from_degrees(base_feedback_.actuators(i + array_index_offset).velocity()); // originally degrees per second 242 | // NOTA BENE: The torque returned by the feedback call is ``wrong''. 243 | // It is -1 times what you would expect. 244 | // That is, if the feedback returns a _positive_ torque, it means the motor 245 | // is pushing the arm toward a more _negative_ position. 246 | // (You can check this by putting the arm in a static position 247 | // and seeing which way the joint must be pushing to counteract gravity) 248 | // We fix this oddity by multiplying the feedback by -1 here to give the 249 | // correct torque that the joint is currently applying. 250 | // An additional benefit of this fix is that you can now take the read effort 251 | // and write that to the robot as a command to have the robot maintain its current torque. 252 | // Note that this oddity is also reflected on the Kinova Dashboard 253 | // (a positive torque there also means the joint is trying to make the position more negative) 254 | eff_[i] = -base_feedback_.actuators(i + array_index_offset).torque(); // originally Newton * meters 255 | cmd_[i] = eff_[i]/2.0; // so that weird stuff doesn't happen before controller loads 256 | // the factor of 2 reduction is to basically just set the control to close to 0 257 | // This read in curr_ is only used for debugging 258 | curr_[i] = base_feedback_.actuators(i + array_index_offset).current_motor(); // Amps 259 | if (isnan(cmd_[i])) { 260 | ROS_ERROR("Had a cmd of NAN!!!!!"); 261 | } 262 | } 263 | 264 | if (NUMBER_OF_JOINTS == 7) 265 | { 266 | ROS_DEBUG_THROTTLE(1, "Read an effort of %f, %f, %f, %f, %f, %f, %f", cmd_[0], cmd_[1], cmd_[2], cmd_[3], cmd_[4], cmd_[5], cmd_[6]); 267 | } 268 | else 269 | { 270 | ROS_DEBUG_THROTTLE(1, "Read effort %03.4f, vel %03.4f, pos %03.4f", cmd_[0], vel_[0], pos_[0]); 271 | } 272 | } 273 | --------------------------------------------------------------------------------