From 3149b743e9ad6159c3be24fc1e78b27aa9915165 Mon Sep 17 00:00:00 2001
From: Danfoa <daniels.ordonez@gmail.com>
Date: Wed, 9 May 2018 22:26:57 -0500
Subject: [PATCH 1/5] Configuration files included for real robot operation,
 update of dependencies, and inclusion of Track-IK kinematics plugin for both
 arms move groups to improve solever performance

---
 motoman_csda10f_moveit_config/CMakeLists.txt  | 15 +++++-
 .../config/controllers.yaml                   | 48 +++++++++++++++++++
 .../config/kinematics.yaml                    |  6 ++-
 .../launch/moveit_planning_execution.launch   | 48 +++++++++++++++++++
 motoman_csda10f_moveit_config/package.xml     | 20 ++++++--
 .../config/csda10f_motion_interface.yaml      | 11 +++--
 .../config/joint_names_csda10f.yaml           |  3 +-
 .../launch/robot_enable.launch                |  4 ++
 .../robot_interface_streaming_csda10f.launch  | 18 +++++--
 .../robot_state_visualize_csda10f.launch      | 30 +++++++-----
 10 files changed, 173 insertions(+), 30 deletions(-)
 create mode 100644 motoman_csda10f_moveit_config/config/controllers.yaml
 create mode 100644 motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch
 create mode 100644 motoman_csda10f_support/launch/robot_enable.launch

diff --git a/motoman_csda10f_moveit_config/CMakeLists.txt b/motoman_csda10f_moveit_config/CMakeLists.txt
index 4e801e2..c9e2f91 100644
--- a/motoman_csda10f_moveit_config/CMakeLists.txt
+++ b/motoman_csda10f_moveit_config/CMakeLists.txt
@@ -1,9 +1,20 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(motoman_csda10f_moveit_config)
 
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED
+  trac_ik_kinematics_plugin             ## Kinematics solver for both arms
+  moveit_core
+  moveit_ros_planning
+  moveit_ros_planning_interface
+  pluginlib
+)
 
-catkin_package()
+catkin_package(
+  CATKIN_DEPENDS 
+    trac_ik_kinematics_plugin 
+    moveit_core
+    moveit_ros_planning_interface
+)
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   PATTERN "setup_assistant.launch" EXCLUDE)
diff --git a/motoman_csda10f_moveit_config/config/controllers.yaml b/motoman_csda10f_moveit_config/config/controllers.yaml
new file mode 100644
index 0000000..f0ce146
--- /dev/null
+++ b/motoman_csda10f_moveit_config/config/controllers.yaml
@@ -0,0 +1,48 @@
+controller_list:
+  - name: ''
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [arm_left_joint_1_s,
+            arm_left_joint_2_l,
+            arm_left_joint_3_e,
+            arm_left_joint_4_u,
+            arm_left_joint_5_r,
+            arm_left_joint_6_b,
+            arm_left_joint_7_t,
+            arm_right_joint_1_s,
+            arm_right_joint_2_l,
+            arm_right_joint_3_e,
+            arm_right_joint_4_u,
+            arm_right_joint_5_r,
+            arm_right_joint_6_b,
+            arm_right_joint_7_t,
+            torso_joint_b1,
+            torso_joint_b2]
+  - name: csda10f/csda10f_r1_controller
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [arm_left_joint_1_s,
+            arm_left_joint_2_l,
+            arm_left_joint_3_e,
+            arm_left_joint_4_u,
+            arm_left_joint_5_r,
+            arm_left_joint_6_b,
+            arm_left_joint_7_t]
+  - name: csda10f/csda10f_b1_controller
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [torso_joint_b1]
+  - name: csda10f/csda10f_b2_controller
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [torso_joint_b2]
+  - name: csda10f/csda10f_r2_controller
+    action_ns: joint_trajectory_action
+    type: FollowJointTrajectory
+    joints: [arm_right_joint_1_s,
+            arm_right_joint_2_l,
+            arm_right_joint_3_e,
+            arm_right_joint_4_u,
+            arm_right_joint_5_r,
+            arm_right_joint_6_b,
+            arm_right_joint_7_t]
diff --git a/motoman_csda10f_moveit_config/config/kinematics.yaml b/motoman_csda10f_moveit_config/config/kinematics.yaml
index 5f81e65..1668d32 100644
--- a/motoman_csda10f_moveit_config/config/kinematics.yaml
+++ b/motoman_csda10f_moveit_config/config/kinematics.yaml
@@ -1,13 +1,15 @@
 arm_left:
-  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
   kinematics_solver_attempts: 3
+  solve_type: Speed
 arm_right:
-  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
   kinematics_solver_attempts: 3
+  solve_type: Speed 
 torso:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
