iirob · destogl · Oct 18, 2016 · Oct 18, 2016 · Oct 18, 2016 · Oct 19, 2016
diff --git a/.travis.rosinstall b/.travis.rosinstall
@@ -0,0 +1,13 @@
+- git:
+    uri: 'https://github.com/iirob/iirob_filters.git'
+    local-name: iirob_filters
+    version: kinetic-devel
+- git:
+    uri: 'https://github.com/iirob/rosparam_handler.git'
+    local-name: rosparam_handler
+    version: master
+- git:
+    uri: 'https://github.com/KITrobotics/force_torque_sensor.git'
+    local-name: force_torque_sensor
+    version: kinetic-devel
+
diff --git a/.travis.yml b/.travis.yml
@@ -10,13 +10,16 @@ notifications:
     on_failure: always
 
 env:
+  global:
+    - UPSTREAM_WORKSPACE=file
   matrix:
     - USE_DEB=true
       ROS_DISTRO="indigo"
       ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
     - USE_DEB=true
       ROS_DISTRO="indigo"
       ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu
+    - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
 
 install:
   - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config

diff --git a/kuka_kr16_support/CMakeLists.txt b/kuka_kr16_support/CMakeLists.txt
@@ -8,7 +8,7 @@ catkin_package()
 
 if (CATKIN_ENABLE_TESTING)
   find_package(roslaunch REQUIRED)
