From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Wed, 18 Sep 2019 05:56:35 +0000 (-0300)
Subject: Revert to joint effort actuation.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=9ebca488626740b7c02eacf0f621b237b01cdd89;p=q2d.git

Revert to joint effort actuation.
---

diff --git a/q2d_bringup/config/pid.yaml b/q2d_bringup/config/pid.yaml
index 3220121..5d0200c 100644
--- a/q2d_bringup/config/pid.yaml
+++ b/q2d_bringup/config/pid.yaml
@@ -1,25 +1,13 @@
 shoulder_controller:
-        type: voltage_controllers/JointPositionController
+        type: effort_controllers/JointPositionController
         joint: shoulder_active_joint
-        pid: {p: 45900, i: 46300, d: 11400}
+        pid: {p: 2310, i: 4640, d: 0.299}   
 
 elbow_controller:
-        type: voltage_controllers/JointPositionController
+        type: effort_controllers/JointPositionController
         joint: elbow_active_joint
-        pid: {p: 5000000, i: 12700000, d: 490000}
+        pid: {p: 339, i: 851, d: 0.351}
 
 joint_states_publisher:
         type: joint_state_controller/JointStateController
         publish_rate: 100
-
-gazebo_ros_electrical:
-        dcmotor:
-                shoulder_active_joint:
-                        Ra: 11.5
-                        La: 0.00316
-                        Kt: 0.119
-                elbow_active_joint:
-                        Ra: 2.32
-                        La: 0.00024
-                        Kt: 0.0234
- 
\ No newline at end of file
diff --git a/q2d_bringup/scripts/tune_pid.m b/q2d_bringup/scripts/tune_pid.m
index 4335e39..1436def 100644
--- a/q2d_bringup/scripts/tune_pid.m
+++ b/q2d_bringup/scripts/tune_pid.m
@@ -45,25 +45,15 @@ Ieac=Iea+mea*(Peac'*Peac*eye(3)-Peac*Peac');
 
 % Inertia tensor of elbow
 Iec=Iepc+Ieac
-Je=Iec(3,3);
 
 % Time constant of elbow joint
 Te=4e-3;
 
 % friction
-fe=Iec(3,3)/Te;
-
-% Motor resistance
-Re=2.32;
-
-% Motor inductance
-Le=0.24;
-
-% Motor gain
-Ke=0.0234;
+fe=Iec(3,3)/Te
 
 % Transfer function of elbow joint
-Ge=tf((Le*Je/Ke),[1 (fe/Je+Re/Le) (Re/Le*fe/Je+Ke/Le/Je) 0])
+Ge=tf(1/Iec(3,3),[1 1/Te 0])
 
 [pide,pe]=pidtune(Ge,'pid',2*pi/Te/10.97)
 
@@ -94,25 +84,14 @@ Isec=Iec+mec*(Psec'*Psec*eye(3)-Psec*Psec');
 
 % Inertia tensor of shoulder
 Isc=Ispc+Isac+Isec
-Js=Isc(3,3);
 
 % Time constant of shoulderjoint
 Ts=5e-3;
 
 % friction
-fs=Isc(3,3)/Ts;
-
-% Motor resistance
-Rs=11.5;
-
-% Motor inductance
-Ls=3.16;
-
-% Motor gain
-Ks=0.119;
-
+fs=Isc(3,3)/Ts
 
 % Transfer function of shoulder joint
-Gs=tf((Ls*Js/Ks),[1 (fs/Js+Rs/Ls) (Rs/Ls*fs/Js+Ks/Ls/Js) 0])
+Gs=tf(1/Isc(3,3),[1 1/Ts 0])
 
 [pids,ps]=pidtune(Gs,'pid',2*pi/Ts/10.97)
diff --git a/q2d_description/urdf/q2d.urdf b/q2d_description/urdf/q2d.urdf
index 22b0fec..25ec90a 100644
--- a/q2d_description/urdf/q2d.urdf
+++ b/q2d_description/urdf/q2d.urdf
@@ -63,8 +63,8 @@
 		<origin xyz="0 0 0.1477"/>
 		<axis xyz="0 0 1"/>
 		<limit lower="-1.570796326794897" upper="1.570796326794897"
-			velocity="2.27" effort="35.7" />			
-		<dynamics damping="17.4775" />
+			velocity="2.27" effort="27.94" />			
+		<dynamics damping="29.7914" />
 	</joint>
 
 	<link name="shoulder_passive_link">
@@ -128,8 +128,8 @@
 		<origin xyz="0.343 0 0"/>
 		<axis xyz="0 0 1"/>
 		<limit lower="-1.570796326794897" upper="1.570796326794897"
-			velocity="23.08" effort="3.51" />
-		<dynamics damping="2.0503" />
+			velocity="23.08" effort="13.62" />
+		<dynamics damping="2.6404" />
 	</joint>
 
 	<link name="elbow_passive_link">
@@ -173,7 +173,7 @@
 	<transmission name="shoulder_active_transmission">
 		<type>transmission_interface/SimpleTransmission</type>
 		<joint name="shoulder_active_joint">
-			<hardwareInterface>VoltageJointInterface</hardwareInterface>
+			<hardwareInterface>EffortJointInterface</hardwareInterface>
 		</joint>
 		<actuator name="shoulder_motor">
 			<mechanicalReduction>1</mechanicalReduction>
@@ -183,7 +183,7 @@
 	<transmission name="elbow_active_transmission">
 		<type>transmission_interface/SimpleTransmission</type>
 		<joint name="elbow_active_joint">
-			<hardwareInterface>VoltageJointInterface</hardwareInterface>
+			<hardwareInterface>EffortJointInterface</hardwareInterface>
 		</joint>
 		<actuator name="elbow_motor">
 			<mechanicalReduction>1</mechanicalReduction>
@@ -192,8 +192,7 @@
 
 	<gazebo>
 		<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-			<!--<robotNamespace>/q2d</robotNamespace>-->
-			<robotSimType>gazebo_ros_electrical/DCmotorRobotHWSim</robotSimType>
+			<!-- <robotNamespace>/q2d</robotNamespace> -->
 			<controlPeriod>0.001</controlPeriod>
 		</plugin>
 	</gazebo>