Importing single_lwr_moveit, and update on several README files.

README.md

@@ -23,24 +23,3 @@ ToDo: one should check the commands in the [`.travis.yaml`](https://github.com/C
 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)
-
-
-(ToDo: perhaps the two sections below could be moved to the `lwr_hw` package.)
-## What to do to run a real LWR 4+
-
-1. Load the script [`lwr_hw/krl/ros_control.src`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_hw/krl/ros_control.src) and the corresponding [`.dat`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_hw/krl/ros_control.src) on the robot.  
-2. Place the robot in a position where the joints 1 and 3 are as bent as possible (at least 45 degrees) to avoid the "__FRI interpolation error__". A good way to check this is to go to gravity compensation, and see if the robot enters succesfully in that mode. 
-3. Set the robot in __Position__ control. 
-4. Start the script with the grey and green buttons; the scripts stops at a point and should be started again by releasing and pressing again the green button. You can also use the script in semi-automatic mode.  
-5. Once the script reaches the loop where it waits for commands, and then, you can proceed to launch your hardware interface + controllers + anything on top of it, e.g. moveit.
-
-## Gravity compensation
-(ToDo: revise this procedure after the merge)
-
-In this mode the robot can be moved manually, while the position of each joint is available in TF (__IMPORTANT__: due to the robot own software policies, this mode is only available in T1). This mode can be very useful for calibration, tracking, or teaching.  
-1. Load the script [`lwr_hw/krl/ros_monitor.src`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_hw/krl/ros_monitor.src) and the corresponding [`.dat`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_hw/krl/ros_monitor.dat) on the robot.
-2. Set the robot in __Position__ control (it will be changed to __Gravity Compensation__ inside the script).  
-3. Start the script with the grey and green buttons; the scripts stops at a point and should be started again by releasing and pressing again the green button (you can also use the script in automatic mode)  
-4. The script should reach the loop where it waits for commands
-5. Launch the hardware interface.
-6. Start the publisher node with 'roslaunch lwr_launch state_publisher.launch'

lwr_hw/README.md

@@ -1,6 +1,22 @@
-LWR HW
-======
+# lwr_hw
 
-This package acts much like a driver. It creates a hardware interface to the real robot, and it requires a URDF description in the `robot_description` parameter or configuration file defining the joint names that this hardware interface refers to.
+This package acts much like a driver. It creates a hardware interface to the real/sim robot, and it requires a URDF description in the `robot_description` parameter or configuration file defining the joint names that this hardware interface refers to.
 
-The robot controller must run the script found in the `krl` folder.
+## What to do to run a real LWR 4+
+
+1. Load the script [`lwr_hw/krl/ros_control.src`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_hw/krl/ros_control.src) and the corresponding [`.dat`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_hw/krl/ros_control.src) on the robot.  
+2. Place the robot in a position where the joints 1 and 3 are as bent as possible (at least 45 degrees) to avoid the "__FRI interpolation error__". A good way to check this is to go to gravity compensation, and see if the robot enters succesfully in that mode. 
+3. Set the robot in __Position__ control.
+4. Start the script with the grey and green buttons; the scripts stops at a point and should be started again by releasing and pressing again the green button. You can also use the script in semi-automatic mode.  
+5. Once the script reaches the loop where it waits for commands, and then, you can proceed to launch your hardware interface + controllers + anything on top of it, e.g. moveit.
+
+### Gravity compensation
+(ToDo: revise this procedure after the merge)
+
+In this mode the robot can be moved manually, while the position of each joint is available in TF (__IMPORTANT__: due to the robot own software policies, this mode is only available in T1). This mode can be very useful for calibration, tracking, or teaching.  
+1. Load the script [`lwr_hw/krl/ros_monitor.src`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_hw/krl/ros_monitor.src) and the corresponding [`.dat`](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_hw/krl/ros_monitor.dat) on the robot.
+2. Set the robot in __Position__ control (it will be changed to __Gravity Compensation__ inside the script).
+3. Start the script with the grey and green buttons; the scripts stops at a point and should be started again by releasing and pressing again the green button (you can also use the script in automatic mode)  
+4. The script should reach the loop where it waits for commands
+5. Launch the hardware interface.
+6. Start the publisher node with 'roslaunch lwr_launch state_publisher.launch'

single_lwr_example/README.md

