From 8e19bd48bd065708bd428fd05b94efdbcbbca4e7 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Fri, 18 May 2018 20:37:33 -0300 Subject: [PATCH] Working voltage interface. --- q2d_bringup/config/pid.yaml | 20 +++++-- q2d_bringup/scripts/tune_pid.m | 118 +++++++++++++++++++++++++++++++++++++++++ q2d_description/urdf/q2d.urdf | 5 -- 3 files changed, 134 insertions(+), 9 deletions(-) create mode 100644 q2d_bringup/scripts/tune_pid.m diff --git a/q2d_bringup/config/pid.yaml b/q2d_bringup/config/pid.yaml index 5d0200c..704f541 100644 --- 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 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 --- a/q2d_description/urdf/q2d.urdf +++ b/q2d_description/urdf/q2d.urdf @@ -174,7 +174,6 @@ transmission_interface/SimpleTransmission VoltageJointInterface - EffortJointInterface 1 @@ -185,7 +184,6 @@ transmission_interface/SimpleTransmission VoltageJointInterface - EffortJointInterface 1 @@ -195,10 +193,7 @@ - gazebo_ros_electrical/DCmotorRobotHWSim - - 0.001 -- 2.12.0