From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Thu, 9 May 2019 04:56:55 +0000 (-0300)
Subject: Add joint group controller.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=HEAD;p=users%2Fgschmitz%2Fmiitzhand.git

Add joint group controller.
---

diff --git a/miitzhand_bringup/config/group_ijc.yaml b/miitzhand_bringup/config/group_ijc.yaml
new file mode 120000
index 0000000..448f45b
--- /dev/null
+++ b/miitzhand_bringup/config/group_ijc.yaml
@@ -0,0 +1 @@
+miitzhand.yaml
\ No newline at end of file
diff --git a/miitzhand_bringup/config/ijc.yaml b/miitzhand_bringup/config/ijc.yaml
new file mode 120000
index 0000000..448f45b
--- /dev/null
+++ b/miitzhand_bringup/config/ijc.yaml
@@ -0,0 +1 @@
+miitzhand.yaml
\ No newline at end of file
diff --git a/miitzhand_bringup/config/miitzhand.yaml b/miitzhand_bringup/config/miitzhand.yaml
index 0bfc297..7b70b04 100644
--- a/miitzhand_bringup/config/miitzhand.yaml
+++ b/miitzhand_bringup/config/miitzhand.yaml
@@ -1,33 +1,55 @@
-    joint_state_controller:
+joint_state_controller:
         type: joint_state_controller/JointStateController
         publish_rate: 1000
 
-    thumb1_controller:
+thumb1_controller:
         type: effort_controllers/JointPositionController
         joint: thumb_j1
-        pid: {p: 29.4, i: 1640, d: 0.0, i_clamp: 1.96, antiwindup: true}
+        pid: {p: 29.6, i: 1600, d: 0.0, i_clamp: 1.96, antiwindup: true}
 
-    thumb2_controller:
+thumb2_controller:
         type: effort_controllers/JointPositionController
         joint: thumb_j2
-        pid: {p: 29.2, i: 1680, d: 0.0, i_clamp: 1.96, antiwindup: true}
+        pid: {p: 29.3, i: 1660, d: 0.0, i_clamp: 1.96, antiwindup: true}
 
-    index_controller:
+index_controller:
         type: effort_controllers/JointPositionController
         joint: index_j1
-        pid: {p: 29.2, i: 1670, d: 0.0, i_clamp: 1.96, antiwindup: true}
+        pid: {p: 29.4, i: 1640, d: 0.0, i_clamp: 1.96, antiwindup: true}
 
-    middle_controller:
+middle_controller:
         type: effort_controllers/JointPositionController
         joint: middle_j1
         pid: {p: 29.2, i: 1670, d: 0.0, i_clamp: 1.96, antiwindup: true}
 
-    ring_controller:
+ring_controller:
         type: effort_controllers/JointPositionController
         joint: ring_j1
         pid: {p: 29.2, i: 1670, d: 0.0, i_clamp: 1.96, antiwindup: true}
 
-    little_controller:
+little_controller:
         type: effort_controllers/JointPositionController
         joint: little_j1
         pid: {p: 29.2, i: 1670, d: 0.0, i_clamp: 1.96, antiwindup: true}
+
+miitzhand_controller:
+        type: effort_controllers/JointGroupPositionController
+        joints:
+                - thumb_j1
+                - thumb_j2
+                - index_j1
+                - middle_j1
+                - ring_j1
+                - little_j1
+        thumb_j1:
+                pid: {p: 29.6, i: 1600, d: 0.0, i_clamp: 1.96, antiwindup: true}
+        thumb_j2:
+                pid: {p: 29.3, i: 1660, d: 0.0, i_clamp: 1.96, antiwindup: true}
+        index_j1:
+                pid: {p: 29.4, i: 1640, d: 0.0, i_clamp: 1.96, antiwindup: true}
+        middle_j1:
+                pid: {p: 29.2, i: 1670, d: 0.0, i_clamp: 1.96, antiwindup: true}
+        ring_j1:
+                pid: {p: 29.2, i: 1670, d: 0.0, i_clamp: 1.96, antiwindup: true}
+        little_j1:
+                pid: {p: 29.2, i: 1670, d: 0.0, i_clamp: 1.96, antiwindup: true}
diff --git a/miitzhand_bringup/launch/gazebo.launch b/miitzhand_bringup/launch/gazebo.launch
index 2d54125..f6a61fa 100644
--- a/miitzhand_bringup/launch/gazebo.launch
+++ b/miitzhand_bringup/launch/gazebo.launch
@@ -5,6 +5,9 @@
 	<arg name="paused" default="true" />
 	<arg name="use_sim_time" default="true"/>
 