diff --git a/motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch b/motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch
new file mode 100644
index 0000000..edae688
--- /dev/null
+++ b/motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch
@@ -0,0 +1,48 @@
+<launch>
+  <!-- The planning and execution components of MoveIt! configured to run -->
+  <!-- using the ROS-Industrial interface. -->
+ 
+  <rosparam command="load" file="$(find motoman_csda10f_support)/config/joint_names_csda10f.yaml"/>
+ 
+  <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
+  <!--  - if sim=false, a robot_ip and controller(fs100|dx100) arguments is required -->
+  <arg name="sim" default="false" />
+  <arg name="robot_ip" unless="$(arg sim)"  doc="IP of controller" />
+  <arg name="controller" unless="$(arg sim)" default="fs100" doc="Series of the controller (dx100, fs100)" />
+ 
+  <!-- load the robot_description parameter before launching ROS-I nodes -->
+  <include file="$(find motoman_csda10f_moveit_config)/launch/planning_context.launch" >
+    <arg name="load_robot_description" value="true" />
+  </include>
+
+  <!-- run the robot simulator and action interface nodes -->
+  <group if="$(arg sim)">
+    <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
+  </group>
+
+  <!-- run the "real robot" interface nodes -->
+  <!--   - this typically includes: robot_state, motion_interface, and joint_7_trajectory_action nodes -->
+  <!--   - replace these calls with appropriate robot-specific calls or launch files -->
+  <group unless="$(arg sim)">
+    <include file="$(find motoman_csda10f_support)/launch/robot_interface_streaming_csda10f.launch" >
+      <arg name="robot_ip" value="$(arg robot_ip)"/>
+      <arg name="controller" value="$(arg controller)"/>
+    </include>
+  </group>
+
+  <!-- publish the robot state (tf transforms) -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+  <include file="$(find motoman_csda10f_moveit_config)/launch/move_group.launch">
+    <arg name="publish_monitored_planning_scene" value="true" />
+  </include>
+
+  <include file="$(find motoman_csda10f_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="config" value="true"/>
+  </include>
+  
+  <!--mongo warehouse is disable but can be enable-->
+  <!--<include file="$(find invite_motoman_moveit_config)/launch/default_warehouse_db.launch" />-->
+
+</launch>
+
diff --git a/motoman_csda10f_moveit_config/package.xml b/motoman_csda10f_moveit_config/package.xml
index 684163e..46dcbbb 100644
--- a/motoman_csda10f_moveit_config/package.xml
+++ b/motoman_csda10f_moveit_config/package.xml
@@ -7,27 +7,37 @@
   </description>
   <author email="amrith1007@gmail.com">Amrith Ganesh</author>
   <maintainer email="amrith1007@gmail.com">Amrith Ganesh</maintainer>
+  <maintainer email="daniels.ordonez@gmail.com">Daniel Felipe Ordonez</maintainer>
 
   <license>BSD</license>
 
-  <url type="website">http://moveit.ros.org/</url>
-  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
-  <url type="repository">https://github.com/ros-planning/moveit</url>
+  <url type="website">https://github.com/ros-industrial/motoman_experimental</url>
+  <url type="bugtracker">https://github.com/ros-industrial/motoman_experimental/issues</url>
+  <url type="repository">https://github.com/ros-industrial/motoman_experimental</url>
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <run_depend>moveit_core</run_depend>
   <run_depend>moveit_ros_move_group</run_depend>
   <run_depend>moveit_fake_controller_manager</run_depend>
   <run_depend>moveit_kinematics</run_depend>
   <run_depend>moveit_planners_ompl</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>pluginlib</run_depend>
   <run_depend>joint_state_publisher</run_depend>
   <run_depend>robot_state_publisher</run_depend>
   <run_depend>xacro</run_depend>
+  <run_depend>trac_ik_kinematics_plugin</run_depend>
+  <run_depend>motoman_csda10f_support</run_depend>
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
   <build_depend>motoman_csda10f_support</build_depend>
-  <run_depend>motoman_csda10f_support</run_depend>
-
+  <build_depend>pluginlib</build_depend>
+  <build_depend>moveit_core</build_depend>
+  <build_depend>moveit_ros_planning_interface</build_depend>
+  <build_depend>geometric_shapes</build_depend>
+  <build_depend>moveit_visual_tools</build_depend>
+  <!-- Kinematic solver dependency for both arms move groups -->
+  <build_depend>trac_ik_kinematics_plugin</build_depend>
 
 </package>
diff --git a/motoman_csda10f_support/config/csda10f_motion_interface.yaml b/motoman_csda10f_support/config/csda10f_motion_interface.yaml
index 2b9883e..730a54c 100644
--- a/motoman_csda10f_support/config/csda10f_motion_interface.yaml
+++ b/motoman_csda10f_support/config/csda10f_motion_interface.yaml
@@ -1,17 +1,20 @@
+# Ordering from 0 to 4 is required due to real controller configuration (might dverge from controller configurations).
 topic_list:
     - name: csda10f_r1_controller
       ns: csda10f
