From 20923e1b82e785ba985433111eed57508f902f32 Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Thu, 16 Mar 2023 03:27:33 -0300
Subject: [PATCH] Change mimic joint actuation to gazebo_ros2_control.

---
 bhand_bringup/CMakeLists.txt                   | 12 +++++-------
 bhand_bringup/config/bhand_controller.yaml     |  3 ++-
 bhand_bringup/package.xml                      |  6 +++---
 bhand_description/xacro/bhand.urdf.xacro       | 23 ++++++++++++-----------
 bhand_description/xacro/bhand_link3.urdf.xacro | 11 -----------
 5 files changed, 22 insertions(+), 33 deletions(-)

diff --git a/bhand_bringup/CMakeLists.txt b/bhand_bringup/CMakeLists.txt
index 915d2df..894ef6b 100644
--- a/bhand_bringup/CMakeLists.txt
+++ b/bhand_bringup/CMakeLists.txt
@@ -7,9 +7,6 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
 
 install(DIRECTORY config launch
 	DESTINATION share/${PROJECT_NAME}
@@ -26,11 +23,12 @@ install(PROGRAMS
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
+  # comment the line when a copyright and license is added to all source files
+  # set(ament_cmake_copyright_FOUND TRUE)
   # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  # set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
 
diff --git a/bhand_bringup/config/bhand_controller.yaml b/bhand_bringup/config/bhand_controller.yaml
index ba8e211..17f9608 100644
--- a/bhand_bringup/config/bhand_controller.yaml
+++ b/bhand_bringup/config/bhand_controller.yaml
@@ -1,7 +1,8 @@
-/bhand_controller:
+bhand_controller:
         ros__parameters:
                 joints:
                         - bhand_finger1_joint_2
                         - bhand_finger2_joint_2
                         - bhand_finger3_joint_2
                         - bhand_spread
+
diff --git a/bhand_bringup/package.xml b/bhand_bringup/package.xml
index 39bf53f..5c4a7ff 100644
--- a/bhand_bringup/package.xml
+++ b/bhand_bringup/package.xml
@@ -2,10 +2,10 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>bhand_bringup</name>
-  <version>2.0.0</version>
-  <description>Bring-up Barrett Hand</description>
+  <version>2.1.0</version>
+  <description>Barrett Hand bring-up</description>
   <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
-  <license>GPLv3</license>
+  <license>GPL-3.0-only</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
diff --git a/bhand_description/xacro/bhand.urdf.xacro b/bhand_description/xacro/bhand.urdf.xacro
index c5543a5..47f8a80 100644
--- a/bhand_description/xacro/bhand.urdf.xacro
+++ b/bhand_description/xacro/bhand.urdf.xacro
@@ -59,17 +59,6 @@
 
   <xacro:bhand_transmission joint="spread_mimic" />
 
-  <gazebo>
-	<plugin name="bhand_spread_mimic_plugin" filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so">
-		<joint>bhand_spread</joint>
-		<mimicJoint>bhand_spread_mimic</mimicJoint>
-		<multiplier>-1.0</multiplier>
-		<offset>0.0</offset>
-		<maxEffort>30.0</maxEffort>
-		<sensitiveness>0.0</sensitiveness>
-	</plugin>
-  </gazebo>
-
   <xacro:bhand_link1 parent="base_link" link="finger3_link_1" joint="finger3_joint_1" type="fixed">
     		<inertial_attr>
  			<inertial>
@@ -125,6 +114,9 @@
 		</joint>
 		
 		<joint name="bhand_finger1_joint_3">
+ 			<param name="mimic">bhand_finger1_joint_2</param>
+ 			<param name="multiplier">${48 / 140}</param>
+ 			<command_interface name="position"/>
 			<state_interface name="position"/>
 		</joint>
 		
@@ -137,6 +129,9 @@
 		</joint>
 		
 		<joint name="bhand_finger2_joint_3">
+ 			<param name="mimic">bhand_finger2_joint_2</param>
+ 			<param name="multiplier">${48 / 140}</param>
+ 			<command_interface name="position"/>
 			<state_interface name="position"/>
 		</joint>
 
@@ -149,6 +144,9 @@
 		</joint>
 
 		<joint name="bhand_finger3_joint_3">
+ 			<param name="mimic">bhand_finger3_joint_2</param>
+ 			<param name="multiplier">${48 / 140}</param>
+ 			<command_interface name="position"/>
 			<state_interface name="position"/>
 		</joint>
 		
@@ -161,6 +159,9 @@
 		</joint>
 		
  		<joint name="bhand_spread_mimic">
+ 			<param name="mimic">bhand_spread</param>
+ 			<param name="multiplier">-1.0</param>
+ 			<command_interface name="position"/>
 			<state_interface name="position"/>
 		</joint>
 	</ros2_control>
diff --git a/bhand_description/xacro/bhand_link3.urdf.xacro b/bhand_description/xacro/bhand_link3.urdf.xacro
index 41177e3..d81cdd2 100644
--- a/bhand_description/xacro/bhand_link3.urdf.xacro
+++ b/bhand_description/xacro/bhand_link3.urdf.xacro
@@ -66,17 +66,6 @@
 		</actuator>
 	</transmission>
 
-  	<gazebo>
-		<plugin name="bhand_${joint}_mimic_plugin" filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so">
-			<joint>bhand_${mimic_joint}</joint>
-			<mimicJoint>bhand_${joint}</mimicJoint>
-			<multiplier>${48 / 140}</multiplier>
-			<offset>0.0</offset>
-			<maxEffort>30.0</maxEffort>
-			<sensitiveness>0.0</sensitiveness>
-		</plugin>
-	</gazebo>
-
   </xacro:macro>
 
 </robot>
-- 
2.12.0