Skip to content

Commit d92df4e

Browse files
committed
Merge pull request #42 from carlosjoserg/lwr_hw_enhancement
[lwr_hw] update
2 parents a9d183e + 260651c commit d92df4e

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

48 files changed

+2692
-2364
lines changed

.gitignore

+11
Original file line numberDiff line numberDiff line change
@@ -18,4 +18,15 @@ ros/kuka-lwr/lwr_moveit/default_warehouse_mongo_db/local.*
1818
ros/kuka_lwr/lwr_moveit/default_warehouse_mongo_db/mongod.lock
1919
ros/kuka_lwr/lwr_moveit/default_warehouse_mongo_db/journal/prealloc.*
2020

21+
# Kdev and QTcreator files
2122
*.kdev_include_paths
23+
*.user
24+
25+
# Typical build folders
26+
build
27+
BUILD
28+
Build
29+
30+
# Avoid FRILibrary
31+
FRILibrary
32+
fril.zip

.travis.yml

+10-13
Original file line numberDiff line numberDiff line change
@@ -1,30 +1,27 @@
1+
# travis beta feature
2+
dist: trusty
3+
14
language:
25
- cpp
36

47
compiler:
58
- gcc
69

7-
env:
8-
global:
9-
- ROS_DISTRO=indigo
10-
1110
before_install:
12-
#- vagrant status
13-
- export CI_SOURCE_PATH=$(pwd)
14-
- export REPOSITORY_NAME=${PWD##*/}
15-
#- codename=`cat /etc/lsb-release | grep -m 1 "DISTRIB_CODENAME=" | cut -d "=" -f2`
16-
- lsb_release -a
17-
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
11+
#- export CI_SOURCE_PATH=$(pwd)
12+
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
1813
- sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-latest.list'
1914
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
2015
- wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
21-
- sudo apt-get update
16+
- sudo apt-get update -qq
2217

2318
install:
24-
- 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
19+
- 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
2520

2621
before_script:
27-
- source /opt/ros/$ROS_DISTRO/setup.bash
22+
- sudo rosdep init
23+
- rosdep update
24+
- source /opt/ros/indigo/setup.bash
2825
- mkdir -p ~/catkin_ws/src
2926
- export REPOSITORY_NAME=${PWD##*/}
3027
- ln -s $(pwd) ~/catkin_ws/src/$REPOSITORY_NAME

README.md

+34-7
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,13 @@
11
# KUKA LWR 4+
22

3-
[![Build Status](https://api.travis-ci.org/CentroEPiaggio/kuka-lwr.svg)](https://travis-ci.org/CentroEPiaggio/kuka-lwr) (<- waiting for Ubuntu 14.04 support)
3+
[![Build Status](https://api.travis-ci.org/CentroEPiaggio/kuka-lwr.svg)](https://travis-ci.org/CentroEPiaggio/kuka-lwr)
44

5-
ROS indigo metapackage that contains packages to work with the KUKA LWR 4+.
5+
ROS (tested on indigo) packages to work with the KUKA LWR 4+.
66

77
## Overview
88
The main packages are:
99
- [__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__)
10-
- [__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.
10+
- [__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.
1111
- [__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).
1212
- [__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.
1313
- [__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
1818

1919
## Install
2020

21-
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.
21+
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.
2222

23-
The most critical that are not straight forward if you want to use the simulation environment are:
24-
- [Gazebo4](http://gazebosim.org/tutorials?tut=install_ubuntu&ver=4.0&cat=install) or higher
25-
- Slightly modified `transmission_interface` package that allows robot composability in gazebo @ (forked) [ros_control](https://github.com/CentroEPiaggio/ros_control/tree/multi-robot-test)
23+
## Adding more interfaces/platforms
24+
25+
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).
26+
27+
The abstraction is enforced with [three pure virtual functions](lwr_hw/include/lwr_hw/lwr_hw.h#L61-L64):
28+
``` c++
29+
virtual bool init() = 0;
30+
virtual void read(ros::Time time, ros::Duration period) = 0;
31+
virtual void write(ros::Time time, ros::Duration period) = 0;
32+
```
33+
34+
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.
35+
36+
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).
37+
38+
## Using the Stanford FRI Library
39+
40+
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:
41+
42+
1. `sudo nano /etc/security/limits.conf` and add these lines:
43+
```
44+
YOUR_USERNAME hard rtprio 95
45+
YOUR_USERNAME soft rtprio 95
46+
YOUR_USERNAME hard memlock unlimited
47+
YOUR_USERNAME soft memlock unlimited
48+
```
49+
2. `sudo nano /etc/pam.d/common-session` and add `session required pam_limits.so`
50+
3. Reboot, open a terminal, and check that `ulimit -r -l` gives you the values set above.
51+
4. You need to edit manually the [lwr_hw.launch](lwr_hw/launch/lwr_hw.launch)
52+
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

+6-6
Original file line numberDiff line numberDiff line change
@@ -8,26 +8,26 @@
88

99
namespace lwr_controllers
1010
{
11-
class GravityCompensation: public controller_interface::KinematicChainControllerBase<hardware_interface::PositionJointInterface>
11+
class GravityCompensation: public controller_interface::KinematicChainControllerBase<hardware_interface::EffortJointInterface>
1212
{
1313
public:
1414

1515
GravityCompensation();
1616
~GravityCompensation();
1717

18-
bool init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n);
18+
bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
1919
void starting(const ros::Time& time);
2020
void update(const ros::Time& time, const ros::Duration& period);
2121
void stopping(const ros::Time& time);
2222

23-
private:
23+
// private:
2424

25-
std::vector<float> previous_stiffness_; /// stiffness before activating controller
25+
// std::vector<float> previous_stiffness_; /// stiffness before activating controller
2626

2727
// hack required as long as there is separate position handle for stiffness
28-
std::vector<hardware_interface::JointHandle> joint_stiffness_handles_;
28+
// std::vector<hardware_interface::JointHandle> joint_stiffness_handles_;
2929

30-
const static float DEFAULT_STIFFNESS = 0.01;
30+
// const static float DEFAULT_STIFFNESS = 0.01;
3131

3232
};
3333
}

lwr_controllers/include/lwr_controllers/one_task_inverse_kinematics.h

+6-12
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,11 @@
11
#ifndef LWR_CONTROLLERS__ONE_TASK_INVERSE_KINEMATICS_H
22
#define LWR_CONTROLLERS__ONE_TASK_INVERSE_KINEMATICS_H
33

4-
#include "PIDKinematicChainControllerBase.h"
5-
#include <lwr_controllers/MultiPriorityTask.h>
6-
#include <lwr_controllers/PoseRPY.h>
4+
#include "KinematicChainControllerBase.h"
5+
#include "lwr_controllers/PoseRPY.h"
76

8-
#include <std_msgs/Float64MultiArray.h>
97
#include <visualization_msgs/Marker.h>
10-
11-
#include <control_toolbox/pid.h>
8+
#include <geometry_msgs/PoseStamped.h>
129

1310
#include <kdl/chainfksolverpos_recursive.hpp>
1411
#include <kdl/chainiksolvervel_pinv.hpp>
@@ -20,13 +17,13 @@
2017

2118
namespace lwr_controllers
2219
{
23-
class OneTaskInverseKinematics: public controller_interface::PIDKinematicChainControllerBase<hardware_interface::EffortJointInterface>
20+
class OneTaskInverseKinematics: public controller_interface::KinematicChainControllerBase<hardware_interface::PositionJointInterface>
2421
{
2522
public:
2623
OneTaskInverseKinematics();
2724
~OneTaskInverseKinematics();
2825

29-
bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
26+
bool init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n);
3027
void starting(const ros::Time& time);
3128
void update(const ros::Time& time, const ros::Duration& period);
3229
void command(const lwr_controllers::PoseRPY::ConstPtr &msg);
@@ -40,7 +37,7 @@ namespace lwr_controllers
4037

4138
KDL::Twist x_err_;
4239

43-
KDL::JntArray tau_cmd_;
40+
KDL::JntArray q_cmd_; // computed set points
4441

4542
KDL::Jacobian J_; //Jacobian
4643

@@ -58,12 +55,9 @@ namespace lwr_controllers
5855
int cmd_flag_;
5956

6057
boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
61-
boost::scoped_ptr<KDL::ChainDynParam> id_solver_;
6258
boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_;
6359
boost::scoped_ptr<KDL::ChainIkSolverVel_pinv> ik_vel_solver_;
6460
boost::scoped_ptr<KDL::ChainIkSolverPos_NR_JL> ik_pos_solver_;
65-
66-
std::vector<control_toolbox::Pid> PIDs_;
6761
};
6862

6963
}

lwr_controllers/src/gravity_compensation.cpp

+21-21
Original file line numberDiff line numberDiff line change
@@ -8,29 +8,29 @@ namespace lwr_controllers
88
GravityCompensation::GravityCompensation() {}
99
GravityCompensation::~GravityCompensation() {}
1010

11-
bool GravityCompensation::init(hardware_interface::PositionJointInterface *robot, ros::NodeHandle &n)
11+
bool GravityCompensation::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
1212
{
13-
KinematicChainControllerBase<hardware_interface::PositionJointInterface>::init(robot, n);
13+
KinematicChainControllerBase<hardware_interface::EffortJointInterface>::init(robot, n);
1414

1515
// find stiffness (dummy) joints; this is necessary until proper position/stiffness/damping interface exists
16-
for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin(); it != kdl_chain_.segments.end(); ++it)
17-
{
18-
joint_stiffness_handles_.push_back(robot->getHandle(it->getJoint().getName()+"_stiffness"));
19-
}
16+
// for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin(); it != kdl_chain_.segments.end(); ++it)
17+
// {
18+
// joint_stiffness_handles_.push_back(robot->getHandle(it->getJoint().getName()+"_stiffness"));
19+
// }
2020

21-
ROS_DEBUG("found %lu stiffness handles", joint_stiffness_handles_.size());
21+
// ROS_DEBUG("found %lu stiffness handles", joint_stiffness_handles_.size());
2222

23-
previous_stiffness_.resize(joint_stiffness_handles_.size());
23+
// previous_stiffness_.resize(joint_stiffness_handles_.size());
2424

25-
return true;
25+
return true;
2626
}
2727

2828
void GravityCompensation::starting(const ros::Time& time)
2929
{
30-
for(size_t i=0; i<joint_handles_.size(); i++)
31-
{
32-
previous_stiffness_[i] = joint_stiffness_handles_[i].getPosition();
33-
}
30+
// for(size_t i=0; i<joint_handles_.size(); i++)
31+
// {
32+
// previous_stiffness_[i] = joint_stiffness_handles_[i].getPosition();
33+
// }
3434
}
3535

3636
void GravityCompensation::update(const ros::Time& time, const ros::Duration& period)
@@ -40,18 +40,18 @@ namespace lwr_controllers
4040
// is raised again
4141
for(size_t i=0; i<joint_handles_.size(); i++)
4242
{
43-
joint_handles_[i].setCommand(joint_handles_[i].getPosition());
44-
joint_stiffness_handles_[i].setCommand(DEFAULT_STIFFNESS);
45-
46-
}
43+
//joint_handles_[i].setCommand(joint_handles_[i].getPosition());
44+
joint_handles_[i].setCommand( 0.0 );
45+
// joint_stiffness_handles_[i].setCommand(DEFAULT_STIFFNESS);
46+
}
4747
}
4848

4949
void GravityCompensation::stopping(const ros::Time& time)
5050
{
51-
for(size_t i=0; i<joint_handles_.size(); i++)
52-
{
53-
joint_stiffness_handles_[i].setCommand(previous_stiffness_[i]);
54-
}
51+
//for(size_t i=0; i<joint_handles_.size(); i++)
52+
//{
53+
// joint_stiffness_handles_[i].setCommand(previous_stiffness_[i]);
54+
//}
5555
}
5656

5757
}

0 commit comments

Comments
 (0)