Skip to content

Commit

Permalink
Fine-tuned the controller for Gazebo
Browse files Browse the repository at this point in the history
  • Loading branch information
turnwald committed Nov 18, 2016
1 parent bdeddbc commit 53ac8a6
Show file tree
Hide file tree
Showing 4 changed files with 35 additions and 111 deletions.
36 changes: 2 additions & 34 deletions .idea/suricate_robot.iml
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

52 changes: 1 addition & 51 deletions simulation/rqt_multiplot_config.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,56 +124,6 @@
<title>vel_x</title>
</curve_1>
<curve_2>
<axes>
<x_axis>
<field></field>
<field_type>1</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-10</relative_minimum>
<type>0</type>
</scale>
<topic>/robot/odom</topic>
<type>nav_msgs/Odometry</type>
</x_axis>
<y_axis>
<field>pose/pose/position/x</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-10</relative_minimum>
<type>0</type>
</scale>
<topic>/robot/odom</topic>
<type>nav_msgs/Odometry</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>1</subscriber_queue_size>
<title>pos_x</title>
</curve_2>
<curve_3>
<axes>
<x_axis>
<field></field>
Expand Down Expand Up @@ -222,7 +172,7 @@
</style>
<subscriber_queue_size>1</subscriber_queue_size>
<title>angle</title>
</curve_3>
</curve_2>
</curves>
<legend>
<visible>true</visible>
Expand Down
13 changes: 9 additions & 4 deletions simulation/worlds/suricate.world
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,11 @@
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
<ode>
<solver> # added from internet to improve friction behaviour
<iters>10</iters>
</solver>
</ode>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
Expand Down Expand Up @@ -249,7 +254,7 @@
<surface>
<friction>
<ode>
<mu>100</mu>
<mu>500</mu>
<mu2>500</mu2>
</ode>
<torsional>
Expand Down Expand Up @@ -1290,8 +1295,8 @@
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>100</mu2>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
Expand Down Expand Up @@ -1892,7 +1897,7 @@


<plugin name='wrench_driver' filename='libwrench_driver.so'>
<updateRate>40</updateRate>
<updateRate>150</updateRate>
<joint_left>join_wheel_left</joint_left>
<link_left>link_wheel_left</link_left>
<joint_right>join_wheel_right</joint_right>
Expand Down
45 changes: 23 additions & 22 deletions src/robot_controller_LQR_V1.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ def process_odometry_message(self, odometry_msg):
odometry_msg.pose.pose.orientation.w)


#self.Idx = self.current_position_x #this is in inertial frame!
self.Idx = self.current_position_x #this is in inertial frame!
(tmDummy, tmpDummy, self.Idr) = euler_from_quaternion(quaternion)

def process_twist_message(self, twist_msg):
Expand All @@ -130,21 +130,21 @@ def reconfig_callback(self, config, level):

def update_controller(self):
# setup up sampling time at 10 hz
#v_xi = self.Idx # integral self.dx from encoder (relative theta * r_w)
v_xi = self.Idx # integral self.dx from encoder (relative theta * r_w)
v_psi = self.Idr # integral self.dr
v_phi = self.pitch - 0.017 # pitch
v_phi = self.pitch - 0.0 # pitch
dv_xi = self.dx # self.dx from encoder (relative theta * r_w)
dv_psi = self.dr # self.dr
dv_phi = self.gz # gy ? or gz


# correction of the angle
r_w = 0.273
# correction of the angle (only for robot)
#r_w = 0.273
#v_xi = v_xi - v_phi*r_w
dv_xi = dv_xi - dv_phi*r_w
self.Idx = self.Idx + dv_xi / 150
v_xi = self.Idx / 1.8 # correction factor
self.IIdx = self.IIdx + v_xi / 150
#dv_xi = dv_xi - dv_phi*r_w
#self.Idx = self.Idx + dv_xi / 150
#v_xi = self.Idx / 1.8 # correction factor
#self.IIdx = self.IIdx + v_xi / 150

self.xi_des = 0
self.psi_des = 0
Expand All @@ -156,10 +156,10 @@ def update_controller(self):
# (0, 1.6286, 0, 0, 2.6639, 0) ] )
#k_dr = np.array( [(-1.7321/1000 , -0, -170.77, -6.756/1000 , 0, 39.551), # cont
# (0, 1.6286, 0, 0, 2.6639, 0) ] )
k_dr = np.array( [( -2.27/20 , -0, -155.1, -6.6/70, 0, 37.9), # cont
(0, -0.3088, 0, 0, -0.55, 0) ] )
#k_dr = np.array( [( 2.27*5 , -0, -155.1/1, 6.6/1.5, 0, 37.9/3), # cont
# (0, -0.3088, 0, 0, -0.55, 0) ] )
#k_dr = np.array( [( -2.27/20 , -0, -155.1, -6.6/70, 0, 37.9), # cont
# (0, -0.3088, 0, 0, -0.55, 0) ] )
k_dr = np.array( [( -1.4*11 , -0, -194.5*1.5, -5.82*6.5, 0, -49.3/1.95), # cont for gazebo
(0, -0.3088, 0, 0, -0.55, 0) ] )


u = np.dot(k_dr, x_transposed) # u = [linear.x; angular.z]
Expand All @@ -171,26 +171,27 @@ def update_controller(self):

#====================================
# compensate for the dead zone
if np.abs(u[0]) < 2.5 :
if np.abs(u[0]) < 0.03:
u[0] = 0.0
else :
u[0] = 2.5 *np.sign(u[0])
#if np.abs(u[0]) < 2.5 :
# if np.abs(u[0]) < 0.1:
# u[0] = 0.0
# else :
# u[0] = 2.5 *np.sign(u[0])
#====================================

self.gain_LQR = 0.0019
#self.gain_LQR = 0.0019 # for the robot
self.gain_LQR = 0.05 # for gazebo
# Publish new wrench value
self.wrench_cmd.force.x = self.bound_limit(u[0] * self.gain_LQR, -2, 2)
self.wrench_cmd.force.y = 0
self.wrench_cmd.force.z = 0
self.wrench_cmd.torque.x = 0
self.wrench_cmd.torque.y = 0 # self.bound_limit(u[0]*0.2, -10, 10)
self.wrench_cmd.torque.z = self.bound_limit(u[1] * self.gain_LQR * 0, -2, 2)
self.wrench_cmd.torque.z = self.bound_limit(u[1] * self.gain_LQR * 1, -2, 2)
self.pub_wrench.publish(self.wrench_cmd)

self.angle_msg.data = v_phi #* rad2degrees
self.angle_msg.data = dv_phi #* rad2degrees
self.pub_angle.publish(self.angle_msg )
self.path_msg.data = v_xi
self.path_msg.data = dv_xi
self.pub_path.publish(self.path_msg)

def disable_controller(self):
Expand Down

0 comments on commit 53ac8a6

Please sign in to comment.