-      group: 1
+      group: 0
       joints: ['arm_left_joint_1_s','arm_left_joint_2_l','arm_left_joint_3_e','arm_left_joint_4_u','arm_left_joint_5_r','arm_left_joint_6_b','arm_left_joint_7_t']
     - name: csda10f_r2_controller
       ns: csda10f
-      group: 2
+      group: 1
       joints: ['arm_right_joint_1_s','arm_right_joint_2_l','arm_right_joint_3_e','arm_right_joint_4_u','arm_right_joint_5_r','arm_right_joint_6_b','arm_right_joint_7_t']
     - name: csda10f_b1_controller
       ns: csda10f
-      group: 3
+      group: 2
       joints: ['torso_joint_b1']
     - name: csda10f_b2_controller
       ns: csda10f
-      group: 4
+      group: 3
       joints: ['torso_joint_b2']
+
+       
\ No newline at end of file
diff --git a/motoman_csda10f_support/config/joint_names_csda10f.yaml b/motoman_csda10f_support/config/joint_names_csda10f.yaml
index b517d00..e569384 100644
--- a/motoman_csda10f_support/config/joint_names_csda10f.yaml
+++ b/motoman_csda10f_support/config/joint_names_csda10f.yaml
@@ -1,4 +1,5 @@
+# The order of this list should match that expected by your robot's socket interface. Typically Base -> Tip.
 controller_joint_names: ['arm_left_joint_1_s','arm_left_joint_2_l','arm_left_joint_3_e'
 ,'arm_left_joint_4_u','arm_left_joint_5_r','arm_left_joint_6_b','arm_left_joint_7_t',
 'arm_right_joint_1_s','arm_right_joint_2_l','arm_right_joint_3_e','arm_right_joint_4_u',
-'arm_right_joint_5_r','arm_right_joint_6_b','arm_right_joint_7_t','torso_joint_b1','torso_joint_b2']
+'arm_right_joint_5_r','arm_right_joint_6_b','arm_right_joint_7_t','torso_joint_b1','torso_joint_b2']
\ No newline at end of file
diff --git a/motoman_csda10f_support/launch/robot_enable.launch b/motoman_csda10f_support/launch/robot_enable.launch
new file mode 100644
index 0000000..fa080d2
--- /dev/null
+++ b/motoman_csda10f_support/launch/robot_enable.launch
@@ -0,0 +1,4 @@
+<!--This node allows the real robot to take motion commands, without it it will not move -->
+<launch>
+<node pkg="rosservice" type="rosservice" name="robot_enable" args="call --wait /robot_enable"/>
+</launch>
\ No newline at end of file
diff --git a/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch b/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch
index e74f387..83740ac 100644
--- a/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch
+++ b/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch
@@ -3,19 +3,27 @@
   Manipulator specific version of 'robot_interface_streaming.launch'.
 
   Defaults provided for csda10f:
-   - 7 joints
+   - 15 joints
 
   Usage:
-    robot_interface_streaming_csda10f.launch robot_ip:=<value> controller:=<fs100|dx100>
+    robot_interface_streaming_cda10f.launch robot_ip:=<value> controller:=<fs100|dx100>
 -->
 <launch>
-  <arg name="robot_ip" />
+  <arg name="robot_ip" doc="IP of controller" />
 	
   <!-- controller: Controller name (fs100 or dx100) -->
-  <arg name="controller"/>
+  <arg name="controller" doc="Series of the controller (DX100/FS100, default is fs100)" />
 
+  <!-- Load the topic_list parapemeter containing the definition of the motion groups and joints mapping to the 
+      real controller ros application-->
   <rosparam command="load" file="$(find motoman_csda10f_support)/config/csda10f_motion_interface.yaml" />
-
+  
+  <!-- Load the custom naming of robot joints (param name = controller_joint_names)-->
+  <!--TO-DO: Not sure if this parameter should be loaded here-->
+  <rosparam command="load" file="$(find motoman_csda10f_support)/config/joint_names_csda10f.yaml" />
+  
+  <!-- Start the comunication with the robot controller, in this case multigroup since the csda10f is a dual-arm
+       robot-->
   <include file="$(find motoman_driver)/launch/robot_multigroup_interface_streaming_$(arg controller).launch">
     <arg name="robot_ip"   value="$(arg robot_ip)" />
 	</include>
diff --git a/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch b/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch
index 73611a5..a6a8343 100644
--- a/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch
+++ b/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch
@@ -3,28 +3,36 @@
   Manipulator specific version of the state visualizer.
 
   Defaults provided for csda10f:
-   - 7 joints
+   - 15 joints
 
   Usage:
-    robot_state_visualize_sia20d.launch robot_ip:=<value> controller:=<fs100|dx100>
+    robot_state_visualize_csda10f.launch robot_ip:=<value> controller:=<fs100|dx100>
 -->
 <launch>
