From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sat, 2 Oct 2021 06:36:48 +0000 (-0300)
Subject: Move setup of path for gazebo models from launch file to package.xml.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=5766a38016c9541472c4b5a768ef654d91518fe1;p=q2d.git

Move setup of path for gazebo models from launch file to package.xml.

Add computed_torque_controller configuration.

Add pid_plus_gravity_controller configuration.
---

diff --git a/q2d_bringup/config/computed_torque.yaml b/q2d_bringup/config/computed_torque.yaml
new file mode 100644
index 0000000..ebadbfd
--- /dev/null
+++ b/q2d_bringup/config/computed_torque.yaml
@@ -0,0 +1,10 @@
+/computed_torque_controller:
+        ros__parameters:
+                joints: 
+                        - shoulder_active_joint
+                        - elbow_active_joint
+                Kp: [2500.0, 0.0, 0.0, 300.0]
+                Kd: [0.3, 0.0, 0.0, 0.3]
+                gravity: {x: 0.0, y: 0.0, z: -9.8}
+                chain: {root: "origin_link", tip: "tool_link"}
+                priority: 99
diff --git a/q2d_bringup/config/pid_plus_gravity.yaml b/q2d_bringup/config/pid_plus_gravity.yaml
new file mode 100644
index 0000000..6a8e286
--- /dev/null
+++ b/q2d_bringup/config/pid_plus_gravity.yaml
@@ -0,0 +1,10 @@
+/pid_plus_gravity_controller:
+        ros__parameters:
+                joints: 
+                        - shoulder_active_joint
+                        - elbow_active_joint
+                shoulder_active_joint: {p: 2310.0, i: 4640.0, d: 0.299, i_clamp_max: 27.94, i_clamp_min: -27.94}
+                elbow_active_joint: {p: 339.0, i: 851.0, d: 0.351, i_clamp_max: 13.62, i_clamp_min: -13.62}
+                gravity: {x: 0.0, y: 0.0, z: -9.8}
+                chain: {root: "origin_link", tip: "tool_link"}
+                priority: 99
diff --git a/q2d_bringup/launch/computed_torque.launch.xml b/q2d_bringup/launch/computed_torque.launch.xml
new file mode 100644
index 0000000..8730b23
--- /dev/null
+++ b/q2d_bringup/launch/computed_torque.launch.xml
@@ -0,0 +1,30 @@
+<!--******************************************************************************
+	                   Quanser 2DSFJE Bringup
+	            Computed Torque  Controller Launch File
+          Copyright (C) 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        Geneal Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+        
+*******************************************************************************-->
+
+<launch>
+	<arg name="config" default="$(find-pkg-share q2d_bringup)/config/computed_torque.yaml"/>
+	
+	<node name="computed_torque_controller_spawner" pkg="controller_manager" exec="spawner"
+		args="-t effort_controllers/ComputedTorqueController -p $(var config) computed_torque_controller"/>
+		
+	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+		args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
diff --git a/q2d_bringup/launch/pid_plus_gravity.launch.xml b/q2d_bringup/launch/pid_plus_gravity.launch.xml
new file mode 100644
index 0000000..0546b12
--- /dev/null
+++ b/q2d_bringup/launch/pid_plus_gravity.launch.xml
@@ -0,0 +1,30 @@
+<!--******************************************************************************
+	                   Quanser 2DSFJE Bringup
+	              PID+gravity Controller Launch File
+          Copyright (C) 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        Geneal Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+        
+*******************************************************************************-->
+
+<launch>
+	<arg name="config" default="$(find-pkg-share q2d_bringup)/config/pid_plus_gravity.yaml"/>
+	
+	<node name="pid_plus_gravity_controller_spawner" pkg="controller_manager" exec="spawner"
+		args="-t effort_controllers/PidPlusGravityController -p $(var config) pid_plus_gravity_controller"/>
+		
+	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+		args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
diff --git a/q2d_description/launch/gazebo.launch.xml b/q2d_description/launch/gazebo.launch.xml
index afe8c63..e01e744 100644
--- a/q2d_description/launch/gazebo.launch.xml
+++ b/q2d_description/launch/gazebo.launch.xml
@@ -1,16 +1,35 @@
+<!--******************************************************************************
+	                   Quanser 2DSFJE Description
+	                     Gazebo Launch File
+          Copyright (C) 2018, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        Geneal Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+        
+*******************************************************************************-->
+
 <launch>
 	<arg name="pause" default="true"/>
 	<arg name="gui" default="true"/>
 	<arg name="use_sim_time" default="true"/>
 
-	<set_env name="GAZEBO_MODEL_PATH" value="$(env GAZEBO_MODEL_PATH ''):$(find-pkg-prefix q2d_description)/share/"/>
-	
 	<include file="$(find-pkg-share gazebo_ros)/launch/gazebo.launch.py">
 		<arg name="pause" value="$(var pause)"/>
 		<arg name="gui" value="$(var gui)"/>
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
 		<arg name="world" value="worlds/empty_sky.world" />
-		<arg name="extra_gazebo_args" value=" --ros-args --params-file $(find-pkg-share q2d_description)/config/gazebo.yaml"/>
+		<arg name="extra_gazebo_args" value="--ros-args --params-file $(find-pkg-share q2d_description)/config/gazebo.yaml"/>
 	</include>
 
 	<include file="$(find-pkg-share q2d_description)/launch/q2d.launch.xml">
diff --git a/q2d_description/package.xml b/q2d_description/package.xml
index 26119d4..bbed41c 100644
--- a/q2d_description/package.xml
+++ b/q2d_description/package.xml
@@ -14,5 +14,6 @@
 
   <export>
     <build_type>ament_cmake</build_type>
+    <gazebo_ros gazebo_model_path="${prefix}/.."/>
   </export>
 </package>