Revert to joint effort actuation.

diff --git a/q2d_bringup/config/pid.yaml b/q2d_bringup/config/pid.yaml
index 3220121..5d0200c 100644 (file)
--- 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 (file)
--- 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 (file)
--- a/q2d_description/urdf/q2d.urdf
+++ b/q2d_description/urdf/q2d.urdf
@@ -63,8 +63,8 @@
                <origin xyz="0 0 0.1477"/>\r
                <axis xyz="0 0 1"/>\r
                <limit lower="-1.570796326794897" upper="1.570796326794897"\r
-                       velocity="2.27" effort="35.7" />                        \r
-               <dynamics damping="17.4775" />\r
+                       velocity="2.27" effort="27.94" />                       \r
+               <dynamics damping="29.7914" />\r
        </joint>\r
 \r
        <link name="shoulder_passive_link">\r
@@ -128,8 +128,8 @@
                <origin xyz="0.343 0 0"/>\r
                <axis xyz="0 0 1"/>\r
                <limit lower="-1.570796326794897" upper="1.570796326794897"\r
-                       velocity="23.08" effort="3.51" />\r
-               <dynamics damping="2.0503" />\r
+                       velocity="23.08" effort="13.62" />\r
+               <dynamics damping="2.6404" />\r
        </joint>\r
 \r
        <link name="elbow_passive_link">\r
@@ -173,7 +173,7 @@
        <transmission name="shoulder_active_transmission">\r
                <type>transmission_interface/SimpleTransmission</type>\r
                <joint name="shoulder_active_joint">\r
-                       <hardwareInterface>VoltageJointInterface</hardwareInterface>\r
+                       <hardwareInterface>EffortJointInterface</hardwareInterface>\r
                </joint>\r
                <actuator name="shoulder_motor">\r
                        <mechanicalReduction>1</mechanicalReduction>\r
@@ -183,7 +183,7 @@
        <transmission name="elbow_active_transmission">\r
                <type>transmission_interface/SimpleTransmission</type>\r
                <joint name="elbow_active_joint">\r
-                       <hardwareInterface>VoltageJointInterface</hardwareInterface>\r
+                       <hardwareInterface>EffortJointInterface</hardwareInterface>\r
                </joint>\r
                <actuator name="elbow_motor">\r
                        <mechanicalReduction>1</mechanicalReduction>\r
@@ -192,8 +192,7 @@
 \r
        <gazebo>\r
                <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">\r
-                       <!--<robotNamespace>/q2d</robotNamespace>-->\r
-                       <robotSimType>gazebo_ros_electrical/DCmotorRobotHWSim</robotSimType>
+                       <!-- <robotNamespace>/q2d</robotNamespace> -->\r
                        <controlPeriod>0.001</controlPeriod>\r
                </plugin>\r
        </gazebo>\r