test dynamic reconfigure and update tuning guide

README.md

@@ -1,6 +1,6 @@
 # Kinova Gen3 Compliant Controllers
 
-This repository provides joint-space and task-space compliant ROS1 controllers for the Kinova Gen3 robot arm.
+This repository provides joint-space and task-space compliant ROS1 controllers with [model-free friction observers](https://ieeexplore.ieee.org/document/8781838) for the Kinova Gen3 robot arm.
 
 ## Requirements
 
@@ -41,9 +41,40 @@ Value: <enter joint value> # in radians
 # Ctrl + \ to exit
 ```
 
-## Tuning Guide
-TODO
+## Tuning Controller Parameters
 
-## Contributors
-TODO
+Ideally, the joint stiffness matrix $K_j$, rotor inertia matrix $B$, friction-observer gains $L, L_p$, should only be tuned once for a given robot and remain fixed after that. Although, at any point if you observe vibrations, these parameters may require retuning. Beyond this, you may tune the PD gains of the motor-side controller to modulate the compliance behavior. This can also be done on-the-fly depending on your use case.
 
+We provide support for tuning these parameters using [`dynamic_reconfigure`](http://wiki.ros.org/dynamic_reconfigure/Tutorials). The reconfigure node can also be accessed using a `dynamic_reconfigure` client ([tutorial](http://wiki.ros.org/dynamic_reconfigure/Tutorials/UsingTheDynamicReconfigurePythonClient)) to implement dynamic compliance behaviors.
+
+To tune the parameters using the slider GUI, start the controller using the previously provided commands and open another terminal where you run 
+
+```rosrun rqt_reconfigure rqt_reconfigure```  and select the `kortex_hardware` node to access the controller parameters.
+
+Upon successfully tuning the parameters, dump the parameters using 
+
+```rosparam get /kortex_hardware | grep -E '\b[bklj]|\b[d]_' | sed 's/^/kortex_hardware\//' > <path-to-gen3_compliant_controllers>/config/filename.yaml # regex ninja to get relevant parameters```.
+
+These parameters can then be loaded into the ROS parameter server using the following snippet in your launch file.
+
+```
+<rosparam file="$(find gen3_compliant_controllers)/config/filename.yaml"/>
+```
+
+In case you would like to restart the tuning later with these parameters, use the GUI to save the YAML file and load it in the GUI when you start tuning again.
+
+Refer to [this paper](https://ieeexplore.ieee.org/document/8781838) for a more conceptual understanding of the different controller parameters.
+
+**Note**: Certain set of parameters may lead to severe vibrations. Please make sure you have the e-stop next to you or another person to manage the e-stop during the tuning process.
+
+
+## Contribution
+Any contributions related to bug fixes/feature additions/documentation are welcome! Contributing is simple. Just follow the following steps:
+* Fork the repository
+* Create a new branch with an appropriate name
+* Make changes, test, and commit.
+* Format the code using `catkin build gen3_compliant_controllers --no-deps --make-args format`
+* Send a PR to the main repository and tag the maintainers to request a review.
+
+## Authors
+This implementation has been made possible through collaboration between [EmPRISE Lab](https://emprise.cs.cornell.edu/) at Cornell University and [Intelligent Robotic Systems Lab](https://sites.google.com/view/kaist-roboticslab) at KAIST. It is currently maintained by Rishabh Madan ([@madan96](https://github.com/madan96)) and Rajat Kumar Jenamani ([@RKJenamani](https://github.com/RKJenamani)). It has received major initial contributions from [Seo Wook Han]([email protected]).

config/config.yaml

@@ -0,0 +1,43 @@
+kortex_hardware/b_0: 0.3
+kortex_hardware/b_1: 0.3
+kortex_hardware/b_2: 0.3
+kortex_hardware/b_3: 0.3
+kortex_hardware/b_4: 0.18
+kortex_hardware/b_5: 0.18
+kortex_hardware/b_6: 0.18
+kortex_hardware/d_0: 2.0
+kortex_hardware/d_1: 2.0
+kortex_hardware/d_2: 2.0
+kortex_hardware/d_3: 2.0
+kortex_hardware/d_4: 2.0
+kortex_hardware/d_5: 2.0
+kortex_hardware/d_6: 2.0
+kortex_hardware/d_z: 40.0
+kortex_hardware/j_0: 4000.0
+kortex_hardware/j_1: 4000.0
+kortex_hardware/j_2: 4000.0
+kortex_hardware/j_3: 4000.0
+kortex_hardware/j_4: 3500.0
+kortex_hardware/j_5: 3500.0
+kortex_hardware/j_6: 3500.0
+kortex_hardware/k_0: 50.0
+kortex_hardware/k_1: 20.0
+kortex_hardware/k_2: 20.0
+kortex_hardware/k_3: 20.0
+kortex_hardware/k_4: 10.0
+kortex_hardware/k_5: 10.0
+kortex_hardware/k_6: 10.0
+kortex_hardware/l_0: 75.0
+kortex_hardware/l_1: 75.0
+kortex_hardware/l_2: 75.0
+kortex_hardware/l_3: 75.0
+kortex_hardware/l_4: 40.0
+kortex_hardware/l_5: 40.0
+kortex_hardware/l_6: 40.0
+kortex_hardware/lp_0: 5.0
+kortex_hardware/lp_1: 5.0
+kortex_hardware/lp_2: 5.0
+kortex_hardware/lp_3: 5.0
+kortex_hardware/lp_4: 4.0
+kortex_hardware/lp_5: 4.0
+kortex_hardware/lp_6: 4.0

launch/controller.launch

@@ -36,16 +36,18 @@
     	joint_state_controller
   	" />
   <group if="$(eval controller_type == 'joint')">
-    <node name="controller_spawner_started" pkg="controller_manager" type="spawner" respawn="false"
+    <node name="compliant_controller_spawner_started" pkg="controller_manager" type="spawner" respawn="false"
     	output="screen"
     	args="
+        --stopped
         joint_space_compliant_controller
     	" />
   </group>
   <group if="$(eval controller_type == 'task')">
-    <node name="controller_spawner_started" pkg="controller_manager" type="spawner" respawn="false"
+    <node name="compliant_controller_spawner_started" pkg="controller_manager" type="spawner" respawn="false"
     	output="screen"
     	args="
+        --stopped
         task_space_compliant_controller
     	" />
   </group>
@@ -62,4 +64,6 @@
 
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />
 
+  <rosparam file="$(find gen3_compliant_controllers)/config/config.yaml"/>
+
 </launch>