Working voltage interface.

diff --git a/q2d_bringup/config/pid.yaml b/q2d_bringup/config/pid.yaml
index 5d0200c..704f541 100644 (file)
--- a/q2d_bringup/config/pid.yaml
+++ b/q2d_bringup/config/pid.yaml
@@ -1,13 +1,25 @@
 shoulder_controller:
-        type: effort_controllers/JointPositionController
+        type: voltage_controllers/JointPositionController
         joint: shoulder_active_joint
-        pid: {p: 2310, i: 4640, d: 0.299}   
+        pid: {p: 45900, i: 46300, d: 11400}
 
 elbow_controller:
-        type: effort_controllers/JointPositionController
+        type: voltage_controllers/JointPositionController
         joint: elbow_active_joint
-        pid: {p: 339, i: 851, d: 0.351}
+        pid: {p: 5000000, i: 12700000, d: 490000}
 
 joint_states_publisher:
         type: joint_state_controller/JointStateController
         publish_rate: 100
+
+gazebo_ros_electrical:
+        dcmotor:
+                shoulder_active_joint:
+                        Ra: 11.5
+                        La: 3.16
+                        Kt: 0.119
+                elbow_active_joint:
+                        Ra: 2.32
+                        La: 0.24
+                        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
new file mode 100644 (file)
index 0000000..4335e39
--- /dev/null
+++ b/q2d_bringup/scripts/tune_pid.m
@@ -0,0 +1,118 @@
+% Mass, inertia tensor, and center of mass of shoulder active link
+msa=0.19730261508;
+Isa=[0.00038685518702305, 0.00000000055222416, -0.00000031340718614;
+0.00000000055222416, 0.00010241438913870, -0.00000000015426019;
+-0.00000031340718614 -0.00000000015426019, 0.00047879093657893];
+Psa=[0.0252456823; -0.00000002723; 0.06470401873];
+
+% Mass, inertia tensor, and center of mass of shoulder passive link
+msp=1.26475817816;
+Isp=[0.00346199967740929, -0.00010902049981923, -0.00401182173261703;
+-0.00010902049981923, 0.03314904030482527, 0.00005087359051462;
+-0.00401182173261703, 0.00005087359051462, 0.03113579694057124];
+Psp=[0.16516344805; -0.00048428845; -0.00016382412];
+
+% Mass, inertia tensor, and center of mass of elbow active link
+mea=0.19712951877;
+Iea=[0.00038850510800265, 0.00000000052121416, 0.00000404728675587;
+0.00000000052121416, 0.00010146693248154, 0.00000000002789435;
+0.00000404728675587, 0.00000000002789435, 0.00048091942023028];
+Pea=[0.02548273493; -0.00000002263; 0.05254513577];
+
+% Mass, inertia tensor, and center of mass of elbow passive link
+mep=0.67529215765;
+Iep=[0.00132247071698947, -0.00000000605403474, -0.00090893541574333;
+-0.00000000605403474, 0.00774007102253750, 0.00000000624688369;
+-0.00090893541574333, 0.00000000624688369, 0.00751638349361413];
+Pep=[0.06204831581; 0.00000013809; 0.01489882531];
+
+% Total mass of elbow
+mec=mea+mep
+% Center of mass of elbow (the origins of the systems are the same)
+Pec=(Pea*mea+Pep*mep)/mec
+
+% Position of the center of mass of the elbow passive link with respect to
+% the combined center of mass
+Pepc=Pep-Pec;
+% Intertia tensor of elbow passive link at the combined center of mass
+Iepc=Iep+mep*(Pepc'*Pepc*eye(3)-Pepc*Pepc');
+
+% Position of the center of mass of the elbow active link with respect to
+% the combined center of mass
+Peac=Pea-Pec;
+% Intertia tensor of elbow active link at the combined center of mass
+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;
+
+% Transfer function of elbow joint
+Ge=tf((Le*Je/Ke),[1 (fe/Je+Re/Le) (Re/Le*fe/Je+Ke/Le/Je) 0])
+
+[pide,pe]=pidtune(Ge,'pid',2*pi/Te/10.97)
+
+% Position of elbow origin with respect to shoulder origin
+Pse=[0.343;0;0];
+% Total mass of shoulder
+msc=msa+msp+mec
+% Center of mass of shoulder (the origins of the systems are the same)
+Psc=(Psa*msa+Psp*msp+(Pse+Pec)*mec)/(msc+mec)
+
+% Position of the center of mass of the shoulder passive link with respect to
+% the combined center of mass
+Pspc=Psp-Psc;
+% Intertia tensor of shoulder passive link at the combined center of mass
+Ispc=Isp+msp*(Pspc'*Pspc*eye(3)-Pspc*Pspc');
+
+% Position of the center of mass of the shoulder active link with respect to
+% the combined center of mass
+Psac=Psa-Psc;
+% Intertia tensor of shoulder active link at the combined center of mass
+Isac=Isa+msa*(Psac'*Psac*eye(3)-Psac*Psac');
+
+% Position of the center of mass of the elbow with respect to
+% the combined center of mass of shoulder
+Psec=Pse+Pec-Psc;
+% Intertia tensor of shoulder active link at the combined center of mass
+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;
+
+
+% Transfer function of shoulder joint
+Gs=tf((Ls*Js/Ks),[1 (fs/Js+Rs/Ls) (Rs/Ls*fs/Js+Ks/Ls/Js) 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 5b542c5..22b0fec 100644 (file)
--- a/q2d_description/urdf/q2d.urdf
+++ b/q2d_description/urdf/q2d.urdf
@@ -174,7 +174,6 @@
                <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
@@ -185,7 +184,6 @@
                <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
@@ -195,10 +193,7 @@
        <gazebo>\r
                <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">\r
                        <!--<robotNamespace>/q2d</robotNamespace>-->\r
-                       <!-- Custom plugin -->
                        <robotSimType>gazebo_ros_electrical/DCmotorRobotHWSim</robotSimType>
-                       <!-- Default plugin -->
-                       <!-- <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType> -->\r
                        <controlPeriod>0.001</controlPeriod>\r
                </plugin>\r
        </gazebo>\r