+	<arg name="controller" default="ijc"/>
+	<arg name="config" default="$(find miitzhand_bringup)/config/$(arg controller).yaml"/>
+
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
 	<arg name="debug" value="$(arg debug)"/>
 	<arg name="gui" value="$(arg gui)"/>
@@ -23,18 +26,7 @@
 		-J thumb_j1 0 -J thumb_j2 -1.5708 -J index_j1 -1.5708
 		-J middle_j1 -1.5708 -J ring_j1 -1.5708 -J little_j1 -1.5708" />
       
-        <!-- Controllers Settings-->
-        <rosparam command="load" file="$(find miitzhand_bringup)/config/miitzhand.yaml" />
-        
-        <node name="controller_spawner" pkg="controller_manager" type="spawner"
-            respawn="false" output="screen"
-            args="
-            joint_state_controller
-            thumb1_controller
-            thumb2_controller
-            index_controller
-            middle_controller
-            ring_controller
-            little_controller
-            --timeout=60" />
+	<rosparam file="$(arg config)" command="load"/>
+
+        <include file="$(find miitzhand_bringup)/launch/$(arg controller).launch" />
 </launch>
diff --git a/miitzhand_bringup/launch/group_ijc.launch b/miitzhand_bringup/launch/group_ijc.launch
new file mode 100644
index 0000000..24adc9c
--- /dev/null
+++ b/miitzhand_bringup/launch/group_ijc.launch
@@ -0,0 +1,8 @@
+<launch>
+        <!-- Controllers Settings-->
+        <rosparam command="load" file="$(find miitzhand_bringup)/config/miitzhand.yaml" />
+        
+        <node name="controller_spawner" pkg="controller_manager" type="spawner"
+            respawn="false" output="screen"
+            args="miitzhand_controller joint_state_controller" />
+</launch>
diff --git a/miitzhand_bringup/launch/ijc.launch b/miitzhand_bringup/launch/ijc.launch
new file mode 100644
index 0000000..4fb04ef
--- /dev/null
+++ b/miitzhand_bringup/launch/ijc.launch
@@ -0,0 +1,16 @@
+<launch>
+        <!-- Controllers Settings-->
+        <rosparam command="load" file="$(find miitzhand_bringup)/config/miitzhand.yaml" />
+        
+        <node name="controller_spawner" pkg="controller_manager" type="spawner"
+            respawn="false" output="screen"
+            args="
+            joint_state_controller
+            thumb1_controller
+            thumb2_controller
+            index_controller
+            middle_controller
+            ring_controller
+            little_controller
+            --timeout=60" />
+</launch>
diff --git a/miitzhand_bringup/worlds/empty_30us.world b/miitzhand_bringup/worlds/empty_30us.world
index d966887..4439439 100644
--- a/miitzhand_bringup/worlds/empty_30us.world
+++ b/miitzhand_bringup/worlds/empty_30us.world
@@ -19,5 +19,12 @@
     <physics type="ode">
       <max_step_size>0.00003</max_step_size>
     </physics>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>0.562356 -0.536963 0.324734 0 0.275643 2.35619</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
   </world>
 </sdf>
diff --git a/miitzhand_description/urdf/miitzhand.urdf b/miitzhand_description/urdf/miitzhand.urdf
index cb87b7e..e447836 100644
--- a/miitzhand_description/urdf/miitzhand.urdf
+++ b/miitzhand_description/urdf/miitzhand.urdf
@@ -868,7 +868,7 @@
 <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
 	<!--robotNamespace>/miitzhand</robotNamespace-->
-	<controlPeriod>0.001</controlPeriod>
+	<controlPeriod>0.00003</controlPeriod>
 	<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
     </plugin>
 </gazebo>