-	<arg name="robot_ip" />
-
+	<arg name="robot_ip" doc="IP of controller" />
   <!-- controller: Controller name (fs100 or dx100) -->
-  <arg name="controller" />
+  <arg name="controller" default="fs100" doc="Series of the controller (dx100, fs100)" />
+  <!-- Parameters for robot description (URDF)-->
+  <param name="robot_description" command="$(find xacro)/xacro.py '$(find motoman_csda10f_support)/urdf/csda10f.xacro'" />
 
+  <!-- Load the custom naming of robot joints (param name = controller_joint_names)-->
   <rosparam command="load" file="$(find motoman_csda10f_support)/config/joint_names_csda10f.yaml" />
+  
+  <!-- Load the topic_list parapemeter containing the definition of the motion groups and joints mapping to the 
+       real controller ros application-->
   <rosparam command="load" file="$(find motoman_csda10f_support)/config/csda10f_motion_interface.yaml" />
 
+  <!-- Run the controler robot state launch file of the motoman_driver package to initiate communication-->
 	<include file="$(find motoman_driver)/launch/robot_state_$(arg controller).launch">
-		<arg name="robot_ip"   value="$(arg robot_ip)" />
+		<arg name="robot_ip" value="$(arg robot_ip)" />
 	</include>
 
-	<node name="robot_state_publisher" pkg="robot_state_publisher" 
-		type="robot_state_publisher" />
-
-	<include file="$(find motoman_csda10f_support)/launch/load_csda10f.launch" />
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 
-	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
+  <!-- Launch Moveit with Rviz for real-time robot state visualization and planning generation-->
+  <include file="$(find invite_motoman_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="config" value="true"/>
+  </include>
+  
+  <include file="$(find invite_motoman_moveit_config)/launch/default_warehouse_db.launch" />
 </launch>

From 408152d48f5e30f8e2ce1cca94d25ef54b515ca8 Mon Sep 17 00:00:00 2001
From: danfoa <daniels.ordonez@gmail.com>
Date: Mon, 14 May 2018 19:08:27 -0500
Subject: [PATCH 2/5] Update dependencies

---
 motoman_csda10f_moveit_config/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/motoman_csda10f_moveit_config/package.xml b/motoman_csda10f_moveit_config/package.xml
index 46dcbbb..b759a2f 100644
--- a/motoman_csda10f_moveit_config/package.xml
+++ b/motoman_csda10f_moveit_config/package.xml
@@ -29,6 +29,7 @@
   <run_depend>xacro</run_depend>
   <run_depend>trac_ik_kinematics_plugin</run_depend>
   <run_depend>motoman_csda10f_support</run_depend>
+  <run_depend>moveit_ros_planning_interface</run_depend>
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
   <build_depend>motoman_csda10f_support</build_depend>

From 31dada43b53d3ed3e50f6e146747a608f8190a33 Mon Sep 17 00:00:00 2001
From: Daniel Ordonez <daniels.ordonez@gmail.com>
Date: Fri, 16 Nov 2018 14:36:57 +0100
Subject: [PATCH 3/5] Update CSDA10F packages to work with FS100 controllers -
 controllers.yaml file added and tested on FS100 controller. -
 moveit_planning_excecution.launch file added with parameters to communicate
 with FS100 controller. - Correction and addition of comments

---
 motoman_csda10f_moveit_config/CMakeLists.txt  | 15 ++---------
 .../config/controllers.yaml                   |  2 +-
 .../config/kinematics.yaml                    |  6 ++---
 ...da10f_moveit_controller_manager.launch.xml |  6 +++++
 .../launch/moveit_planning_execution.launch   | 22 +++++++++++-----
 motoman_csda10f_moveit_config/package.xml     | 26 ++++++-------------
 .../config/csda10f_motion_interface.yaml      |  4 +--
 .../config/joint_names_csda10f.yaml           |  2 +-
 .../launch/robot_enable.launch                |  4 ---
 .../robot_interface_streaming_csda10f.launch  | 15 +++--------
 .../robot_state_visualize_csda10f.launch      | 25 +++++++-----------
 .../launch/test_csda10f.launch                |  4 +++
 motoman_csda10f_support/package.xml           |  6 ++++-
 13 files changed, 58 insertions(+), 79 deletions(-)
 delete mode 100644 motoman_csda10f_support/launch/robot_enable.launch

diff --git a/motoman_csda10f_moveit_config/CMakeLists.txt b/motoman_csda10f_moveit_config/CMakeLists.txt
index c9e2f91..4e801e2 100644
--- a/motoman_csda10f_moveit_config/CMakeLists.txt
+++ b/motoman_csda10f_moveit_config/CMakeLists.txt
@@ -1,20 +1,9 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(motoman_csda10f_moveit_config)
 
