From 8b7bc9dc9e94ac44309ed0cb3ab25a5702ba87a5 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 23 Sep 2019 15:41:09 -0400 Subject: [PATCH 1/2] Add back fin aerodynamics. --- ros/src/f16/models/rocket/rocket.sdf | 108 +++++++++++++-------------- 1 file changed, 54 insertions(+), 54 deletions(-) diff --git a/ros/src/f16/models/rocket/rocket.sdf b/ros/src/f16/models/rocket/rocket.sdf index 5e08be28..0f65597e 100644 --- a/ros/src/f16/models/rocket/rocket.sdf +++ b/ros/src/f16/models/rocket/rocket.sdf @@ -121,24 +121,24 @@ - - - - - - - - - - - - - - - - - - + + 0.0 + 4.752798721 + 0.6417112299 + 0.0 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + 0 0 0 + 0.1 + 1.2041 + 0 0 1 + 0 1 0 + body + fin1 + -4.0 + 0 0 0 0 0 1.57 @@ -179,24 +179,24 @@ - - - - - - - - - - - - - - - - - - + + 0.0 + 4.752798721 + 0.6417112299 + 0.0 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + 0 0 0 + 0.1 + 1.2041 + 0 0 1 + 1 0 0 + body + fin2 + -4.0 + 0 0 0 0 0 3.14 @@ -237,24 +237,24 @@ - - - - - - - - - - - - - - - - - - + + 0.0 + 4.752798721 + 0.6417112299 + 0.0 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + 0 0 0 + 0.1 + 1.2041 + 0 0 1 + 0 1 0 + body + fin3 + -4.0 + 0 0 0 0 0 -1.57 From 10250c7f3a7f32172afa03cf5315a46165c9bb64 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 23 Sep 2019 16:16:09 -0400 Subject: [PATCH 2/2] Update rocket model. --- ros/src/f16/launch/rocket.launch | 2 + ros/src/f16/models/rocket/meshes/rocket.scad | 6 +- ros/src/f16/models/rocket/meshes/stl/fin.stl | Bin 6484 -> 6484 bytes ros/src/f16/models/rocket/rocket.sdf | 88 +++++++++++++++++-- ros/src/f16/scripts/rocket_controller.py | 71 +++------------ 5 files changed, 97 insertions(+), 70 deletions(-) diff --git a/ros/src/f16/launch/rocket.launch b/ros/src/f16/launch/rocket.launch index a04f37fc..0763169c 100644 --- a/ros/src/f16/launch/rocket.launch +++ b/ros/src/f16/launch/rocket.launch @@ -26,4 +26,6 @@ + + diff --git a/ros/src/f16/models/rocket/meshes/rocket.scad b/ros/src/f16/models/rocket/meshes/rocket.scad index 74160783..d5f3c06b 100644 --- a/ros/src/f16/models/rocket/meshes/rocket.scad +++ b/ros/src/f16/models/rocket/meshes/rocket.scad @@ -15,9 +15,9 @@ module body() { module fin() { color([0.3, 0.3, 0.3]) rotate([90, 0, 0]) linear_extrude(fh, center=true) polygon([ - [0, fl], - [fl, 0], - [0, 0] + [r+0.01, fl], + [r+0.01 + fl, 0], + [r+0.01, 0] ]); } diff --git a/ros/src/f16/models/rocket/meshes/stl/fin.stl b/ros/src/f16/models/rocket/meshes/stl/fin.stl index 09f3ffb0b859ba9c7ad33b6bde85cdf2d855e529..702ac99b0633ec57e91135c9229f0ae3b21eb56e 100644 GIT binary patch literal 6484 zcmb7IJ&R>U5beN7!H?X9(FH#OMFbJqaRvnA{SUHZr0y~*7~X#(`Y`ejEbM^qFPJ!q z$!4bN2!oMz>eT73{&;V3f_X*XK3(TjRaf`DpMLk~;pI0M4^BS6eE8+bCtqBA_2A<2 zo%i4U@S~HrzW(;JllR^(|Lbq7v$u1Yzn(w+`1$L*r+2RJ{wUjq&;I!S(V4~t*?xib z@0+il+mkCpynb{47PGE^kd1N7<7`-3xB1xJv*9z1!^XVhh&$K2ABZ3th*DlC+fP6S zE7w6LyL&(cD<3D~l?8(6jG$f0KhJ&Oar)DK6W?>j2Tb-gRa`OcCF~n`G*Q1HZRl*Tf z{?6A89m$6fWWpV#DnTXE=J?D?do6T$?Phe(Dl5-wVT&z(`yIt6TkJd5RaB)#8aY3P zAX>YY=tBtYdB&>wUF%{wGdFZ%8ZX>#8*GT*LVMIe` z#J;f_eB@a=eQy^cJq*)@Yll%oT*Sd|*Lxics}cp@9D zGP-BAp0!PgzKe1VMVIF}e8k$mkDi6-(LnYH-aj}P)Io-25#6jqYO}wU56@bcXCe@3R{NyfPO=h+ zeAepxu%J(T&**=gDWrq3mGZ21Mo2l& zRK`8dSHA)3WN5uz@0=MCZ;+nzj3z7j$g|?q4=Mmkst{C!!YY6!m5z0qq z%dxCOv%pIBjOa5+CXpN05gBxwtB6(9Rp;roc>%s?B(nr|#)4w~T8iepz8Jc%Ep+`iXr9HSp zYsuIc(1=KWH%{qANi>h=@5fMlC-{HntQrjai zs-CD_gwq*z=_ws?x^S` qm~RNLI_*K!k8FD=beuR2SRh(Ni%tgHwaf=F^pR&ZTV0z`FXcZe!n&IP literal 6484 zcmb7|L5e0t5QS@@AR{OqV4!tC#)Tq+i1grq;6-HGl{(TYxU8#a;}Kl-z|<}J9_j&% z7cXAs%dDzyEa8%Y1bATCd3#y^tXNQ0c|gg{C5cqk@>`bo4hiJs zgzSIc_&n?Mcij;qM4&BWAZyxahb>Q$Mo9Za|5Ic0zGA$%a9&22 z^Y^1A>sEGrJSMg?ItX7SiV%0Kl3R|ORVW)C@0zN<5>rX7MA%xPAY4`NS#w=+?GE%5 z10&Ss*;-Cf@7Hqach;=0t9+tr&hH}>3wNiFV$iqON(5(cB>KA2v(0&7JMD*vmQ$>; zBZ7LGSId^|Bw#BJ=0H1K&RXqow6(*X5u~FRbCDyJ4Pc59pUo?k6p`&D@MOl7lh2weMA_XE7yi?+NKmFRz=$8&z%D!*?z1=tn$zcbyZiaG(g5RtsS5Bd@N3s?F8*3!o6|^E5fhg5;c%!Am|uFAb+ zue*3oFmvqyRFW|=54E2XRjW>vLIAt!x>u^ITCu&&MY8<2)vUGmx%zTHc!=23Bf(u? zUU}-vaoNt<+!1X@c7-MDKvj&@2-<7r9D9@fqC3Fhb`N5gchx=v0KYtM%&Zcu(}5A=i~DT+==$D)S;bJreF! zcHW{^Ex(aqkLCcia{G7&F_yewQ#&(f`%w({a+`#o`j*U6`k_#jRvYjAoH!7%Gjzi3 zF^2byGuY`J(axWv6943?B1RF|6J-x-107GV@>;zLZU-^O=qDD7fgR3HGAD$q3PQ1w z&q#O=dU=gRKk08a2e9>gpbD}vLjTU`#QfdBi5|nf@~vObpvOausk*h6Jl?FT$3Tx( z!)9~f^qdDi*OmJp?U|UWs}(-fXROsR;R=_5KFb_LtEk!d2Km;Te|^Yrk}X?gZQs?n zBQU4&JEs%OHoPW!j)CfSucpf4%`vdzNO-*aRCOY274#aaoE~Gi7d&wt5nijPDBgF< zR=b3qMaLMZkBF>Q<&>2!`k+?LD@E}iYE^`OC#hd3B1YS(bDo(s^3z%L9s;|;ZG^xY zHhTW`!@P8+x|cxo_Rc69b8)U}B$zY^s8W7&9_B@}&Syv0&CJC#NS>gBP_c258v0`cAzi!QXne>V=0qVdI+KIQFFKDRLS_kKs?J*J~#F z8waZN7G}0o2wUxA3@2bqmD3=`7~KnfyhdVoBEy_po!nlt{ha8k6pQqpV7AT6jl&q) z6LIqH^UU>jtZF8x#Ob}NF>{XV1Z0ZJ`KXfxDynZ2v~}t~BSUQi{Q iIVGW|)YnhiM%$?`#yBFdQ|o + + + + model://rocket/meshes/stl/fin.stl + + + @@ -115,8 +122,8 @@ 1 0 0 - 0 - 0 + -1 + 1 @@ -164,6 +171,14 @@ + + + + model://rocket/meshes/stl/fin.stl + + + + @@ -173,8 +188,8 @@ 1 0 0 - 0 - 0 + -1 + 1 @@ -222,6 +237,14 @@ + + + + model://rocket/meshes/stl/fin.stl + + + + @@ -231,8 +254,8 @@ 1 0 0 - 0 - 0 + -1 + 1 @@ -280,6 +303,14 @@ + + + + model://rocket/meshes/stl/fin.stl + + + + @@ -289,8 +320,8 @@ 1 0 0 - 0 - 0 + -1 + 1 @@ -317,7 +348,7 @@ 0 0 0 0 0 0 - 0.8 + 0.00001 0 @@ -345,6 +376,45 @@ rocket_motor + + body + rocket/ground_truth + world + 100 + + + + true + 100 + 100.0 + fin1 + rocket/fin1_torque + + + + true + 100 + 100.0 + fin2 + rocket/fin2_torque + + + + true + 100 + 100.0 + fin3 + rocket/fin3_torque + + + + true + 100 + 100.0 + fin4 + rocket/fin4_torque + + diff --git a/ros/src/f16/scripts/rocket_controller.py b/ros/src/f16/scripts/rocket_controller.py index 2a920b35..aa42f318 100755 --- a/ros/src/f16/scripts/rocket_controller.py +++ b/ros/src/f16/scripts/rocket_controller.py @@ -15,10 +15,14 @@ class RocketController: def __init__(self): rospy.init_node('rocket_controller') self.time_last_plan = rospy.Time.now() - self.pub_rear_left = rospy.Publisher('/rocket/force_rear_left', Wrench, queue_size=10) - self.pub_rear_right = rospy.Publisher('/rocket/force_rear_right', Wrench, queue_size=10) - self.pub_front_left = rospy.Publisher('/rocket/force_front_left', Wrench, queue_size=10) - self.pub_front_right = rospy.Publisher('/rocket/force_front_right', Wrench, queue_size=10) + + # fin torques + self.pub_fin1 = rospy.Publisher('/rocket/fin1_torque', Wrench, queue_size=10) + self.pub_fin2 = rospy.Publisher('/rocket/fin2_torque', Wrench, queue_size=10) + self.pub_fin3 = rospy.Publisher('/rocket/fin3_torque', Wrench, queue_size=10) + self.pub_fin4 = rospy.Publisher('/rocket/fin4_torque', Wrench, queue_size=10) + + # where rocket is at self.sub_ground_truth = rospy.Subscriber('/rocket/ground_truth', Odometry, self.control_callback) @@ -30,6 +34,7 @@ def control_callback(self, odom): orientation = odom.pose.pose.orientation euler = tf.transformations.euler_from_quaternion( (orientation.x, orientation.y, orientation.z, orientation.w)) + pitch = euler[1] yaw = euler[2] # rotation rate @@ -46,67 +51,17 @@ def control_callback(self, odom): "base_link", "map") - #rospy.loginfo('position %f %f %f, m', position.x, position.y, position.z) - #rospy.loginfo('yaw %f, deg', np.rad2deg(yaw)) - #rospy.loginfo('yaw rate %f, deg/s', np.rad2deg(yaw_rate)) - - drive_torque = 0.1 - desired_yaw = np.deg2rad(90) - - # steering PD controller - steer_kP = 1 - steer_kD = 1 - yaw_error = desired_yaw - yaw - #rospy.loginfo("yaw error %f deg", np.rad2deg(yaw_error)) - yaw_rate_error = 0 - yaw_rate - steer_torque = steer_kP*yaw_error + steer_kD*yaw_rate_error - - # speed P controller - speed_kP = 1 - speed = np.sqrt(vel.x**2 + vel.y**2) - desired_speed = 0.1 - speed_error = desired_speed - speed - drive_torque = speed_kP*speed_error - #rospy.loginfo("speed error %f m/s", speed_error) - - # saturate torques - if np.abs(steer_torque) > 0.1: - steer_torque /= np.abs(steer_torque) - if np.abs(drive_torque) > 0.1: - drive_torque /= np.abs(drive_torque) - - # rear drive + # fin1 msg = Wrench() msg.force.x = 0 msg.force.y = 0 msg.force.z = 0 - msg.torque.x = -drive_torque + msg.torque.x = 10 msg.torque.y = 0 msg.torque.z = 0 - self.pub_rear_left.publish(msg) - self.pub_rear_right.publish(msg) - - # front left steering - msg = Wrench() - msg.force.x = 0 - msg.force.y = 0 - msg.force.z = 0 - msg.torque.x = 0 - msg.torque.y = 0 - msg.torque.z = steer_torque - self.pub_front_left.publish(msg) - - # front right steering - msg = Wrench() - msg.force.x = 0 - msg.force.y = 0 - msg.force.z = 0 - msg.torque.x = 0 - msg.torque.y = 0 - msg.torque.z = steer_torque - self.pub_front_right.publish(msg) + self.pub_fin1.publish(msg) if __name__ == "__main__": - CarController() + RocketController() rospy.spin()