def __init__(self):
rospy.init_node('Cartpole')
self._sub_invpend_states = rospy.Subscriber('/invpend/joint_states',
JointState,
self.jstates_callback)
self._pub_vel_cmd = rospy.Publisher(
'/invpend/joint1_velocity_controller/command',
Float64,
queue_size=50)
self._pub_pole_position_cmd = rospy.Publisher(
'/invpend/joint2_position_controller/command',
Float64,
queue_size=50)
self._pub_set_pole = rospy.Publisher('/gazebo/set_link_state',
LinkState,
queue_size=50)
self._pub_set_cart = rospy.Publisher('/gazebo/set_link_state',
LinkState,
queue_size=50)
# init observation parameters
self.pos_cart = 0
self.vel_cart = 0
self.pos_pole = 0
self.vel_pole = 0
self.reward = 0
self.out_range = False
self.reset_stamp = time.time()
self.time_elapse = 0.
# init reset_env parameters
self.reset_dur = .2 # reset duration, sec
self.freq = 1000 # topics pub and sub frequency, Hz
## pole
self.PoleState = LinkState()
self.PoleState.link_name = 'pole'
self.PoleState.pose.position = Point(
0.0, -0.275, 0.0) # pole's position w.r.t. world
self.PoleState.reference_frame = 'cart'
self.pole_length = 0.5
## cart
self.CartState = LinkState()
self.CartState.link_name = 'cart'
self.CartState.pose.position = Point(
0.0, 0.0, 0.0) # pole's position w.r.t. world
self.CartState.reference_frame = 'slidebar'
# velocity control command
self.cmd = 0
self.positionpid = pid_controller(10**-3, 5, -5, 2.1, 0.0, 0.4, 15)
self.velocitypid = pid_controller(10**-3, 5, -5, 1.5, 0.0, 0.0, 15)
def __init__(self, update_rate, wheel_dist, drive_kp, drive_ki, drive_kd,
drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd,
turn_ff, turn_max_output, max_linear_vel, max_angular_vel,
wpt_tol_default, wpt_def_drive_vel, wpt_def_turn_vel,
turn_start_at_heading_err, turn_stop_at_heading_err,
ramp_drive_vel_at_dist, ramp_min_drive_vel,
ramp_turn_vel_at_angle, ramp_min_turn_vel, stop_nav_at_dist,
debug):
# constants
self.UPDATE_NONE = 0
self.UPDATE_ARRIVAL = 1
self.pi2 = 2.0 * pi
self.deg_to_rad = pi / 180.0
self.rad_to_deg = 180.0 / pi
# list index for destination (b) and origin (a) waypoints
self.W_E = 0
self.W_N = 1
self.W_YAW = 2
self.W_ID = 3
self.W_MODE = 4
self.W_TOL = 5
self.W_LIN_VEL = 6
self.W_ANG_VEL = 7
self.W_WAIT = 8
self.W_IMPLEMENT = 9
# parameters
self.update_rate = update_rate # [Hz]
self.update_interval = (1.0 / self.update_rate) # [s]
self.wheel_dist = wheel_dist
self.max_linear_vel = max_linear_vel # [m/s]
self.max_angular_vel = max_angular_vel # [radians/s]
self.angular_vel_limit = self.max_angular_vel
self.drive_kp = drive_kp
self.drive_ki = drive_ki
self.drive_kd = drive_kd
self.drive_ff = drive_ff
self.drive_max_output = drive_max_output
self.turn_kp = turn_kp
self.turn_ki = turn_ki
self.turn_kd = turn_kd
self.turn_ff = turn_ff
self.turn_max_output = turn_max_output
self.wpt_tolerance_default = wpt_tol_default # [m]
self.wpt_def_drive_vel = wpt_def_drive_vel # [m/s]
self.wpt_def_turn_vel = wpt_def_turn_vel # [m/s]
self.turn_start_at_heading_err = turn_start_at_heading_err * self.deg_to_rad # [radians] set to 2pi if not applicable to the robot9
self.turn_acceptable_heading_err = turn_stop_at_heading_err * self.deg_to_rad # [radians]
self.ramp_drive_vel_at_dist_default = ramp_drive_vel_at_dist # [m]
self.ramp_min_drive_vel_default = ramp_min_drive_vel # [m/s]
self.ramp_turn_vel_at_angle_default = ramp_turn_vel_at_angle * pi / 180.0 # [deg]
self.ramp_min_turn_vel_default = ramp_min_turn_vel # [rad/s]
self.stop_nav_at_dist = stop_nav_at_dist
self.print_interval = self.update_rate / 20
# navigation controller state machine
self.STATE_STOP = 0
self.STATE_STANDBY = 1
self.STATE_DRIVE = 2
self.STATE_TURN = 3
self.state = self.STATE_STOP
self.prev_state = self.STATE_STOP
# reset waypoints
self.start = False
self.a = False
self.b = False
self.pose = False
self.vel = 0.0
self.target = False
self.wpt_tolerance = 0.0
self.wpt_drive_vel = 0.0
self.wpt_turn_vel = 0.0
self.wpt_ramp_drive_vel_at_dist = 0.0
self.wpt_ramp_min_drive_vel = 0.0
# initialize navigation state vars
self.dist = 0.0
self.dist_minimum = 20000000.0
self.bearing = 0.0
self.heading_err = 0.0
self.heading_err_prev = 0.0
#self.heading_err_minimum = self.pi2
self.ab_len = 0.0
self.ab_dist_to_pose = 0.0
self.ab_norm = [0, 0]
self.target_dist = 0.0
self.target_bearing = 0.0
self.target_heading_err = 0.0
#self.target_heading_err_prev = 0.0
self.turn_bearing_origin = 0.0
# PID drive controller
self.pid_drive = pid_controller(self.update_interval)
self.pid_drive.set_parameters(self.drive_kp, self.drive_ki,
self.drive_kd, self.drive_ff,
self.drive_max_output)
self.pid_status_reset = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.pid_status = self.pid_status_reset
# PID turn controller
self.pid_turn = pid_controller(self.update_interval)
self.pid_turn.set_parameters(self.turn_kp, self.turn_ki, self.turn_kd,
self.turn_ff, self.turn_max_output)
# initialize output
self.linear_vel = 0.0
self.angular_vel = 0.0
# debug
self.debug = debug
if self.debug:
self.debug_time_stamp = 0.0
self.print_count = 0
def __init__(self, update_rate, wheel_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd, turn_ff, turn_max_output, max_linear_vel, max_angular_vel, wpt_tol_default, wpt_def_drive_vel, wpt_def_turn_vel, target_ahead, turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel, stop_nav_at_dist, debug): # constants self.UPDATE_NONE = 0 self.UPDATE_ARRIVAL = 1 self.pi2 = 2.0*pi self.deg_to_rad = pi/180.0 self.rad_to_deg = 180.0/pi # list index for destination (b) and origin (a) waypoints self.W_E = 0 self.W_N = 1 self.W_YAW = 2 self.W_ID = 3 self.W_MODE = 4 self.W_TOL = 5 self.W_LIN_VEL = 6 self.W_ANG_VEL = 7 self.W_WAIT = 8 self.W_IMPLEMENT = 9 # parameters self.update_rate = update_rate # [Hz] self.update_interval = (1.0/self.update_rate) # [s] self.wheel_dist = wheel_dist self.max_linear_vel = max_linear_vel # [m/s] self.max_angular_vel = max_angular_vel # [radians/s] self.angular_vel_limit = self.max_angular_vel self.drive_kp = drive_kp self.drive_ki = drive_ki self.drive_kd = drive_kd self.drive_ff = drive_ff self.drive_max_output = drive_max_output self.turn_kp = turn_kp self.turn_ki = turn_ki self.turn_kd = turn_kd self.turn_ff = turn_ff self.turn_max_output = turn_max_output self.wpt_tolerance_default = wpt_tol_default # [m] self.wpt_def_drive_vel = wpt_def_drive_vel # [m/s] self.wpt_def_turn_vel = wpt_def_turn_vel # [m/s] self.target_ahead = target_ahead self.turn_start_at_heading_err = turn_start_at_heading_err*self.deg_to_rad # [radians] set to 2pi if not applicable to the robot9 self.turn_acceptable_heading_err = turn_stop_at_heading_err*self.deg_to_rad # [radians] self.ramp_drive_vel_at_dist_default = ramp_drive_vel_at_dist # [m] self.ramp_min_drive_vel_default = ramp_min_drive_vel # [m/s] self.ramp_turn_vel_at_angle_default = ramp_turn_vel_at_angle*pi/180.0 # [deg] self.ramp_min_turn_vel_default = ramp_min_turn_vel # [rad/s] self.stop_nav_at_dist = stop_nav_at_dist self.print_interval = self.update_rate/20 # navigation controller state machine self.STATE_STOP = 0 self.STATE_STANDBY = 1 self.STATE_DRIVE = 2 self.STATE_TURN = 3 self.state = self.STATE_STOP self.prev_state = self.STATE_STOP # reset waypoints self.start = False self.a = False self.b = False self.pose = False self.vel = 0.0 self.target = False self.wpt_tolerance = 0.0 self.wpt_drive_vel = 0.0 self.wpt_turn_vel = 0.0 self.wpt_ramp_drive_vel_at_dist = 0.0 self.wpt_ramp_min_drive_vel = 0.0 # initialize navigation state vars self.dist = 0.0 self.dist_minimum = 20000000.0 self.bearing = 0.0 self.heading_err = 0.0 self.heading_err_prev = 0.0 #self.heading_err_minimum = self.pi2 self.ab_len = 0.0 self.ab_dist_to_pose = 0.0 self.ab_norm = [0,0] self.target_dist = 0.0 self.target_bearing = 0.0 self.target_heading_err = 0.0 #self.target_heading_err_prev = 0.0 self.turn_bearing_origin = 0.0 # PID drive controller self.pid_drive = pid_controller(self.update_interval) self.pid_drive.set_parameters(self.drive_kp, self.drive_ki, self.drive_kd, self.drive_ff, self.drive_max_output) self.pid_status_reset = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.pid_status = self.pid_status_reset # PID turn controller self.pid_turn = pid_controller(self.update_interval) self.pid_turn.set_parameters(self.turn_kp, self.turn_ki, self.turn_kd, self.turn_ff, self.turn_max_output) # initialize output self.linear_vel = 0.0 self.angular_vel = 0.0 # debug self.debug = debug if self.debug: self.debug_time_stamp = 0.0 self.print_count = 0
def __init__(self, update_rate, drive_kp, drive_ki, drive_kd,
drive_int_max, turn_kp, turn_ki, turn_kd, turn_int_max,
lin_spd_max, ang_spd_max, wpt_tol_default,
wpt_target_distance, wpt_turn_start_at_heading_err,
wpt_turn_stop_at_heading_err, wpt_linear_velocity,
wpt_ramp_down_velocity_at_distance,
wpt_ramp_down_minimum_velocity, debug):
# constants
self.UPDATE_NONE = 0
self.UPDATE_ARRIVAL = 1
self.pi2 = 2.0 * pi
self.deg_to_rad = pi / 180.0
self.rad_to_deg = 180.0 / pi
# parameters
self.update_rate = update_rate # [Hz]
self.update_interval = (1.0 / self.update_rate) # [s]
self.linear_speed_max = lin_spd_max # [m/s]
self.angular_speed_max = ang_spd_max # [radians/s]
self.drive_kp = drive_kp
self.drive_ki = drive_ki
self.drive_kd = drive_kd
self.drive_integral_max = drive_int_max
self.turn_kp = turn_kp
self.turn_ki = turn_ki
self.turn_kd = turn_kd
self.turn_integral_max = turn_int_max
self.wpt_tolerance_default = wpt_tol_default # [m]
self.target_ahead = wpt_target_distance # [m] the intermediate target is along the ab line 'self.target_ahead' meters ahead of the pose
self.target_percent = 0.5 # [0;1] when distance to b is less than self.target_ahead, the target is self.target_percent times the distance to b ahead of the pose
self.turn_start_at_heading_err = wpt_turn_start_at_heading_err * self.deg_to_rad # [radians] set to 2pi if not applicable to the robot9
self.turn_acceptable_heading_err = wpt_turn_stop_at_heading_err * self.deg_to_rad # [radians]
self.wpt_linear_speed_default = wpt_linear_velocity # [m/s]
self.wpt_linear_ramp_down_at_dist_default = wpt_ramp_down_velocity_at_distance # [m]
self.wpt_linear_ramp_down_speed_default = wpt_ramp_down_minimum_velocity # [m/s]
self.print_interval = self.update_rate / 2
# navigation controller state machine
self.STATE_STOP = 0
self.STATE_STANDBY = 1
self.STATE_DRIVE_INIT = 2
self.STATE_DRIVE = 3
self.STATE_TURN_INIT = 4
self.STATE_TURN = 5
self.state = self.STATE_STOP
self.prev_state = self.STATE_STOP
# reset waypoints
self.b = False
self.a = False
self.pose = False
self.target = False
self.wpt_tolerance = 0.0
self.wpt_linear_speed = 0.0
self.wpt_linear_ramp_down_at_dist = 0.0
self.wpt_linear_ramp_down_speed = 0.0
# initialize navigation state vars
self.dist = 0.0
self.dist_minimum = 20000000.0
self.bearing = 0.0
self.heading_err = 0.0
self.heading_err_minimum = self.pi2
self.ab_len = 0.0
self.ab_dist_to_pose = 0.0
self.ab_norm = [0, 0]
self.target_dist = 0.0
self.target_bearing = 0.0
self.target_heading_err = 0.0
# PID drive controller
self.pid_drive = pid_controller(self.update_interval)
self.pid_drive.set_parameters(self.drive_kp, self.drive_ki,
self.drive_kd, self.drive_integral_max)
# PID turn controller
self.pid_turn = pid_controller(self.update_interval)
self.pid_turn.set_parameters(self.turn_kp, self.turn_ki, self.turn_kd,
self.turn_integral_max)
# initialize output
self.linear_speed = 0.0
self.angular_speed = 0.0
# debug
self.debug = debug
if self.debug:
self.debug_time_stamp = 0.0
self.print_count = 0
def __init__(self, update_rate, debug): # constants self.UPDATE_NONE = 0 self.UPDATE_ARRIVAL = 1 self.pi2 = 2.0*pi self.deg_to_rad = pi/180.0 self.rad_to_deg = 180.0/pi # parameters self.update_rate = update_rate # [Hz] self.update_interval = (1.0/self.update_rate) # [s] self.wpt_linear_speed_default = 0.7 # [m/s] self.linear_speed_max = 0.35 # [m/s] self.wpt_linear_ramp_down_speed_default = 0.3 # [m/s] self.wpt_linear_ramp_down_at_dist_default = 1.0 # [m] self.angular_speed_max = 0.30 # [radians/s] self.turn_start_at_heading_err = 17.0*self.deg_to_rad # [radians] set to 2pi if not applicable to the robot9 self.turn_acceptable_heading_err = 2.0*self.deg_to_rad # [radians] self.drive_kp =2.0 self.drive_ki = 0.20 self.drive_kd = 0.0 self.drive_integral_max = 0.7 self.turn_kp = 5.5 self.turn_ki = 0.0 self.turn_kd = 0.0 self.turn_integral_max = 1.0 self.target_ahead = 1.3 # [m] the intermediate target is along the ab line 'self.target_ahead' meters ahead of the pose self.target_percent = 0.5 # [0;1] when distance to b is less than self.target_ahead, the target is self.target_percent times the distance to b ahead of the pose self.wpt_tolerance_default = 0.5 # [m] self.print_interval = 10 # navigation controller state machine self.STATE_STOP = 0 self.STATE_STANDBY = 1 self.STATE_DRIVE_INIT = 2 self.STATE_DRIVE = 3 self.STATE_TURN_INIT = 4 self.STATE_TURN = 5 self.state = self.STATE_STOP self.prev_state = self.STATE_STOP # reset waypoints self.b = False self.a = False self.pose = False self.target = False self.wpt_tolerance = 0.0 self.wpt_linear_speed = 0.0 self.wpt_linear_ramp_down_at_dist = 0.0 self.wpt_linear_ramp_down_speed = 0.0 # initialize navigation state vars self.dist = 0.0 self.dist_minimum = 20000000.0 self.bearing = 0.0 self.heading_err = 0.0 self.heading_err_minimum = self.pi2 self.ab_len = 0.0 self.ab_dist_to_pose = 0.0 self.ab_norm = [0,0] self.target_dist = 0.0 self.target_bearing = 0.0 self.target_heading_err = 0.0 # PID drive controller self.pid_drive = pid_controller(self.update_interval) self.pid_drive.set_parameters(self.drive_kp, self.drive_ki, self.drive_kd, self.drive_integral_max) # PID turn controller self.pid_turn = pid_controller(self.update_interval) self.pid_turn.set_parameters(self.turn_kp, self.turn_ki, self.turn_kd, self.turn_integral_max) # initialize output self.linear_speed = 0.0 self.angular_speed = 0.0 # debug self.debug = debug if self.debug: self.debug_time_stamp = 0.0 self.print_count = 0
def __init__(self, update_rate, wheel_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd,
turn_ff, turn_max_output, max_linear_vel, max_angular_vel, wpt_tol_default, wpt_def_drive_vel, wpt_def_turn_vel,
turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle,
ramp_min_turn_vel, stop_nav_at_dist, debug):
self.param1="Heading Err"
self.param2 = "Linear Vel"
self.param3 = "Angular Vel"
self.param4 = "dist"
self.param5 = "pose - x"
self.param6 = "pose - y"
self.param7 = "pose - z"
self.param8 = "ab length"
self.param9 = "Error"
self.param10 = "Status"
self.param11 = "ab_dist_to_pose"
self.param12 = "d"
self.param13 = "pt[E]"
self.param14 = "pt[N]"
self.param15 = "Target[E]"
self.param16 = "Target[N]"
self.param17 = "Target_Heading_Error"
self.arr=False
self.id=0
self.log_results_strangeturn = LogResultsStrangeTurn(self.id)
self.logresults = LogResults(self.id,self.param1,self.param2,self.param3,self.param4,self.param5,
self.param6,self.param7,self.param8,self.param9,self.param10,
self.param11,self.param12,self.param13,self.param14,self.param15,
self.param16,self.param17)#0=SimpleAlgorithm, 1=Kjeld's Algorithm
self.UPDATE_NONE = 0
self.UPDATE_ARRIVAL = 1
self.pi2 = 2.0*pi
self.deg_to_rad = pi/180.0
self.rad_to_deg = 180.0/pi
self.debug_count=0
self.prev_time_strangeturn = rospy.Time.now().to_sec()
rospy.loginfo("Ishan Algorithm Running")
# list index for destination (b) and origin (a) waypoints
self.W_E = 0
self.W_N = 1
self.W_YAW = 2
self.W_ID = 3
self.W_MODE = 4
self.W_TOL = 5
self.W_LIN_VEL = 6
self.W_ANG_VEL = 7
self.W_WAIT = 8
self.W_IMPLEMENT = 9
# parameters
self.update_rate = update_rate # [Hz]
self.update_interval = (1.0/self.update_rate) # [s]
self.wheel_dist = wheel_dist
self.max_linear_vel = max_linear_vel # [m/s]
self.max_angular_vel = max_angular_vel # [radians/s]
self.angular_vel_limit = self.max_angular_vel
self.drive_kp = drive_kp
self.drive_ki = drive_ki
self.drive_kd = drive_kd
self.drive_ff = drive_ff
self.drive_max_output = drive_max_output
self.turn_kp = turn_kp
self.turn_ki = turn_ki
self.turn_kd = turn_kd
self.turn_ff = turn_ff
self.turn_max_output = turn_max_output
self.wpt_tolerance_default = wpt_tol_default # [m]
self.wpt_def_drive_vel = wpt_def_drive_vel # [m/s]
self.wpt_def_turn_vel = wpt_def_turn_vel # [m/s]
self.turn_start_at_heading_err = turn_start_at_heading_err*self.deg_to_rad # [radians] set to 2pi if not applicable to the robot9
self.turn_acceptable_heading_err = turn_stop_at_heading_err*self.deg_to_rad # [radians]
self.ramp_drive_vel_at_dist_default = ramp_drive_vel_at_dist # [m] default velocity for driving in a ramp
self.ramp_min_drive_vel_default = ramp_min_drive_vel # [m/s]
self.ramp_turn_vel_at_angle_default = ramp_turn_vel_at_angle*pi/180.0 # [deg] i think converting to radians
self.ramp_min_turn_vel_default = ramp_min_turn_vel # [rad/s]
self.stop_nav_at_dist = stop_nav_at_dist
self.print_interval = self.update_rate/20
# navigation controller state machine
self.STATE_STOP = 0 # value when robot is in stop state
self.STATE_STANDBY = 1 # value when robot is in standby state
self.STATE_DRIVE = 2 # value when robot is drive state
self.STATE_TURN = 3 # value when robot is in turning state
self.state = self.STATE_STOP # initial state is stop
self.prev_state = self.STATE_STOP # initial previous state is also stop
# reset waypoints
self.start = False
self.a = False # starting waypoint for the robot
self.b = False # Destination waypoint for the robot
self.pose = False # pose position of the robot. Initially set to false as no pose obtained
self.vel = 0.0 # velocity of the robot. initially set to 0
self.target = False # whether robot has reached the target or not. initally false
self.wpt_tolerance = 0.0 #
self.wpt_drive_vel = 0.0
self.wpt_turn_vel = 0.0
self.wpt_ramp_drive_vel_at_dist = 0.0
self.wpt_ramp_min_drive_vel = 0.0
# initialize navigation state vars
self.dist = 0.0
self.dist_minimum = 20000000.0# minimum distance at which it is decided that robot has reached the waypooint
self.bearing = 0.0
self.heading_err = 0.0 # Current error in the heading of the robot
self.heading_err_prev = 0.0 # previous error in the heading of the robot
#self.heading_err_minimum = self.pi2
self.ab_len = 0.0 # distance betweem the start and the destination waypoints for the robot to navigate
self.ab_dist_to_pose = 0.0 # not clear
self.ab_norm = [0,0] # not clear
self.target_dist = 0.0 # target distance that robot needs to navigate
self.target_bearing = 0.0 # target bearing of the robot
self.target_heading_err = 0.0 # target error position of the heading of the robot
#self.target_heading_err_prev = 0.0
self.turn_bearing_origin = 0.0 # not clear
# PID drive controller
self.pid_drive = pid_controller(self.update_interval) # For driving the robot call a pid controller class which initializes the parameters
# like kp, ki, kd and which updates the error at a fixed interval of time evry time
self.pid_drive.set_parameters(self.drive_kp, self.drive_ki, self.drive_kd, self.drive_ff, self.drive_max_output)
self.pid_status_reset = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # pid reset status indicates that all values for pid control are reset to 0
self.pid_status = self.pid_status_reset # the current status of the pid which is the reset state all 0
# PID turn controller
self.pid_turn = pid_controller(self.update_interval) # For turn controller call a pid controller class which initializes the parameters
# like kp, ki, kd and which updates the error at a fixed interval of time evry time
self.pid_turn.set_parameters(self.turn_kp, self.turn_ki, self.turn_kd, self.turn_ff, self.turn_max_output)
# initialize output
self.linear_vel = 0.0
self.angular_vel = 0.0
self.debug = debug # initially debug state set to true
self.prev_time = rospy.Time.now().to_sec()
def __init__(self, update_rate, debug):
# constants
self.UPDATE_NONE = 0
self.UPDATE_ARRIVAL = 1
self.pi2 = 2.0 * pi
self.deg_to_rad = pi / 180.0
self.rad_to_deg = 180.0 / pi
# navigation controller state constants
self.STATE_STOP = 0
self.STATE_STANDBY = 1
self.STATE_DRIVE_INIT = 2
self.STATE_DRIVE = 3
self.STATE_TURN_INIT = 4
self.STATE_TURN = 5
# parameters
self.update_rate = update_rate # [Hz]
self.update_interval = (1.0 / self.update_rate) # [s]
self.wpt_linear_speed_default = 0.5 # [m/s]
self.linear_speed_max = 0.7 # [m/s]
self.angular_speed_max = 0.25 # [radians/s]
self.wpt_tolerance_default = 0.5 # [m]
self.target_ahead = 1.5 # [m] the intermediate target is along the ab line 'self.target_ahead' meters ahead of the pose
self.target_percent = 0.5 # [0;1] when distance to b is less than self.target_ahead, the target is self.target_percent times the distance to b ahead of the pose
self.turn_start_at_heading_err = 15.0 * self.deg_to_rad # [radians] set to 2pi if not applicable to the robot
self.turn_acceptable_heading_err = 2.5 * self.deg_to_rad # [radians]
self.print_interval = 10
# state machine
self.state = self.STATE_STOP
self.prev_state = self.STATE_STOP
# reset waypoints
self.b = False
self.a = False
self.pose = False
self.target = False
self.wpt_tolerance = 0.0
self.wpt_linear_speed = 0.0
# initialize navigation state vars
self.dist = 0.0
self.dist_minimum = 20000000.0
self.bearing = 0.0
self.heading_err = 0.0
self.heading_err_minimum = self.pi2
self.ab_len = 0.0
self.ab_dist_to_pose = 0.0
self.ab_norm = [0, 0]
self.target_dist = 0.0
self.target_bearing = 0.0
self.target_heading_err = 0.0
# PID drive controller
self.pid_drive = pid_controller(self.update_interval)
Kp = 4.0
Ki = 0.0
Kd = 0.2
integral_max = 1.0
self.pid_drive.set_parameters(Kp, Ki, Kd, integral_max)
# PID turn controller
self.pid_turn = pid_controller(self.update_interval)
Kp = 0.8
Ki = 0.3
Kd = 0.0
integral_max = 1.0
self.pid_turn.set_parameters(Kp, Ki, Kd, integral_max)
# initialize output
self.linear_speed = 0.0
self.angular_speed = 0.0
# debug
self.debug = debug
if self.debug:
self.debug_time_stamp = 0.0
self.print_count = 0
def __init__(self, update_rate, debug): # constants self.UPDATE_NONE = 0 self.UPDATE_ARRIVAL = 1 self.pi2 = 2.0*pi self.deg_to_rad = pi/180.0 self.rad_to_deg = 180.0/pi # parameters self.update_rate = update_rate # [Hz] self.update_interval = 1.0/self.update_rate # [s] self.linear_speed_max = 1.2 # [m/s] self.angular_speed_max = pi/2.0 # [radians/s] self.wpt_tolerance_default = 0.5 # [m] self.wpt_linear_speed_default = 0.7 # [m/s] self.target_ahead = 0.8 # [m] the intermediate target is along the ab line 'self.target_ahead' meters ahead of the pose self.target_percent = 0.5 # [0;1] when distance to b is less than self.target_ahead, the target is self.target_percent times the distance to b ahead of the pose self.turn_start_at_heading_err = 20.0*self.deg_to_rad # [radians] set to 2pi if not applicable to the robot self.turn_acceptable_heading_err = 3.0*self.deg_to_rad # [radians] self.turn_speed = pi/3.0 # [radians/s] # state machine self.state = STATE_STOP # reset waypoints self.b = False self.a = False self.pose = False self.target = False self.wpt_tolerance = 0.0 self.wpt_linear_speed = 0.0 # initialize navigation state vars self.dist = 0.0 self.dist_minimum = 20000000.0 self.bearing = 0.0 self.heading_err = 0.0 self.heading_err_minimum = self.pi2 self.ab_len = 0.0 self.ab_dist_to_pose = 0.0 self.ab_norm = [0,0] self.target_dist = 0.0 self.target_bearing = 0.0 self.target_heading_err = 0.0 # PID drive controller self.pid_drive = pid_controller(self.update_interval) Kp = 2.5 Ki = 0.9 Kd = 0.0 integral_max = 1.5 self.pid_drive.set_parameters(Kp, Ki, Kd, integral_max) # PID turn controller self.pid_turn = pid_controller(self.update_interval) Kp = 1.1 Ki = 0.3 Kd = 0.0 integral_max = 1 self.pid_turn.set_parameters(Kp, Ki, Kd, integral_max) # initialize output self.linear_speed = 0.0 self.angular_speed = 0.0 # debug self.debug = debug if self.debug: self.debug_time_stamp = 0.0 self.print_interval = 10 self.print_count = 0
def __init__(self, update_rate, wheel_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd,
turn_ff, turn_max_output, max_linear_vel, max_angular_vel, wpt_tol_default, wpt_def_drive_vel, wpt_def_turn_vel,
turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle,
ramp_min_turn_vel, stop_nav_at_dist, debug):
# constants
self.UPDATE_NONE = 0
self.UPDATE_ARRIVAL = 1
self.pi2 = 2.0*pi
self.deg_to_rad = pi/180.0
self.rad_to_deg = 180.0/pi
# list index for destination (b) and origin (a) waypoints
self.W_E = 0
self.W_N = 1
self.W_YAW = 2
self.W_ID = 3
self.W_MODE = 4
self.W_TOL = 5
self.W_LIN_VEL = 6
self.W_ANG_VEL = 7
self.W_WAIT = 8
self.W_IMPLEMENT = 9
# parameters
self.update_rate = update_rate # [Hz]
self.update_interval = (1.0/self.update_rate) # [s]
self.wheel_dist = wheel_dist
self.max_linear_vel = max_linear_vel # [m/s]
self.max_angular_vel = max_angular_vel # [radians/s]
self.angular_vel_limit = self.max_angular_vel
self.drive_kp = drive_kp
self.drive_ki = drive_ki
self.drive_kd = drive_kd
self.drive_ff = drive_ff
self.drive_max_output = drive_max_output
self.turn_kp = turn_kp
self.turn_ki = turn_ki
self.turn_kd = turn_kd
self.turn_ff = turn_ff
self.turn_max_output = turn_max_output
self.wpt_tolerance_default = wpt_tol_default # [m]
self.wpt_def_drive_vel = wpt_def_drive_vel # [m/s]
self.wpt_def_turn_vel = wpt_def_turn_vel # [m/s]
self.turn_start_at_heading_err = turn_start_at_heading_err*self.deg_to_rad # [radians] set to 2pi if not applicable to the robot9
self.turn_acceptable_heading_err = turn_stop_at_heading_err*self.deg_to_rad # [radians]
self.ramp_drive_vel_at_dist_default = ramp_drive_vel_at_dist # [m] default velocity for driving in a ramp
self.ramp_min_drive_vel_default = ramp_min_drive_vel # [m/s]
self.ramp_turn_vel_at_angle_default = ramp_turn_vel_at_angle*pi/180.0 # [deg] i think converting to radians
self.ramp_min_turn_vel_default = ramp_min_turn_vel # [rad/s]
self.stop_nav_at_dist = stop_nav_at_dist
self.print_interval = self.update_rate/20
# navigation controller state machine
self.STATE_STOP = 0 # value when robot is in stop state
self.STATE_STANDBY = 1 # value when robot is in standby state
self.STATE_DRIVE = 2 # value when robot is drive state
self.STATE_TURN = 3 # value when robot is in turning state
self.state = self.STATE_STOP # initial state is stop
self.prev_state = self.STATE_STOP # initial previous state is also stop
# reset waypoints
self.start = False # Value whether robot should start or not. Initially false as robot is stopped
self.a = False # Starting waypoint for the robot
self.b = False # Destination waypoint for the robot
self.pose = False # pose position of the robot. Initially set to false as no pose obtained
self.vel = 0.0 # velocity of the robot. initially set to 0
self.target = False # whether robot has reached the target or not. initally false
self.wpt_tolerance = 0.0 #
self.wpt_drive_vel = 0.0
self.wpt_turn_vel = 0.0
self.wpt_ramp_drive_vel_at_dist = 0.0
self.wpt_ramp_min_drive_vel = 0.0
# initialize navigation state vars
self.dist = 0.0
self.dist_minimum = 20000000.0# minimum distance at which it is decided that robot has reached the waypooint
self.bearing = 0.0
self.heading_err = 0.0 # Current error in the heading of the robot
self.heading_err_prev = 0.0 # previous error in the heading of the robot
#self.heading_err_minimum = self.pi2
self.ab_len = 0.0 # distance betweem the start and the destination waypoints for the robot to navigate
self.ab_dist_to_pose = 0.0 # not clear
self.ab_norm = [0,0] # not clear
self.target_dist = 0.0 # target distance that robot needs to navigate
self.target_bearing = 0.0 # target bearing of the robot
self.target_heading_err = 0.0 # target error position of the heading of the robot
#self.target_heading_err_prev = 0.0
self.turn_bearing_origin = 0.0 # not clear
# PID drive controller
self.pid_drive = pid_controller(self.update_interval) # For driving the robot call a pid controller class which initializes the parameters
# like kp, ki, kd and which updates the error at a fixed interval of time evry time
self.pid_drive.set_parameters(self.drive_kp, self.drive_ki, self.drive_kd, self.drive_ff, self.drive_max_output)
self.pid_status_reset = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # pid reset status indicates that all values for pid control are reset to 0
self.pid_status = self.pid_status_reset # the current status of the pid which is the reset state all 0
# PID turn controller
self.pid_turn = pid_controller(self.update_interval) # For turn controller call a pid controller class which initializes the parameters
# like kp, ki, kd and which updates the error at a fixed interval of time evry time
self.pid_turn.set_parameters(self.turn_kp, self.turn_ki, self.turn_kd, self.turn_ff, self.turn_max_output)
# initialize output
self.linear_vel = 0.0
self.angular_vel = 0.0
# debug
self.debug = debug # initially debug state set to true
if self.debug:
self.debug_time_stamp = 0.0
self.print_count = 0
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
self.state = self.create.update()
if self.state is not None:
stdVel = 0
self.time.sleep(0.1)
betterControl = pid_controller(100, 10, 10, self.time.time(), 10)
speedControl = pid_controller(200, 10, 10, self.time.time(), 0.1)
initAngle = 0
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
theta = self.odometry.theta
goalX = 1
goalY = 1
deltaX = goalX - self.odometry.x
deltaY = goalY - self.odometry.y
goalAngle = math.atan2(deltaY, deltaX)
while True:
self.state = self.create.update()
if self.state is not None:
self.odometry.update(self.state.leftEncoderCounts, self.state.rightEncoderCounts)
theta = self.odometry.theta
deltaX = goalX - self.odometry.x
deltaY = goalY - self.odometry.y
goalAngle = math.atan2(deltaY, deltaX)
betterVel = betterControl.update(goalAngle, theta, self.time.time())
eucDist = math.sqrt(pow(deltaX, 2) + pow(deltaY, 2))
stdVel = speedControl.update(0, eucDist, self.time.time())
print(theta, goalAngle)
if abs(eucDist) < 0.05:
break
rVel = -betterVel - stdVel
lVel = (betterVel) - stdVel
if rVel > 200:
rVel = 200
elif rVel < -200:
rVel = -200
if lVel > 200:
lVel = 200
elif lVel < -200:
lVel = -200
self.create.drive_direct(int(lVel), int(rVel))
self.time.sleep(0.1)
def __init__(self, update_rate, wheel_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd, turn_ff, turn_max_output, max_linear_vel, max_angular_vel, wpt_tol_default, wpt_def_drive_vel, wpt_def_turn_vel, turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel, stop_nav_at_dist, debug):
# constants
self.UPDATE_NONE = 0
self.id=1
self.UPDATE_ARRIVAL = 1
self.pi2 = 2.0*pi
self.deg_to_rad = pi/180.0
self.rad_to_deg = 180.0/pi
rospy.loginfo("Kjeld's Algorithm Running")
self.param1="Heading Err"
self.param2 = "Linear Vel"
self.param3 = "Angular Vel"
self.param4 = "dist"
self.param5 = "pose - x"
self.param6 = "pose - y"
self.param7 = "pose - z"
self.param8 = "ab length"
self.param9 = "Error"
self.param10 = "Status"
self.param11 = "ab_dist_to_pose"
self.param12 = "d"
self.param13 = "pt[E]"
self.param14 = "pt[N]"
self.param15 = "Target[E]"
self.param16 = "Target[N]"
self.param17 = "Target_Heading_Error"
self.logresults = LogResults(self.id,self.param1,self.param2,self.param3,self.param4,self.param5,
self.param6,self.param7,self.param8,self.param9,self.param10,
self.param11,self.param12,self.param13,self.param14,self.param15,
self.param16,self.param17)
self.W_E = 0
self.W_N = 1
self.W_YAW = 2
self.W_ID = 3
self.W_MODE = 4
self.W_TOL = 5
self.W_LIN_VEL = 6
self.W_ANG_VEL = 7
self.W_WAIT = 8
self.W_IMPLEMENT = 9
# parameters
self.arr=0
self.update_rate = update_rate # [Hz]
self.update_interval = (1.0/self.update_rate) # [s]
self.wheel_dist = wheel_dist
self.max_linear_vel = max_linear_vel # [m/s]
self.max_angular_vel = max_angular_vel # [radians/s]
self.angular_vel_limit = self.max_angular_vel
self.drive_kp = drive_kp
self.drive_ki = drive_ki
self.drive_kd = drive_kd
self.drive_ff = drive_ff
self.drive_max_output = drive_max_output
rospy.loginfo("KP is "+str(self.drive_kp))
self.log_results_strangeturn = LogResultsStrangeTurn(self.id,self.drive_kp,
self.drive_ki,self.drive_kd,
self.drive_ff)
self.turn_kp = turn_kp
self.turn_ki = turn_ki
self.turn_kd = turn_kd
self.turn_ff = turn_ff
self.turn_max_output = turn_max_output
self.wpt_tolerance_default = wpt_tol_default # [m]
self.wpt_def_drive_vel = wpt_def_drive_vel # [m/s]
self.wpt_def_turn_vel = wpt_def_turn_vel # [m/s]
self.turn_start_at_heading_err = turn_start_at_heading_err*self.deg_to_rad # [radians] set to 2pi if not applicable to the robot9
self.turn_acceptable_heading_err = turn_stop_at_heading_err*self.deg_to_rad # [radians]
self.ramp_drive_vel_at_dist_default = ramp_drive_vel_at_dist # [m]
self.ramp_min_drive_vel_default = ramp_min_drive_vel # [m/s]
self.ramp_turn_vel_at_angle_default = ramp_turn_vel_at_angle*pi/180.0 # [deg]
self.ramp_min_turn_vel_default = ramp_min_turn_vel # [rad/s]
self.stop_nav_at_dist = stop_nav_at_dist
self.print_interval = self.update_rate/20
# navigation controller state machine
self.STATE_STOP = 0
self.STATE_STANDBY = 1
self.STATE_DRIVE = 2
self.STATE_TURN = 3
self.state = self.STATE_STOP
self.prev_state = self.STATE_STOP
# reset waypoints
self.start = False
self.a = False
self.b = False
self.pose = False
self.vel = 0.0
self.target = False
self.wpt_tolerance = 0.0
self.wpt_drive_vel = 0.0
self.wpt_turn_vel = 0.0
self.wpt_ramp_drive_vel_at_dist = 0.0
self.wpt_ramp_min_drive_vel = 0.0
# initialize navigation state vars
self.dist = 0.0
self.dist_minimum = 20000000.0
self.bearing = 0.0
self.heading_err = 0.0
self.heading_err_prev = 0.0
#self.heading_err_minimum = self.pi2
self.ab_len = 0.0
self.ab_dist_to_pose = 0.0
self.ab_norm = [0,0]
self.target_dist = 0.0
self.target_bearing = 0.0
self.target_heading_err = 0.0
#self.target_heading_err_prev = 0.0
self.turn_bearing_origin = 0.0
# PID drive controller
self.pid_drive = pid_controller(self.update_interval)
self.pid_drive.set_parameters(self.drive_kp, self.drive_ki, self.drive_kd, self.drive_ff, self.drive_max_output)
self.pid_status_reset = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.pid_status = self.pid_status_reset
# PID turn controller
self.pid_turn = pid_controller(self.update_interval)
self.pid_turn.set_parameters(self.turn_kp, self.turn_ki, self.turn_kd, self.turn_ff, self.turn_max_output)
# initialize output
self.linear_vel = 0.0
self.angular_vel = 0.0
self.prev_time = rospy.Time.now().to_sec()
self.prev_time_strangeturn = rospy.Time.now().to_sec()
self.dee=0
self.point=0
self.dist_pt_to_target_g = 0
# debug
self.debug = debug
if self.debug:
self.debug_time_stamp = 0.0
self.print_count = 0