-find_package(catkin REQUIRED
-  trac_ik_kinematics_plugin             ## Kinematics solver for both arms
-  moveit_core
-  moveit_ros_planning
-  moveit_ros_planning_interface
-  pluginlib
-)
+find_package(catkin REQUIRED)
 
-catkin_package(
-  CATKIN_DEPENDS 
-    trac_ik_kinematics_plugin 
-    moveit_core
-    moveit_ros_planning_interface
-)
+catkin_package()
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   PATTERN "setup_assistant.launch" EXCLUDE)
diff --git a/motoman_csda10f_moveit_config/config/controllers.yaml b/motoman_csda10f_moveit_config/config/controllers.yaml
index f0ce146..4d88c9b 100644
--- a/motoman_csda10f_moveit_config/config/controllers.yaml
+++ b/motoman_csda10f_moveit_config/config/controllers.yaml
@@ -45,4 +45,4 @@ controller_list:
             arm_right_joint_4_u,
             arm_right_joint_5_r,
             arm_right_joint_6_b,
-            arm_right_joint_7_t]
+            arm_right_joint_7_t]
\ No newline at end of file
diff --git a/motoman_csda10f_moveit_config/config/kinematics.yaml b/motoman_csda10f_moveit_config/config/kinematics.yaml
index 1668d32..5f81e65 100644
--- a/motoman_csda10f_moveit_config/config/kinematics.yaml
+++ b/motoman_csda10f_moveit_config/config/kinematics.yaml
@@ -1,15 +1,13 @@
 arm_left:
-  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
   kinematics_solver_attempts: 3
-  solve_type: Speed
 arm_right:
-  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
   kinematics_solver_timeout: 0.005
   kinematics_solver_attempts: 3
-  solve_type: Speed 
 torso:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
diff --git a/motoman_csda10f_moveit_config/launch/motoman_csda10f_moveit_controller_manager.launch.xml b/motoman_csda10f_moveit_config/launch/motoman_csda10f_moveit_controller_manager.launch.xml
index 5d02698..452a8c9 100644
--- a/motoman_csda10f_moveit_config/launch/motoman_csda10f_moveit_controller_manager.launch.xml
+++ b/motoman_csda10f_moveit_config/launch/motoman_csda10f_moveit_controller_manager.launch.xml
@@ -1,3 +1,9 @@
 <launch>
 
+  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
+  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
+  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
+  <!-- load controller_list -->
+  <rosparam file="$(find motoman_csda10f_moveit_config)/config/controllers.yaml"/>
+
 </launch>
diff --git a/motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch b/motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch
index edae688..7c9afd7 100644
--- a/motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch
+++ b/motoman_csda10f_moveit_config/launch/moveit_planning_execution.launch
@@ -1,20 +1,23 @@
 <launch>
+  <!--<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}]: ${message}"/> -->
+
+  
   <!-- The planning and execution components of MoveIt! configured to run -->
   <!-- using the ROS-Industrial interface. -->
- 
   <rosparam command="load" file="$(find motoman_csda10f_support)/config/joint_names_csda10f.yaml"/>
  
   <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
   <!--  - if sim=false, a robot_ip and controller(fs100|dx100) arguments is required -->
   <arg name="sim" default="false" />
-  <arg name="robot_ip" unless="$(arg sim)"  doc="IP of controller" />
+  <arg name="robot_enable" default="false" doc="Set to true if you want to enable robot at start"/>
+  <arg name="robot_ip" unless="$(arg sim)" doc="IP of controller" />
   <arg name="controller" unless="$(arg sim)" default="fs100" doc="Series of the controller (dx100, fs100)" />
  
   <!-- load the robot_description parameter before launching ROS-I nodes -->
   <include file="$(find motoman_csda10f_moveit_config)/launch/planning_context.launch" >
     <arg name="load_robot_description" value="true" />
   </include>
-
+ 
   <!-- run the robot simulator and action interface nodes -->
   <group if="$(arg sim)">
     <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
@@ -34,15 +37,20 @@
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 
   <include file="$(find motoman_csda10f_moveit_config)/launch/move_group.launch">
-    <arg name="publish_monitored_planning_scene" value="true" />
+    <arg name="publish_monitored_planning_scene" value="true"/>
+    <arg name="fake_execution" if="$(arg sim)" value="true"/>
   </include>
 
   <include file="$(find motoman_csda10f_moveit_config)/launch/moveit_rviz.launch">
     <arg name="config" value="true"/>
   </include>
   
-  <!--mongo warehouse is disable but can be enable-->
-  <!--<include file="$(find invite_motoman_moveit_config)/launch/default_warehouse_db.launch" />-->
-
+  <!--<include file="$(find motoman_csda10f_moveit_config)/launch/default_warehouse_db.launch" />-->
+  
+  <!-- Enable robot if requested -->
+  <group if="$(arg robot_enable)">
+    <include file="$(find motoman_csda10f_support)/launch/robot_enable.launch" />
+  </group>
+  
 </launch>
 
