forked from cesar.alejandro/oscillation_ctrl
added sonar (topic: /mavros/distance_sensor)
This commit is contained in:
parent
ef4ea858ea
commit
59190e9d42
|
|
@ -2,7 +2,7 @@
|
|||
<sdf version='1.6'>
|
||||
<model name='spiri'>
|
||||
<link name='base'>
|
||||
<pose frame=''>0 0 0 0 -0 3.141</pose>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<gravity>1</gravity>
|
||||
<inertial>
|
||||
<pose frame=''>0 0.00268 -0.000742 0 -0 0</pose>
|
||||
|
|
@ -408,6 +408,118 @@
|
|||
<child>gps0_link</child>
|
||||
</joint>
|
||||
</model>
|
||||
<!--model name="sonar">
|
||||
<link name="sonar_link">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>0.000000000000001</mass>
|
||||
<inertia>
|
||||
<ixx>2.08333333e-7</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>2.08333333e-7</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>4.16666667e-8</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.005 0.015 0.005</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Black</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<sensor name="sonar" type="sonar">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<sonar>
|
||||
<min>0.02</min>
|
||||
<max>10.0</max>
|
||||
<radius>1.33974388395</radius>
|
||||
</sonar>
|
||||
<plugin name="SonarPlugin" filename="libgazebo_sonar_plugin.so">
|
||||
<robotNamespace></robotNamespace>
|
||||
<topic>/height_cesar</topic>
|
||||
</plugin>
|
||||
<always_on>1</always_on>
|
||||
<update_rate>20</update_rate>
|
||||
<visualize>true</visualize>
|
||||
</sensor>
|
||||
</link>
|
||||
</model>
|
||||
<joint name='sonar_joint' type='fixed'>
|
||||
<parent>base</parent>
|
||||
<child>sonar_link</child>
|
||||
</joint-->
|
||||
<!--sonar-->
|
||||
<link name="sonar_link">
|
||||
<pose frame=''>0 0 0 0 1.57 0</pose>
|
||||
<inertial> <!-- causes issues when omitted -->
|
||||
<mass>0.05</mass>
|
||||
<inertia>
|
||||
<ixx>2.08333333333e-07</ixx>
|
||||
<iyy>5.20833333333e-07</iyy>
|
||||
<izz>5.20833333333e-07</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="sonar_link_visual">
|
||||
<pose frame=''>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.008</length>
|
||||
<radius>0.004</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Blue</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<sensor type="ray" name="spiri_sonar">
|
||||
<update_rate>10</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>1</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>-0</min_angle>
|
||||
<max_angle>0</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.06</min> <!-- do not lower past 0.06 -->
|
||||
<max>35</max> <!-- do not increase past 35 -->
|
||||
<resolution>0.01</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<plugin name="spiri_sonar" filename="libgazebo_ros_range.so">
|
||||
<gaussianNoise>0.005</gaussianNoise>
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>10</updateRate>
|
||||
<topicName>/mavros/distance_sensor</topicName>
|
||||
<frameName>sonar_link</frameName>
|
||||
<fov>0</fov>
|
||||
<radiation>ultrasound</radiation>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
<joint name="sonar_joint" type="fixed">
|
||||
<parent>base</parent>
|
||||
<child>sonar_link</child>
|
||||
</joint>
|
||||
<plugin name='rosbag' filename='libgazebo_multirotor_base_plugin.so'>
|
||||
<robotNamespace/>
|
||||
<linkName>base</linkName>
|
||||
|
|
|
|||
|
|
@ -101,8 +101,10 @@ class Main:
|
|||
|
||||
# PD Thrust Controller terms
|
||||
# gains for thrust PD Controller
|
||||
self.Kp = 2.7
|
||||
self.Kd = 3 #2
|
||||
#self.Kp = 2.7
|
||||
#self.Kd = 3
|
||||
self.Kp = 2.7
|
||||
self.Kd = 3
|
||||
self.max_a = 14.2
|
||||
self.max_t = self.drone_m*self.max_a
|
||||
self.R = np.empty([3,3]) # rotation matrix
|
||||
|
|
@ -252,15 +254,22 @@ class Main:
|
|||
# Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
|
||||
self.waypoints_srv_cb()
|
||||
self.error = np.array([[self.dr_pos.position.x - self.xd.x],
|
||||
[self.dr_pos.position.y - self.xd.y],
|
||||
[self.dr_pos.position.z - self.xd.z - self.thrust_offset]])
|
||||
[self.dr_pos.position.y - self.xd.y],
|
||||
[self.dr_pos.position.z - self.xd.z - self.thrust_offset]])
|
||||
|
||||
self.R = self.quaternion_rotation_matrix() # determine Rotation Matrix
|
||||
self.error_vel = self.dr_vel - self.path_vel
|
||||
|
||||
self.R = self.quaternion_rotation_matrix() # determine Rotation Matrix
|
||||
self.R_e3 = np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]])
|
||||
|
||||
|
||||
#thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel.T,self.R.T[2])/self.max_t
|
||||
thrust_vector = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel.T,self.R.T[2])/self.max_t
|
||||
thrust = thrust_vector[2]
|
||||
#thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel,self.R.T[2])/self.max_t
|
||||
#thrust_vector = np.dot(9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel,self.R_e3)/self.max_t
|
||||
#thrust_vector = (9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel -9.81*self.drone_m*self.path_acc)/self.max_t
|
||||
thrust_vector = (9.81*self.drone_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t
|
||||
#thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2]
|
||||
thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1]))
|
||||
#thrust = thrust_vector[2]
|
||||
|
||||
# Value needs to be between 0 - 1.0
|
||||
self.att_targ.thrust = max(0.0,min(thrust,1.0))
|
||||
|
|
|
|||
Loading…
Reference in New Issue