@@ -0,0 +1,62 @@
+# single_lwr_example
+
+This package is a guide-trough-example to show how to use the [kuka_lwr](https://github.com/CentroEPiaggio/kuka-lwr) packages.
+
+## 1. Set your scenario
+
+Create you robot and environment using the lwr model. 
+
+For instance, in the package [__single_lwr_robot__](./single_lwr_robot/), the [robot](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/single_lwr_example/single_lwr_robot/robot/single_lwr_robot.urdf.xacro) is a lwr mounted on a box, and the [environment](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/single_lwr_example/single_lwr_robot/worlds/simple_environment.world) is just a ground plane with a sun.
+
+[Controllers](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/single_lwr_example/single_lwr_robot/config/controllers.yaml) and [joint names](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/single_lwr_example/single_lwr_robot/config/joint_names.yaml) are in the [config](https://github.com/CentroEPiaggio/kuka-lwr/tree/master/single_lwr_example/single_lwr_robot/config) folder. Note that the name space of controllers and the joint names contain the word `lwr`, which is the name you gave to the arm when creating the robot [here](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/single_lwr_example/single_lwr_robot/robot/single_lwr_robot.urdf.xacro#L36).
+
+Custom controllers can be found in the [__lwr_controllers__](https://github.com/CentroEPiaggio/kuka-lwr/tree/master/lwr_controllers) package.
+
+## 2. Configure MoveIt!
+
+Use the setup assistant to configure a MoveIt! package of your robot.
+
+We already did that in the package [__single_lwr_moveit__](./single_lwr_moveit/). We followed the instructions given in Section 3 [here](http://wiki.ros.org/Industrial/Tutorials/Create_a_MoveIt_Pkg_for_an_Industrial_Robot) to update the configuration files of MoveIt!.
+
+Note that the [name of the controller](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/single_lwr_example/single_lwr_moveit/config/controllers.yaml#L2) and [joint names](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/single_lwr_example/single_lwr_moveit/config/controllers.yaml#L7-13) must coincide with the name given in the robot package for the [controller](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/single_lwr_example/single_lwr_robot/config/controllers.yaml#L8) and [joint names](https://github.com/CentroEPiaggio/kuka-lwr/blob/master/single_lwr_example/single_lwr_robot/config/joint_names.yaml).
+
+__NOTE__: recall that to run a real LWR 4+, you must follow instructions in [lwr_hw](https://github.com/CentroEPiaggio/kuka-lwr/tree/master/lwr_hw).
+
+## 3. Set your launch control panel
+
+We encourage to have a separated package with your launch file to configure your different uses of your robot, including the network parameters.
+
+We have an example in the package [__single_lwr_launch__](./single_lwr_launch). To check which arguments are available use:
+
+`roslaunch --ros-args single_lwr_launch single_lwr.launch`
+
+Argument autocomplete was available in Groovy, but somehow it skipped Hydro, hence Indigo, but it is comming, see [this issue](https://github.com/ros/ros_comm/issues/575).
+
+## 4. Test in simulation
+
+a. Just bring your robot on:
+
+`roslaunch single_lwr_launch single_lwr.launch`
+
+By default, it does not load the MoveIt! configuration. Eventhough, you can open `rqt`, and open Plugins->Robot Tools->Joint trajectory controller and move the robot manually with slides.
+
+b. Testing MoveIt! the configuration in simulation:
+
+`roslaunch single_lwr_launch single_lwr.launch load_moveit:=true`
+
+## 5. Test in real
+
+a. Once everything went well in simulation, you can procede to use the real robot by using:
+
+`roslaunch single_lwr_launch single_lwr.launch use_lwr_sim:=false lwr_powered:=true`
+
+b. If you wish to use MoveIt!, you can potentially use:
+
+`roslaunch single_lwr_launch single_lwr.launch use_lwr_sim:=false lwr_powered:=true load_moveit:=true`
+
+If the robot complains a lot about bad communication quality, then just launch the robot with the previous command, and after everything has started, then use:
+
+`roslaunch single_lwr_moveit move_group.launch allow_trajectory_execution:=true fake_execution:=false info:=true debug:=false`
+
+And that's it!
+:metal:

single_lwr_example/single_lwr_launch/launch/single_lwr.launch

@@ -26,9 +26,7 @@
 	<!-- LAUNCH IMPLEMENTATION -->
 
 	<!-- the urdf/srdf parameter -->
-	<!-- include file="$(find single_lwr_robot)/launch/robot_upload.launch" /-->
 	<param name="robot_description" command="$(find xacro)/xacro.py $(find single_lwr_robot)/robot/$(arg robot_name).urdf.xacro"/>
-	<param name="robot_description_semantic" textfile="$(find single_lwr_moveit)/config/lwr.srdf"/>
 
 	<!-- joint and robot state publishers of the full robot description -->
 	<param name="use_gui" value="$(arg gui)"/>
@@ -67,6 +65,12 @@
             <arg name="info" value="true"/>
             <arg name="debug" value="false"/>
         </include>
+
+		<!-- run Rviz and load the default config to see the state of the move_group node -->
+		<include file="$(find single_lwr_moveit)/launch/moveit_rviz.launch">
+			<arg name="config" value="true"/>
+			<arg name="debug" value="false"/>
+		</include>
     </group>
 
     <!-- load all controller configurations to rosparam server-->
@@ -81,8 +85,8 @@
 			<rosparam command="load" file="$(find single_lwr_robot)/config/joint_names.yaml" />
 
 			<include file="$(find lwr_hw)/launch/lwr_hw.launch">
-				<arg name="port" value="$(port)"/>
-				<arg name="ip" value="192.168.0.20"/>
+				<arg name="port" value="$(arg port)"/>
+				<arg name="ip" value="$(arg ip)"/>
 				<arg name="use_default_joint_names" default="false"/>
 				<arg name="use_default_namespace" default="false"/>
 			</include>

single_lwr_example/single_lwr_moveit/.setup_assistant

@@ -0,0 +1,8 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: single_lwr_robot
+    relative_path: robot/single_lwr_robot.urdf.xacro
+  SRDF:
+    relative_path: config/single_lwr_robot.srdf
+  CONFIG:
+    generated_timestamp: 1433350719

single_lwr_example/single_lwr_moveit/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(single_lwr_moveit)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

single_lwr_example/single_lwr_moveit/config/controllers.yaml

@@ -0,0 +1,13 @@
+controller_list:
+  - name: /lwr/joint_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints: 
+      - lwr_0_joint
+      - lwr_1_joint
+      - lwr_2_joint
+      - lwr_3_joint
+      - lwr_4_joint
+      - lwr_5_joint
+      - lwr_6_joint

single_lwr_example/single_lwr_moveit/config/fake_controllers.yaml

@@ -0,0 +1,10 @@
+controller_list:
+  - name: fake_full_lwr_controller
+    joints:
+      - lwr_0_joint
+      - lwr_1_joint
+      - lwr_2_joint
+      - lwr_3_joint
+      - lwr_4_joint
+      - lwr_5_joint
+      - lwr_6_joint

single_lwr_example/single_lwr_moveit/config/joint_limits.yaml

@@ -0,0 +1,39 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  lwr_0_joint:
+    has_velocity_limits: true
+    max_velocity: 1.91986217719
+    has_acceleration_limits: false
+    max_acceleration: 0
+  lwr_1_joint:
+    has_velocity_limits: true
+    max_velocity: 1.91986217719
+    has_acceleration_limits: false
+    max_acceleration: 0
+  lwr_2_joint:
+    has_velocity_limits: true
+    max_velocity: 2.23402144255
+    has_acceleration_limits: false
+    max_acceleration: 0
+  lwr_3_joint:
+    has_velocity_limits: true
+    max_velocity: 2.23402144255
+    has_acceleration_limits: false
+    max_acceleration: 0
+  lwr_4_joint:
+    has_velocity_limits: true
+    max_velocity: 3.56047167407
+    has_acceleration_limits: false
+    max_acceleration: 0
+  lwr_5_joint:
+    has_velocity_limits: true
+    max_velocity: 3.21140582367
+    has_acceleration_limits: false
+    max_acceleration: 0
+  lwr_6_joint:
+    has_velocity_limits: true
+    max_velocity: 3.21140582367
+    has_acceleration_limits: false
+    max_acceleration: 0

single_lwr_example/single_lwr_moveit/config/kinematics.yaml

@@ -0,0 +1,5 @@
+full_lwr:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3

single_lwr_example/single_lwr_moveit/config/ompl_planning.yaml

@@ -0,0 +1,67 @@
+planner_configs:
+  SBLkConfigDefault:
+    type: geometric::SBL
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ESTkConfigDefault:
+    type: geometric::EST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  BKPIECEkConfigDefault:
+    type: geometric::BKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  KPIECEkConfigDefault:
+    type: geometric::KPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  RRTkConfigDefault:
+    type: geometric::RRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+  RRTConnectkConfigDefault:
+    type: geometric::RRTConnect
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  RRTstarkConfigDefault:
+    type: geometric::RRTstar
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
+  TRRTkConfigDefault:
+    type: geometric::TRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    max_states_failed: 10  # when to start increasing temp. default: 10
+    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
+    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
+    init_temperature: 10e-6  # initial temperature. default: 10e-6
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 
+    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
+  PRMkConfigDefault:
+    type: geometric::PRM
+    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
+  PRMstarkConfigDefault:
+    type: geometric::PRMstar
+full_lwr:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault