Skip to content

Commit

Permalink
Merge branch 'master' of github.com:jgoppert/aae497-f19
Browse files Browse the repository at this point in the history
  • Loading branch information
@AlexanderJChapa
AlexanderJChapa committed Sep 23, 2019
2 parents 759bca0 + 10250c7 commit e0a5d2f
Show file tree
Hide file tree
Showing 5 changed files with 151 additions and 124 deletions.
2 changes: 2 additions & 0 deletions ros/src/f16/launch/rocket.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,4 +26,6 @@
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>

<node name="rocket_controller" pkg="f16" type="rocket_controller.py"/>

</launch>
6 changes: 3 additions & 3 deletions ros/src/f16/models/rocket/meshes/rocket.scad
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
]);
}

Expand Down
Binary file modified ros/src/f16/models/rocket/meshes/stl/fin.stl
View file
Binary file not shown.
196 changes: 133 additions & 63 deletions ros/src/f16/models/rocket/rocket.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,13 @@
</script>
</material>
</visual>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://rocket/meshes/stl/fin.stl</uri>
</mesh>
</geometry>
</collision>
</link>

<joint name='fin1' type='revolute'>
Expand All @@ -115,30 +122,30 @@
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<lower>-1</lower>
<upper>1</upper>
</limit>
</axis>
</joint>

<!--<plugin name="fin1" filename="libLiftDragPlugin.so">-->
<!--<a0>0.0</a0>-->
<!--<cla>4.752798721</cla>-->
<!--<cda>0.6417112299</cda>-->
<!--<cma>0.0</cma>-->
<!--<alpha_stall>0.3391428111</alpha_stall>-->
<!--<cla_stall>-3.85</cla_stall>-->
<!--<cda_stall>-0.9233984055</cda_stall>-->
<!--<cma_stall>0</cma_stall>-->
<!--<cp>0 0 0</cp>-->
<!--<area>0.1</area>-->
<!--<air_density>1.2041</air_density>-->
<!--<forward>0 0 1</forward>-->
<!--<upward>0 1 0</upward>-->
<!--<link_name>body</link_name>-->
<!--<control_joint_name>fin1</control_joint_name>-->
<!--<control_joint_rad_to_cl>-4.0</control_joint_rad_to_cl>-->
<!--</plugin>-->
<plugin name="fin1" filename="libLiftDragPlugin.so">
<a0>0.0</a0>
<cla>4.752798721</cla>
<cda>0.6417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.3391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0</cma_stall>
<cp>0 0 0</cp>
<area>0.1</area>
<air_density>1.2041</air_density>
<forward>0 0 1</forward>
<upward>0 1 0</upward>
<link_name>body</link_name>
<control_joint_name>fin1</control_joint_name>
<control_joint_rad_to_cl>-4.0</control_joint_rad_to_cl>
</plugin>

<link name='fin2'>
<pose>0 0 0 0 0 1.57</pose>
Expand All @@ -164,6 +171,14 @@
</script>
</material>
</visual>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://rocket/meshes/stl/fin.stl</uri>
</mesh>
</geometry>
</collision>

</link>

<joint name='fin2' type='revolute'>
Expand All @@ -173,30 +188,30 @@
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<lower>-1</lower>
<upper>1</upper>
</limit>
</axis>
</joint>

<!--<plugin name="fin2" filename="libLiftDragPlugin.so">-->
<!--<a0>0.0</a0>-->
<!--<cla>4.752798721</cla>-->
<!--<cda>0.6417112299</cda>-->
<!--<cma>0.0</cma>-->
<!--<alpha_stall>0.3391428111</alpha_stall>-->
<!--<cla_stall>-3.85</cla_stall>-->
<!--<cda_stall>-0.9233984055</cda_stall>-->
<!--<cma_stall>0</cma_stall>-->
<!--<cp>0 0 0</cp>-->
<!--<area>0.1</area>-->
<!--<air_density>1.2041</air_density>-->
<!--<forward>0 0 1</forward>-->
<!--<upward>1 0 0</upward>-->
<!--<link_name>body</link_name>-->
<!--<control_joint_name>fin2</control_joint_name>-->
<!--<control_joint_rad_to_cl>-4.0</control_joint_rad_to_cl>-->
<!--</plugin>-->
<plugin name="fin2" filename="libLiftDragPlugin.so">
<a0>0.0</a0>
<cla>4.752798721</cla>
<cda>0.6417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.3391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0</cma_stall>
<cp>0 0 0</cp>
<area>0.1</area>
<air_density>1.2041</air_density>
<forward>0 0 1</forward>
<upward>1 0 0</upward>
<link_name>body</link_name>
<control_joint_name>fin2</control_joint_name>
<control_joint_rad_to_cl>-4.0</control_joint_rad_to_cl>
</plugin>

<link name='fin3'>
<pose>0 0 0 0 0 3.14</pose>
Expand All @@ -222,6 +237,14 @@
</script>
</material>
</visual>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://rocket/meshes/stl/fin.stl</uri>
</mesh>
</geometry>
</collision>

</link>

<joint name='fin3' type='revolute'>
Expand All @@ -231,30 +254,30 @@
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<lower>-1</lower>
<upper>1</upper>
</limit>
</axis>
</joint>

<!--<plugin name="fin3" filename="libLiftDragPlugin.so">-->
<!--<a0>0.0</a0>-->
<!--<cla>4.752798721</cla>-->
<!--<cda>0.6417112299</cda>-->
<!--<cma>0.0</cma>-->
<!--<alpha_stall>0.3391428111</alpha_stall>-->
<!--<cla_stall>-3.85</cla_stall>-->
<!--<cda_stall>-0.9233984055</cda_stall>-->
<!--<cma_stall>0</cma_stall>-->
<!--<cp>0 0 0</cp>-->
<!--<area>0.1</area>-->
<!--<air_density>1.2041</air_density>-->
<!--<forward>0 0 1</forward>-->
<!--<upward>0 1 0</upward>-->
<!--<link_name>body</link_name>-->
<!--<control_joint_name>fin3</control_joint_name>-->
<!--<control_joint_rad_to_cl>-4.0</control_joint_rad_to_cl>-->
<!--</plugin>-->
<plugin name="fin3" filename="libLiftDragPlugin.so">
<a0>0.0</a0>
<cla>4.752798721</cla>
<cda>0.6417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.3391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0</cma_stall>
<cp>0 0 0</cp>
<area>0.1</area>
<air_density>1.2041</air_density>
<forward>0 0 1</forward>
<upward>0 1 0</upward>
<link_name>body</link_name>
<control_joint_name>fin3</control_joint_name>
<control_joint_rad_to_cl>-4.0</control_joint_rad_to_cl>
</plugin>

<link name='fin4'>
<pose>0 0 0 0 0 -1.57</pose>
Expand All @@ -280,6 +303,14 @@
</script>
</material>
</visual>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://rocket/meshes/stl/fin.stl</uri>
</mesh>
</geometry>
</collision>

</link>

<joint name='fin4' type='revolute'>
Expand All @@ -289,8 +320,8 @@
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<lower>-1</lower>
<upper>1</upper>
</limit>
</axis>
</joint>
Expand All @@ -317,7 +348,7 @@
<link name='rocket_motor'>
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>0.8</mass>
<mass>0.00001</mass>
</inertial>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
Expand Down Expand Up @@ -345,6 +376,45 @@
<motor>rocket_motor</motor>
</plugin>

<plugin name="p3d" filename="libgazebo_ros_p3d.so">
<bodyName>body</bodyName>
<topicName>rocket/ground_truth</topicName>
<frameName>world</frameName>
<updateRate>100</updateRate>
</plugin>

<plugin name="fin1_torque" filename="libgazebo_ros_force.so">
<alwaysOn>true</alwaysOn>
<update>100</update>
<updateRate>100.0</updateRate>
<bodyName>fin1</bodyName>
<topicName>rocket/fin1_torque</topicName>
</plugin>

<plugin name="fin2_torque" filename="libgazebo_ros_force.so">
<alwaysOn>true</alwaysOn>
<update>100</update>
<updateRate>100.0</updateRate>
<bodyName>fin2</bodyName>
<topicName>rocket/fin2_torque</topicName>
</plugin>

<plugin name="fin3_torque" filename="libgazebo_ros_force.so">
<alwaysOn>true</alwaysOn>
<update>100</update>
<updateRate>100.0</updateRate>
<bodyName>fin3</bodyName>
<topicName>rocket/fin3_torque</topicName>
</plugin>

<plugin name="fin4_torque" filename="libgazebo_ros_force.so">
<alwaysOn>true</alwaysOn>
<update>100</update>
<updateRate>100.0</updateRate>
<bodyName>fin4</bodyName>
<topicName>rocket/fin4_torque</topicName>
</plugin>

</model>
</sdf>

Expand Down
71 changes: 13 additions & 58 deletions ros/src/f16/scripts/rocket_controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand All @@ -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
Expand All @@ -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()

0 comments on commit e0a5d2f

Please sign in to comment.