[lwr_hw] enhancement

.gitignore

@@ -18,4 +18,15 @@ ros/kuka-lwr/lwr_moveit/default_warehouse_mongo_db/local.*
 ros/kuka_lwr/lwr_moveit/default_warehouse_mongo_db/mongod.lock
 ros/kuka_lwr/lwr_moveit/default_warehouse_mongo_db/journal/prealloc.*
 
+# Kdev and QTcreator files
 *.kdev_include_paths
+*.user
+
+# Typical build folders
+build
+BUILD
+Build
+
+# Avoid FRILibrary
+FRILibrary
+fril.zip

.travis.yml

@@ -1,30 +1,27 @@
+# travis beta feature
+dist: trusty
+
 language:
   - cpp
 
 compiler:
   - gcc
 
-env:
-  global:
-  - ROS_DISTRO=indigo
-
 before_install:
-  #- vagrant status
-  - export CI_SOURCE_PATH=$(pwd)
-  - export REPOSITORY_NAME=${PWD##*/}
-  #- codename=`cat /etc/lsb-release | grep -m 1 "DISTRIB_CODENAME=" | cut -d "=" -f2`
-  - lsb_release -a
-  - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+  #- export CI_SOURCE_PATH=$(pwd)
+  - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
   - sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-latest.list'
   - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
   - wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
-  - sudo apt-get update
+  - sudo apt-get update -qq
 
 install:
-  - sudo apt-get install ros-$ROS_DISTRO-desktop-full ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllers ros-$ROS_DISTRO-gazebo4-ros ros-$ROS_DISTRO-gazebo4-ros-pkgs 
+  - sudo apt-get install ros-indigo-ros-base ros-indigo-cmake-modules ros-indigo-orocos-kdl ros-indigo-kdl-parser ros-indigo-ros-control ros-indigo-ros-controllers ros-indigo-gazebo4-ros ros-indigo-gazebo4-ros-pkgs
 
 before_script:
-  - source /opt/ros/$ROS_DISTRO/setup.bash
+  - sudo rosdep init
+  - rosdep update
+  - source /opt/ros/indigo/setup.bash
   - mkdir -p ~/catkin_ws/src
   - export REPOSITORY_NAME=${PWD##*/}
   - ln -s $(pwd) ~/catkin_ws/src/$REPOSITORY_NAME

README.md

@@ -1,13 +1,13 @@
 # KUKA LWR 4+
 
-[![Build Status](https://api.travis-ci.org/CentroEPiaggio/kuka-lwr.svg)](https://travis-ci.org/CentroEPiaggio/kuka-lwr) (<- waiting for Ubuntu 14.04 support)
+[![Build Status](https://api.travis-ci.org/CentroEPiaggio/kuka-lwr.svg)](https://travis-ci.org/CentroEPiaggio/kuka-lwr)
 
-ROS indigo metapackage that contains packages to work with the KUKA LWR 4+.
+ROS (tested on indigo) packages to work with the KUKA LWR 4+.
 
 ## Overview
 The main packages are:
 - [__lwr_description__](https://github.com/CentroEPiaggio/kuka-lwr/tree/master/lwr_description): a package that defines the model of the robot (ToDo: name it __lwr_model__)
-- [__lwr_hw__](https://github.com/CentroEPiaggio/kuka-lwr/tree/master/lwr_hw): a package that allows communication with an LWR 4+ through FRI or through a gazebo simulation. If you are using the joint impedance control strategy, it adds the gravity term computed from the URDF model.
+- [__lwr_hw__](https://github.com/CentroEPiaggio/kuka-lwr/tree/master/lwr_hw): a package that contains the LWR 4+ definition within the ros control framework, and also final interfaces using Kuka FRI, Stanford FRI Library or a Gazebo plugin. Read adding an interface below if you wish to add a different non-existing interface. 
 - [__lwr_controllers__](https://github.com/CentroEPiaggio/kuka-lwr/tree/master/lwr_controllers): a package that implement a set of useful controllers (ToDo: perhaps moving this to a forked version of `ros_controllers` would be ok, but some controllers are specific for the a 7-dof arm).
 - [__single_lwr_example__](https://github.com/CentroEPiaggio/kuka-lwr/tree/master/single_lwr_example): a cofiguration-based meta-package that shows how to use the `kuka_lwr` packages.
 	- [__single_lwr_robot__](https://github.com/CentroEPiaggio/kuka-lwr/tree/master/single_lwr_example/single_lwr_robot): the package where you define your robot using the LWR 4+ arm.
@@ -18,8 +18,35 @@ For an example using two LWR 4+ arms and two Pisa/IIT SoftHands, see the [Vito r
 
 ## Install
 
-ToDo: one should check the commands in the [`.travis.yaml`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/.travis.yml) file when that is working in Ubuntu 14.04.
+Check the [`.travis.yaml`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/.travis.yml) file, that'll give you the basic steps to install all necessary packages to compile the package.
 
-The most critical that are not straight forward if you want to use the simulation environment are:
-- [Gazebo4](http://gazebosim.org/tutorials?tut=install_ubuntu&ver=4.0&cat=install) or higher
-- Slightly modified `transmission_interface` package that allows robot composability in gazebo @ (forked) [ros_control](https://github.com/CentroEPiaggio/ros_control/tree/multi-robot-test)
+## Adding more interfaces/platforms
+
+The package [lwr_hw](lwr_hw) contains the abstraction that allows to make the most of the new ros control framework. To create an instance of the arm you need to call the function [`void LWRHW::create(std::string name, std::string urdf_string)`](lwr_hw/include/lwr_hw/lwr_hw.h#L40), where `name` MUST match the name you give to the xacro instance in the URDF and the `urdf_string` is any robot description containing one single instance of the lwr called as `name` (note that, several lwr can exist in `urdf_string` if they are called differently).
+
+The abstraction is enforced with [three pure virtual functions](lwr_hw/include/lwr_hw/lwr_hw.h#L61-L64):
+``` c++
+  virtual bool init() = 0;
+  virtual void read(ros::Time time, ros::Duration period) = 0;
+  virtual void write(ros::Time time, ros::Duration period) = 0;
+```
+
+Adding an interface boils down to inherit from the [LWRHW class](lwr_hw/include/lwr_hw/lwr_hw.h#L33), implement these three function according to your final platform, and creating either a node or a plugin that uses your new class. This way you can use all your planning and controllers setup in any final real or simulated robot.
+
+Examples of final interface class implementations are found for the [Kuka FRI](lwr_hw/include/lwr_hw/lwr_hw_fri.hpp), [Stanford FRI library](lwr_hw/include/lwr_hw/lwr_hw_fril.hpp) and a [Gazebo simulation](lwr_hw/include/lwr_hw/lwr_hw_gazebo.hpp). The corresponding nodes and plugin are found [here](lwr_hw/src/lwr_hw_fri_node.cpp), [here](lwr_hw/src/lwr_hw_fril_node.cpp), and [here](lwr_hw/src/lwr_hw_gazebo_plugin.cpp).
+
+## Using the Stanford FRI Library
+
+You need to provide your user name with real time priority and memlock limits higher than the default ones. You can do it permanently like this:
+
+1. `sudo nano /etc/security/limits.conf` and add these lines: 
+```
+YOUR_USERNAME hard rtprio 95
+YOUR_USERNAME soft rtprio 95
+YOUR_USERNAME hard memlock unlimited
+YOUR_USERNAME soft memlock unlimited
+```
+2. `sudo  nano /etc/pam.d/common-session` and add `session required pam_limits.so`
+3. Reboot, open a terminal, and check that `ulimit -r -l` gives you the values set above.
+4. You need to edit manually the [lwr_hw.launch](lwr_hw/launch/lwr_hw.launch)
+5. Load the KRL script that is downloaded with the library to the Robot, and follow the instructions from the [original link](http://cs.stanford.edu/people/tkr/fri/html/) to set up network and other requirements properly.

lwr_controllers/include/lwr_controllers/gravity_compensation.h

@@ -8,26 +8,26 @@
 
 namespace lwr_controllers
 {
-    class GravityCompensation: public controller_interface::KinematicChainControllerBase<hardware_interface::PositionJointInterface>
+    class GravityCompensation: public controller_interface::KinematicChainControllerBase<hardware_interface::EffortJointInterface>
     {
     public:
 
         GravityCompensation();
         ~GravityCompensation();
 
-        bool init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n);
+        bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
         void starting(const ros::Time& time);
         void update(const ros::Time& time, const ros::Duration& period);
         void stopping(const ros::Time& time);
 
-    private:
+    // private:
 
-        std::vector<float> previous_stiffness_; /// stiffness before activating controller
+        // std::vector<float> previous_stiffness_; /// stiffness before activating controller
 
         // hack required as long as there is separate position handle for stiffness
-        std::vector<hardware_interface::JointHandle> joint_stiffness_handles_;
+        // std::vector<hardware_interface::JointHandle> joint_stiffness_handles_;
 
-        const static float DEFAULT_STIFFNESS = 0.01;
+        // const static float DEFAULT_STIFFNESS = 0.01;
 
     };
 }

lwr_controllers/include/lwr_controllers/one_task_inverse_kinematics.h

@@ -1,14 +1,11 @@
 #ifndef LWR_CONTROLLERS__ONE_TASK_INVERSE_KINEMATICS_H
 #define LWR_CONTROLLERS__ONE_TASK_INVERSE_KINEMATICS_H
 
-#include "PIDKinematicChainControllerBase.h"
-#include <lwr_controllers/MultiPriorityTask.h>
-#include <lwr_controllers/PoseRPY.h>
+#include "KinematicChainControllerBase.h"
+#include "lwr_controllers/PoseRPY.h"
 
-#include <std_msgs/Float64MultiArray.h>
 #include <visualization_msgs/Marker.h>
-
-#include <control_toolbox/pid.h>
+#include <geometry_msgs/PoseStamped.h>
 
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/chainiksolvervel_pinv.hpp>
@@ -20,13 +17,13 @@
 
 namespace lwr_controllers
 {
-	class OneTaskInverseKinematics: public controller_interface::PIDKinematicChainControllerBase<hardware_interface::EffortJointInterface>
+	class OneTaskInverseKinematics: public controller_interface::KinematicChainControllerBase<hardware_interface::PositionJointInterface>
 	{
 	public:
 		OneTaskInverseKinematics();
 		~OneTaskInverseKinematics();
 
-		bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
+		bool init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n);
 		void starting(const ros::Time& time);
 		void update(const ros::Time& time, const ros::Duration& period);
 		void command(const lwr_controllers::PoseRPY::ConstPtr &msg);
@@ -40,7 +37,7 @@ namespace lwr_controllers
 
 		KDL::Twist x_err_;
 
-		KDL::JntArray tau_cmd_;
+		KDL::JntArray q_cmd_; // computed set points
 
 		KDL::Jacobian J_;	//Jacobian
 
@@ -58,12 +55,9 @@ namespace lwr_controllers
 		int cmd_flag_;
 
 		boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
-		boost::scoped_ptr<KDL::ChainDynParam> id_solver_;
 		boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_;
 		boost::scoped_ptr<KDL::ChainIkSolverVel_pinv> ik_vel_solver_;
 		boost::scoped_ptr<KDL::ChainIkSolverPos_NR_JL> ik_pos_solver_;
-
-		std::vector<control_toolbox::Pid> PIDs_;
 	};
 
 }

lwr_controllers/src/gravity_compensation.cpp

@@ -8,29 +8,29 @@ namespace lwr_controllers
     GravityCompensation::GravityCompensation() {}
     GravityCompensation::~GravityCompensation() {}
 
-    bool GravityCompensation::init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n)
+    bool GravityCompensation::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
     {
-        KinematicChainControllerBase<hardware_interface::PositionJointInterface>::init(robot, n);
+        KinematicChainControllerBase<hardware_interface::EffortJointInterface>::init(robot, n);
 
         // find stiffness (dummy) joints; this is necessary until proper position/stiffness/damping interface exists
-        for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin(); it != kdl_chain_.segments.end(); ++it)
-        {
-            joint_stiffness_handles_.push_back(robot->getHandle(it->getJoint().getName()+"_stiffness"));
-        }
+        // for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin(); it != kdl_chain_.segments.end(); ++it)
+        // {
+        //     joint_stiffness_handles_.push_back(robot->getHandle(it->getJoint().getName()+"_stiffness"));
+        // }
 
-        ROS_DEBUG("found %lu stiffness handles", joint_stiffness_handles_.size());
+        // ROS_DEBUG("found %lu stiffness handles", joint_stiffness_handles_.size());
 
-        previous_stiffness_.resize(joint_stiffness_handles_.size());
+        // previous_stiffness_.resize(joint_stiffness_handles_.size());
 
-        return true;		
+        return true;
     }
 
     void GravityCompensation::starting(const ros::Time& time)
     {
-        for(size_t i=0; i<joint_handles_.size(); i++) 
-        {
-            previous_stiffness_[i] = joint_stiffness_handles_[i].getPosition();
-        }
+        // for(size_t i=0; i<joint_handles_.size(); i++)
+        // {
+        //    previous_stiffness_[i] = joint_stiffness_handles_[i].getPosition();
+        // }
     }
 
     void GravityCompensation::update(const ros::Time& time, const ros::Duration& period)
@@ -40,18 +40,18 @@ namespace lwr_controllers
         // is raised again
         for(size_t i=0; i<joint_handles_.size(); i++) 
         {
-            joint_handles_[i].setCommand(joint_handles_[i].getPosition());
-            joint_stiffness_handles_[i].setCommand(DEFAULT_STIFFNESS);
-
-        }      
+            //joint_handles_[i].setCommand(joint_handles_[i].getPosition());
+            joint_handles_[i].setCommand( 0.0 );
+            // joint_stiffness_handles_[i].setCommand(DEFAULT_STIFFNESS);
+        }
     }
 
     void GravityCompensation::stopping(const ros::Time& time)
     {
-        for(size_t i=0; i<joint_handles_.size(); i++) 
-        {
-            joint_stiffness_handles_[i].setCommand(previous_stiffness_[i]);
-        }
+        //for(size_t i=0; i<joint_handles_.size(); i++)
+        //{
+        //    joint_stiffness_handles_[i].setCommand(previous_stiffness_[i]);
+        //}
     }
 
 }