diff --git a/motoman_csda10f_moveit_config/package.xml b/motoman_csda10f_moveit_config/package.xml
index b759a2f..e1ad137 100644
--- a/motoman_csda10f_moveit_config/package.xml
+++ b/motoman_csda10f_moveit_config/package.xml
@@ -1,44 +1,34 @@
 <package>
 
   <name>motoman_csda10f_moveit_config</name>
-  <version>0.3.0</version>
+  <version>0.2.0</version>
   <description>
-     An automatically generated package with all the configuration and launch files for using the motoman_csda10f with the MoveIt! Motion Planning Framework
+     Package with all the configuration and launch files for using the motoman_csda10f with the MoveIt! Motion Planning Framework, and ready to work with FS100 controller.
   </description>
   <author email="amrith1007@gmail.com">Amrith Ganesh</author>
   <maintainer email="amrith1007@gmail.com">Amrith Ganesh</maintainer>
-  <maintainer email="daniels.ordonez@gmail.com">Daniel Felipe Ordonez</maintainer>
+  <maintainer email="daniels.ordonez@gmail.com">Daniel Ordonez</maintainer>
 
   <license>BSD</license>
 
-  <url type="website">https://github.com/ros-industrial/motoman_experimental</url>
-  <url type="bugtracker">https://github.com/ros-industrial/motoman_experimental/issues</url>
-  <url type="repository">https://github.com/ros-industrial/motoman_experimental</url>
+  <url type="website">http://wiki.ros.org/motoman_sda10f_moveit_config</url>
+  <url type="bugtracker">https://github.com/ros-industrial/motoman/issues</url>
+  <url type="repository">https://github.com/ros-industrial/motoman</url>
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <run_depend>moveit_core</run_depend>
   <run_depend>moveit_ros_move_group</run_depend>
   <run_depend>moveit_fake_controller_manager</run_depend>
   <run_depend>moveit_kinematics</run_depend>
   <run_depend>moveit_planners_ompl</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
-  <run_depend>pluginlib</run_depend>
   <run_depend>joint_state_publisher</run_depend>
   <run_depend>robot_state_publisher</run_depend>
   <run_depend>xacro</run_depend>
-  <run_depend>trac_ik_kinematics_plugin</run_depend>
-  <run_depend>motoman_csda10f_support</run_depend>
-  <run_depend>moveit_ros_planning_interface</run_depend>
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
   <build_depend>motoman_csda10f_support</build_depend>
-  <build_depend>pluginlib</build_depend>
-  <build_depend>moveit_core</build_depend>
-  <build_depend>moveit_ros_planning_interface</build_depend>
-  <build_depend>geometric_shapes</build_depend>
-  <build_depend>moveit_visual_tools</build_depend>
-  <!-- Kinematic solver dependency for both arms move groups -->
-  <build_depend>trac_ik_kinematics_plugin</build_depend>
+  <run_depend>motoman_csda10f_support</run_depend>
+
 
 </package>
diff --git a/motoman_csda10f_support/config/csda10f_motion_interface.yaml b/motoman_csda10f_support/config/csda10f_motion_interface.yaml
index 730a54c..c246739 100644
--- a/motoman_csda10f_support/config/csda10f_motion_interface.yaml
+++ b/motoman_csda10f_support/config/csda10f_motion_interface.yaml
@@ -1,4 +1,4 @@
-# Ordering from 0 to 4 is required due to real controller configuration (might dverge from controller configurations).
+# Ordering from 0 to 4 is required due to FS100 controller configuration, but it can be that ordering starting from 1 works for you.
 topic_list:
     - name: csda10f_r1_controller
       ns: csda10f
@@ -16,5 +16,3 @@ topic_list:
       ns: csda10f
       group: 3
       joints: ['torso_joint_b2']
