Esempio n. 1
0
 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()
Esempio n. 7
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

        # 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
Esempio n. 9
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
Esempio n. 10
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