-  roslaunch_add_file_check(test/roslaunch_test.xml)
+  roslaunch_add_file_check(launch)
 endif()
 
 install(DIRECTORY config launch meshes urdf

diff --git a/kuka_kr16_support/test/roslaunch_test.xml b/kuka_kr16_support/test/roslaunch_test.xml
diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt
@@ -11,6 +11,9 @@ find_package(catkin REQUIRED COMPONENTS
   realtime_tools
   roscpp
   std_msgs
+  force_torque_sensor
+  force_controllers
+  kdl_parser
 )
 
 find_package(Boost REQUIRED COMPONENTS system)
@@ -28,6 +31,8 @@ catkin_package(
     joint_limits_interface
     roscpp
     std_msgs
+    force_torque_sensor
+    force_controllers
   DEPENDS
     TinyXML
 )
@@ -43,21 +48,32 @@ include_directories(
 add_library(kuka_hardware_interface
   src/kuka_hardware_interface.cpp
 )
+add_library(kuka_hardware_interface_with_fts
+  src/kuka_hardware_interface_with_fts.cpp
+)
 
 target_link_libraries(kuka_hardware_interface
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
   ${TinyXML_LIBRARIES}
 )
+target_link_libraries(kuka_hardware_interface_with_fts
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${TinyXML_LIBRARIES}
+  kuka_hardware_interface
+)
 
 add_executable(kuka_hardware_interface_node
   src/kuka_hardware_interface_node.cpp
 )
 
 target_link_libraries(kuka_hardware_interface_node
   kuka_hardware_interface
+  kuka_hardware_interface_with_fts
 )
 
+
 install(
   TARGETS kuka_hardware_interface
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -67,3 +83,7 @@ install(
 install(
   TARGETS kuka_hardware_interface_node
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(DIRECTORY config launch test
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/kuka_rsi_hw_interface/config/hardware_controllers.yaml b/kuka_rsi_hw_interface/config/hardware_controllers.yaml
@@ -1,8 +1,7 @@
 #Publish all joint states
 joint_state_controller:
   type: joint_state_controller/JointStateController
-  publish_rate: 50
-
+  publish_rate: 1000
 # Joint trajectory controller
 position_trajectory_controller:
   type: "position_controllers/JointTrajectoryController"
@@ -14,5 +13,7 @@ position_trajectory_controller:
     - joint_a5
     - joint_a6
 
-  state_publish_rate: 50 # Defaults to 50
+  state_publish_rate: 1000 # Defaults to 50
   action_monitor_rate: 20 # Defaults to 20
+  constraints:
+    goal_time: 0.5
diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h
@@ -85,6 +85,7 @@ class KukaHardwareInterface : public hardware_interface::RobotHW
   std::vector<std::string> joint_names_;
 
   std::vector<double> joint_position_;
+  std::vector<double> joint_position_last_;
   std::vector<double> joint_velocity_;
   std::vector<double> joint_effort_;
   std::vector<double> joint_position_command_;
@@ -112,20 +113,23 @@ class KukaHardwareInterface : public hardware_interface::RobotHW
   ros::Duration control_period_;
   ros::Duration elapsed_time_;
   double loop_hz_;
-
+protected:
   // Interfaces
   hardware_interface::JointStateInterface joint_state_interface_;
   hardware_interface::PositionJointInterface position_joint_interface_;
+// TODO
+  hardware_interface::VelocityJointInterface velocity_joint_interface_;
 
 public:
 
-  KukaHardwareInterface();
+  KukaHardwareInterface(ros::NodeHandle nh);
   ~KukaHardwareInterface();
 
   void start();
   void configure();
   bool read(const ros::Time time, const ros::Duration period);
   bool write(const ros::Time time, const ros::Duration period);
+//   virtual void handleInit();
 
 };
 

diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface_with_fts.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface_with_fts.h
@@ -0,0 +1,33 @@
+#ifndef KUKA_RSI_HARDWARE_INTERFACE_WITH_FTS
+#define KUKA_RSI_HARDWARE_INTERFACE_WITH_FTS
+
+#include <force_controllers/force_controller.h>
+#include <force_torque_sensor/force_torque_sensor_handle.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <kuka_rsi_hw_interface/kuka_hardware_interface.h>
+#include <ros/console.h>
+#include <boost/shared_ptr.hpp>
+
+namespace kuka_rsi_hw_interface
+{
+
+class KukaHardwareInterfaceWithFTS : public KukaHardwareInterface
+{
+    force_torque_control::ForceTorqueControllerInterface ftc_interface_;
+//     ForceTorqueSensorHandle* ftsh_;
+    boost::shared_ptr<hardware_interface::ForceTorqueSensorHandle> ftsh_;
+    ros::NodeHandle nh_;
+
+    std::string fts_name;
+    std::string fts_sensor_frame;
+    std::string fts_transform_frame;
+
+public:
+    KukaHardwareInterfaceWithFTS(ros::NodeHandle nh);
+//     ~KukaHardwareInterfaceWithFTS();
+
+    void handleInit();
+};
+} // namespace kuka_rsi_hw_interface
+
+#endif
diff --git a/kuka_rsi_hw_interface/krl/KR_C2/README.md b/kuka_rsi_hw_interface/krl/KR_C2/README.md
@@ -4,17 +4,19 @@ This guide highlights the steps needed in order to successfully configure the **
 
 ## 1. Controller network configuration
 
-Controller setup is described in chapter 4 of the **KUKA.Ethernet RSI XML 1.1** manual.
-**KUKA.RobotSensorInterface** package must be installed. Depending on the version of the **KUKA.RobotSensorInterface** package you might also need the **KUKA.Ethernet RSI XML**. This can be verified in the *InstallTech* window (`Setup -> 9 Install additional Software` on the KCP).
+Controller setup is described in chapter 4 of the **KUKA.Ethernet RSI XML 1.1** manual. 
+**KUKA.RobotSensorInterface** package must be installed. Depending on the version of the **KUKA.RobotSensorInterface** package you might also need the **KUKA.Ethernet RSI XML**. This can be verified in the *InstallTech* window (`Setup -> 9 Install additional Software` on the KCP)
 
-Make sure not to use the `192.0.1.x` IP range for the controller or PC.
+Make sure the not to use the `192.0.1.x` IP range for the controller or PC.
 
 From the Ethernet RSI XML manual:
 
 > The IP address range 192.0.1.x is reserved and is disabled for applications.
-> Configuring the VxWorks network connection with this address range results in a system error in the KUKA system software. It is no longer possible to boot the robot controller.
+> Configuring the VxWorks network connection with this address range results
+> in a system error in the KUKA system software. It is no longer possible to
+> boot the robot controller. 
 
-Ping the controller from the PC with ROS to make sure the network configuration is working.
+Ping the controller from the PC with ROS to make sure the network configuration is working. 
 
 
 ## 2. KRL Files
@@ -38,7 +40,7 @@ This should only be edited if the start position or max joint movements specifie
 1. Copy the `ros_rsi.src` file to `KRC:\R1\Program` (Alternatively `C:\KRC\ROBOTER\KRC\R1\Program`)
 2. Copy the `ros_rsi_ethernet.xml` file to `C:\KRC\ROBOTER\Init`
 
-Note: You must reload KSS or reboot the controller if `ros_rsi.src` is not created / copied using the HMI.
+Note: You must reload KSS or reboot the controller if `ros_rsi.src` is not created\copied using the HMI.
 
 ## 3. Configure the kuka_rsi_hw_interface
 
@@ -49,7 +51,7 @@ The **kuka_rsi_hardware_interface** needs to be configured in order to successfu
 
 We recommend that you copy the configuration files, edit the copies for your needs and use these files to create your own launch file. A template will be provided at a later time. However, at this time, have a look at `test_hardware_interface.launch`, `test_params.yaml`, `controller_joint_names.yaml` and `hardware_controllers.yaml` to achieve a working test-launch.
 
-In order to successfully launch the **kuka_rsi_hardware_interface** a parameter `robot_description` needs to be present on the ROS parameter server. This parameter can be set manually or by adding this line inside the launch file (replace support package and `.xacro` to match your application):
+In order to successfully launch the **kuka_rsi_hardware_interface** a parameter `robot_description` needs to be present on the ROS parameter server. This parameter can be set manually or by adding this line inside the launch file (replace support package and .xacro to match your application):
 
 ```
 <param name="robot_description" command="$(find xacro)/xacro.py '$(find kuka_kr6_support)/urdf/kr6r900sixx.xacro'"/>
@@ -100,16 +102,21 @@ Choose **controller manager ns** and **controller** and you should be able to mo
 
 ### RSI: Error in function <ST_ETHERNET>
 
-Problems establishing connection between robot or PC. Check IP, port and that the **kuka_rsi_hardware_interface** is running on the PC. May also come from malformed XML in `ros_rsi_ethernet.xml`.
-
+Problems establishing connection between robot or PC. Check IP, port and that the **kuka_rsi_hardware_interface** is running on the PC. May also come from malformed XML in ros_rsi_ethernet.xml
 
+
 ### SEN: RSI execution error <execute> - RSI stopped
 
 Most likely due to late packages. From the Ethernet RSI XML manual:
 
-> A data packet received by the external system must be answered within approx. 10 ms. If the data packet is not received by the robot controller within this period, the response is classified as too late. When the maximum number of external data packets for which a response has been sent too late has been exceeded, the robot interprets this as an error and stops.
+> A data packet received by the external system must be answered within approx.
+> 10 ms. If the data packet is not received by the robot controller within this
+> period, the response is classified as too late. When the maximum number of
+> external data packets for which a response has been sent too late has been
+> exceeded, the robot interprets this as an error and stops
 
 If you have problems with the connection to RSI shutting down now and then while moving the robot it is suggested to:
-
 * Compile and install a [RT-Preempt](https://rt.wiki.kernel.org/index.php/RT_PREEMPT_HOWTO) kernel for your PC.
 * Give **kuka_rsi_hardware_interface** on your PC real-time priority when the RSI connection is established.
+
+
diff --git a/kuka_rsi_hw_interface/package.xml b/kuka_rsi_hw_interface/package.xml
@@ -22,4 +22,7 @@
   <depend>realtime_tools</depend>
   <depend>roscpp</depend>
   <depend>std_msgs</depend>
+  <depend>force_torque_sensor</depend>
+  <depend>force_controllers</depend>
+  <depend>kdl_parser</depend>
 </package>
diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp
@@ -45,43 +45,51 @@
 namespace kuka_rsi_hw_interface
 {
 
-KukaHardwareInterface::KukaHardwareInterface() :
-    joint_position_(6, 0.0), joint_velocity_(6, 0.0), joint_effort_(6, 0.0), joint_position_command_(6, 0.0), joint_velocity_command_(
-        6, 0.0), joint_effort_command_(6, 0.0), joint_names_(6), rsi_initial_joint_positions_(6, 0.0), rsi_joint_position_corrections_(
-        6, 0.0), ipoc_(0), n_dof_(6)
+KukaHardwareInterface::KukaHardwareInterface(ros::NodeHandle nh) :
+    joint_position_(6, 0.0), joint_position_last_(6, 0.0), joint_velocity_(6, 0.0), joint_effort_(6, 0.0),
+    joint_position_command_(6, 0.0), joint_velocity_command_(6, 0.0), joint_effort_command_(6, 0.0), joint_names_(6),
+    rsi_initial_joint_positions_(6, 0.0), rsi_joint_position_corrections_(6, 0.0), ipoc_(0), n_dof_(6)
 {
-  in_buffer_.resize(1024);
-  out_buffer_.resize(1024);
-  remote_host_.resize(1024);
-  remote_port_.resize(1024);
-
-  if (!nh_.getParam("controller_joint_names", joint_names_))
-  {
-    ROS_ERROR("Cannot find required parameter 'controller_joint_names' "
-      "on the parameter server.");
-    throw std::runtime_error("Cannot find required parameter "
-      "'controller_joint_names' on the parameter server.");
-  }
-
-  //Create ros_control interfaces
-  for (std::size_t i = 0; i < n_dof_; ++i)
-  {
-    // Create joint state interface for all joints
-    joint_state_interface_.registerHandle(
-        hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], &joint_velocity_[i],
-                                             &joint_effort_[i]));
-
-    // Create joint position control interface
-    position_joint_interface_.registerHandle(
-        hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[i]),
-                                        &joint_position_command_[i]));
-  }
-
-  // Register interfaces
-  registerInterface(&joint_state_interface_);
-  registerInterface(&position_joint_interface_);
-
-  ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded kuka_rsi_hardware_interface");
+    nh_ = nh;
+    in_buffer_.resize(1024);
+    out_buffer_.resize(1024);
+    remote_host_.resize(1024);
+    remote_port_.resize(1024);
+
+    if (!nh_.getParam("controller_joint_names", joint_names_))
+    {
+        ROS_ERROR("Cannot find required parameter 'controller_joint_names' "
+        "on the parameter server.");
+        throw std::runtime_error("Cannot find required parameter "
+        "'controller_joint_names' on the parameter server.");
+    }
+
+    //Create ros_control interfaces
+    for (std::size_t i = 0; i < n_dof_; ++i)
+    {
+        // Create joint state interface for all joints
+        joint_state_interface_.registerHandle(
+            hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], &joint_velocity_[i],
+                                                &joint_effort_[i]));
+
+        // Create joint position control interface
+        position_joint_interface_.registerHandle(
+            hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[i]),
+                                            &joint_position_command_[i]));
+
+        // TODO Create joint velocity control interface
+        velocity_joint_interface_.registerHandle(
+            hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[i]),
+                                            &joint_velocity_command_[i]));
+    }
+
+    // Register interfaces
+    registerInterface(&joint_state_interface_);
+    registerInterface(&position_joint_interface_);
+    // TODO
+    registerInterface(&velocity_joint_interface_);
+
+    ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded kuka_rsi_hardware_interface");
 }
 
 KukaHardwareInterface::~KukaHardwareInterface()
@@ -107,8 +115,13 @@ bool KukaHardwareInterface::read(const ros::Time time, const ros::Duration perio
   for (std::size_t i = 0; i < n_dof_; ++i)
   {
     joint_position_[i] = DEG2RAD * rsi_state_.positions[i];
+    if(!period.isZero())
+    {
+      joint_velocity_[i] = (joint_position_[i] - joint_position_last_[i]) / period.toSec();
+    }
   }
   ipoc_ = rsi_state_.ipoc;
+  joint_position_last_ = joint_position_;
 
   return true;
 }
@@ -142,7 +155,7 @@ void KukaHardwareInterface::start()
   {
     bytes = server_->recv(in_buffer_);
   }
-
+  
   rsi_state_ = RSIState(in_buffer_);
   for (std::size_t i = 0; i < n_dof_; ++i)
   {
@@ -177,6 +190,7 @@ void KukaHardwareInterface::configure()
     throw std::runtime_error(msg);
   }
   rt_rsi_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::String>(nh_, "rsi_xml_doc", 3));
-}
+  }
+// void handleInit(){};
 
 } // namespace kuka_rsi_hardware_interface