-
-       
\ No newline at end of file
diff --git a/motoman_csda10f_support/config/joint_names_csda10f.yaml b/motoman_csda10f_support/config/joint_names_csda10f.yaml
index e569384..bdd124f 100644
--- a/motoman_csda10f_support/config/joint_names_csda10f.yaml
+++ b/motoman_csda10f_support/config/joint_names_csda10f.yaml
@@ -2,4 +2,4 @@
 controller_joint_names: ['arm_left_joint_1_s','arm_left_joint_2_l','arm_left_joint_3_e'
 ,'arm_left_joint_4_u','arm_left_joint_5_r','arm_left_joint_6_b','arm_left_joint_7_t',
 'arm_right_joint_1_s','arm_right_joint_2_l','arm_right_joint_3_e','arm_right_joint_4_u',
-'arm_right_joint_5_r','arm_right_joint_6_b','arm_right_joint_7_t','torso_joint_b1','torso_joint_b2']
\ No newline at end of file
+'arm_right_joint_5_r','arm_right_joint_6_b','arm_right_joint_7_t','torso_joint_b1','torso_joint_b2']
diff --git a/motoman_csda10f_support/launch/robot_enable.launch b/motoman_csda10f_support/launch/robot_enable.launch
deleted file mode 100644
index fa080d2..0000000
--- a/motoman_csda10f_support/launch/robot_enable.launch
+++ /dev/null
@@ -1,4 +0,0 @@
-<!--This node allows the real robot to take motion commands, without it it will not move -->
-<launch>
-<node pkg="rosservice" type="rosservice" name="robot_enable" args="call --wait /robot_enable"/>
-</launch>
\ No newline at end of file
diff --git a/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch b/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch
index 83740ac..33d26ab 100644
--- a/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch
+++ b/motoman_csda10f_support/launch/robot_interface_streaming_csda10f.launch
@@ -1,4 +1,3 @@
-
 <!--
   Manipulator specific version of 'robot_interface_streaming.launch'.
 
@@ -9,21 +8,13 @@
     robot_interface_streaming_cda10f.launch robot_ip:=<value> controller:=<fs100|dx100>
 -->
 <launch>
-  <arg name="robot_ip" doc="IP of controller" />
+  <arg name="robot_ip" doc="IP of controller"/>
 	
   <!-- controller: Controller name (fs100 or dx100) -->
-  <arg name="controller" doc="Series of the controller (DX100/FS100, default is fs100)" />
+  <arg name="controller"/>
 
-  <!-- Load the topic_list parapemeter containing the definition of the motion groups and joints mapping to the 
-      real controller ros application-->
   <rosparam command="load" file="$(find motoman_csda10f_support)/config/csda10f_motion_interface.yaml" />
-  
-  <!-- Load the custom naming of robot joints (param name = controller_joint_names)-->
-  <!--TO-DO: Not sure if this parameter should be loaded here-->
-  <rosparam command="load" file="$(find motoman_csda10f_support)/config/joint_names_csda10f.yaml" />
-  
-  <!-- Start the comunication with the robot controller, in this case multigroup since the csda10f is a dual-arm
-       robot-->
+
   <include file="$(find motoman_driver)/launch/robot_multigroup_interface_streaming_$(arg controller).launch">
     <arg name="robot_ip"   value="$(arg robot_ip)" />
 	</include>
diff --git a/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch b/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch
index a6a8343..80c377a 100644
--- a/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch
+++ b/motoman_csda10f_support/launch/robot_state_visualize_csda10f.launch
@@ -10,29 +10,24 @@
 -->
 <launch>
 	<arg name="robot_ip" doc="IP of controller" />
+
   <!-- controller: Controller name (fs100 or dx100) -->
-  <arg name="controller" default="fs100" doc="Series of the controller (dx100, fs100)" />
-  <!-- Parameters for robot description (URDF)-->
-  <param name="robot_description" command="$(find xacro)/xacro.py '$(find motoman_csda10f_support)/urdf/csda10f.xacro'" />
+  <arg name="controller" default="fs100" doc="Series of the controller (dx100, fs100)"/>
 
   <!-- Load the custom naming of robot joints (param name = controller_joint_names)-->
   <rosparam command="load" file="$(find motoman_csda10f_support)/config/joint_names_csda10f.yaml" />
-  
-  <!-- Load the topic_list parapemeter containing the definition of the motion groups and joints mapping to the 
-       real controller ros application-->
+  <!-- Load the topic_list parapemeter containing the definition of the motion groups and joints mapping to the real controller ros application-->
   <rosparam command="load" file="$(find motoman_csda10f_support)/config/csda10f_motion_interface.yaml" />
 
-  <!-- Run the controler robot state launch file of the motoman_driver package to initiate communication-->
+  <!-- Obtain the robot state from the robot controller-->
 	<include file="$(find motoman_driver)/launch/robot_state_$(arg controller).launch">
-		<arg name="robot_ip" value="$(arg robot_ip)" />
+		<arg name="robot_ip"   value="$(arg robot_ip)" />
 	</include>
 
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="robot_state_publisher" pkg="robot_state_publisher" 
+		type="robot_state_publisher" />
+
+	<include file="$(find motoman_csda10f_support)/launch/load_csda10f.launch" />
 
-  <!-- Launch Moveit with Rviz for real-time robot state visualization and planning generation-->
-  <include file="$(find invite_motoman_moveit_config)/launch/moveit_rviz.launch">
-    <arg name="config" value="true"/>
-  </include>
-  
-  <include file="$(find invite_motoman_moveit_config)/launch/default_warehouse_db.launch" />
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
 </launch>
diff --git a/motoman_csda10f_support/launch/test_csda10f.launch b/motoman_csda10f_support/launch/test_csda10f.launch
index 51caf91..efc3f45 100644
--- a/motoman_csda10f_support/launch/test_csda10f.launch
+++ b/motoman_csda10f_support/launch/test_csda10f.launch
@@ -1,8 +1,12 @@
 
 <launch>
+	<!-- Load robot description -->
 	<include file="$(find motoman_csda10f_support)/launch/load_csda10f.launch" />
+
 	<param name="use_gui" value="true" />
 	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
 	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	
+	<!-- Start Rviz-->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find industrial_robot_client)/config/robot_state_visualize.rviz" required="true" />
 </launch>
diff --git a/motoman_csda10f_support/package.xml b/motoman_csda10f_support/package.xml
index a2aa8b4..0f43fa9 100644
--- a/motoman_csda10f_support/package.xml
+++ b/motoman_csda10f_support/package.xml
@@ -18,7 +18,7 @@
   </ul>
   <p>
       Joint limits and maximum joint velocities are based on the information 
-      found in the online 
+      found in the online.
       All urdfs are based on the default motion and joint velocity limits, 
       unless noted otherwise.
     </p>
@@ -29,11 +29,15 @@
     </p>
  </description>
  <author email="amrith1007@gmail.com">Amrith Ganesh</author>
+ <maintainer email="daniels.ordonez@gmail.com">Amrith Ganesh</maintainer>
  <maintainer email="amrith1007@gmail.com">Amrith Ganesh</maintainer>
+
  <license>BSD</license>
+ 
  <url type="website">http://ros.org/wiki/motoman_sda10f_support</url>
  <url type="bugtracker">https://github.com/ros-industrial/motoman/issues</url>
  <url type="repository">https://github.com/ros-industrial/motoman</url>
+ 
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roslaunch</build_depend>
  <run_depend>motoman_driver</run_depend>

From cb1b360f9b1c8e97fa549a6f65bc86939d2f12cc Mon Sep 17 00:00:00 2001
From: Daniel Ordonez <daniels.ordonez@gmail.com>
Date: Mon, 17 Dec 2018 17:33:11 +0100
Subject: [PATCH 4/5] Fix arm macro: joint T axis direction was incorrectly
 oriented

---
 motoman_csda10f_support/urdf/arm_macro.xacro | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/motoman_csda10f_support/urdf/arm_macro.xacro b/motoman_csda10f_support/urdf/arm_macro.xacro
index a35dc7f..0dfbef0 100644
--- a/motoman_csda10f_support/urdf/arm_macro.xacro
+++ b/motoman_csda10f_support/urdf/arm_macro.xacro
@@ -162,7 +162,7 @@
 			<parent link="${prefix}link_6_b"/>
 			<child link="${prefix}link_7_t"/>
 			<origin xyz="0 0.118 -0.015" rpy="1.57 0 0"/>
-			<axis xyz="0 0 ${-reflect}" />
+			<axis xyz="0 0 ${reflect}" />
 			<limit lower="-3.13" upper="3.13" effort="0" velocity="6.97" />
 		</joint>
 

From 68c37c8707416cdffcc1feecdb694534f669aac1 Mon Sep 17 00:00:00 2001
From: Daniel Ordonez <daniels.ordonez@gmail.com>
Date: Fri, 15 Feb 2019 10:55:52 +0100
Subject: [PATCH 5/5] Correct CSDA10F arm link joints to avoid controller joint
 limit alarm

---
 motoman_csda10f_support/urdf/arm_macro.xacro | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/motoman_csda10f_support/urdf/arm_macro.xacro b/motoman_csda10f_support/urdf/arm_macro.xacro
index 0dfbef0..2af4f24 100644
--- a/motoman_csda10f_support/urdf/arm_macro.xacro
+++ b/motoman_csda10f_support/urdf/arm_macro.xacro
@@ -121,7 +121,7 @@
 			<child link="${prefix}link_1_s"/>
 			<origin xyz="0.1 ${reflect*0.023} 0.3381" rpy="1.57 0 ${(reflect-1)*1.57}"/>
 			<axis xyz="0 0 ${-reflect}" />
-			<limit lower="-3.13" upper="3.13" effort="0" velocity="2.95" />
+			<limit lower="-2.95" upper="2.95" effort="0" velocity="2.95" />
 		</joint>
 		<joint name="${prefix}joint_2_l" type="revolute">
 			<parent link="${prefix}link_1_s"/>
@@ -142,7 +142,7 @@
 			<child link="${prefix}link_4_u"/>
 			<origin xyz="0.00001 -0.0155 -0.185" rpy="-1.57 0 0"/>
 			<axis xyz="0 0 ${reflect}" />
-			<limit lower="-2.36" upper="2.36" effort="0" velocity="2.95" />
+			<limit lower="-2.35" upper="2.35" effort="0" velocity="2.95" />
 		</joint>
 		<joint name="${prefix}joint_5_r" type="revolute">
 			<parent link="${prefix}link_4_u"/>