def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.arm = factory.create_kuka_lbr4p()
self.virtual_create = factory.create_virtual_create()
# self.virtual_create = factory.create_virtual_create("192.168.1.221")
self.odometry = odometry.Odometry()
self.particle_map = lab9_map.Map("finalproject_map2.json")
self.map = lab11_map.Map("finalproject_map2_config.png")
self.path = lab11.Run(factory)
# TODO identify good PID controller gains
self.pidTheta = pid_controller.PIDController(200,
0,
100, [-10, 10], [-50, 50],
is_angle=True)
# TODO identify good particle filter parameters
self.pf = particle_filter.ParticleFilter(self.particle_map, 1200, 0.10,
0.20, 0.1)
self.joint_angles = np.zeros(7)
def __init__(self, number, radius, length):
self.servo = Servo(number)
self.arm = radius + length
self.odometry = odometry.Odometry()
self.length = length
self.radius = radius
print("created pen holder")
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
self.pdTheta = pd_controller2.PDController(500,
100,
-200,
200,
is_angle=True)
self.pdDistance = pd_controller2.PDController(1000,
0,
-300,
300,
is_angle=False)
self.pidTheta = pid_controller.PIDController(300,
0,
0, [-10, 10], [-300, 300],
is_angle=True)
self.pidDistance = pid_controller.PIDController(1000,
0,
0, [0, 0], [-300, 300],
is_angle=False)
self.result = np.empty((0, 5))
self.base_speed = 300
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.arm = factory.create_kuka_lbr4p()
self.virtual_create = factory.create_virtual_create()
# self.virtual_create = factory.create_virtual_create("192.168.1.XXX")
self.odometry = odometry.Odometry()
self._map = lab8_map.Map("lab8_map.json")
# self.map = lab10_map.Map("configuration_space.png")
self.map = lab10_map.Map("config2.png")
self.rrt = rrt.RRT(self.map)
self.min_dist_to_localize = 0.1
self.min_theta_to_localize = math.pi / 4
# TODO identify good PID controller gains
self.pidTheta = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
# self.pidTheta = pid_controller.PIDController(200, 0, 100, [-10, 10], [-50, 50], is_angle=True)
# self.pidDistance = pid_controller.PIDController(300, 0, 20, [0, 0], [-200, 200], is_angle=False)
# TODO identify good particle filter parameters
self.pf = particle_filter.ParticleFilter(self._map, 5000, 0.06, 0.15,
0.2)
_ = self.create.sim_get_position()
self.my_arm_robot = MyArmRobot(factory.create_kuka_lbr4p(), self.time)
self.joint_angles = np.zeros(7)
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
# Sensors
self.odometry = odometry.Odometry()
self.tracker = factory.create_tracker(1,
sd_x=0.01,
sd_y=0.01,
sd_theta=0.01,
rate=10)
# Controllers for Odometry
self.pidTheta = pid_controller_soln.PIDController(300,
5,
50, [-10, 10],
[-180, 180],
is_angle=True)
self.pidDistance = pid_controller_soln.PIDController(1000,
0,
50, [0, 0],
[-500, 500],
is_angle=False)
# Pen Holder - raises and lowers pen
self.penholder = factory.create_pen_holder()
# Image to be drawn
self.img = lab11_image.VectorImage("lab11_img1.yaml")
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
self.penholder = factory.create_pen_holder()
self.base_speed = 100
def __init__(self, motors, _evitement):
#136mm * PI = 1600 step
self.mm_to_step = 80.0 / 79.0 * (1600 / (math.pi * 136)) / 2.0
self.step_to_mm = 1.0 / self.mm_to_step
self.motors = motors
self.D = 306.2
self.rayon = self.D / 2.0
self.encoder_tick_to_mm = 0.07088165616
self.rayon_encoder = 136.6092
self.odo = odometry.Odometry(self.encoder_tick_to_mm,
self.rayon_encoder)
self.odo.position = (0, 0)
self.odo.angle = math.pi / 2
self.approach_speed = 90
self.evitement = _evitement
self.blockage_detection = Blockage(self.odo)
#wait event params
self.precision = 1
self.blockage = Blockage.ANY
self.obs_detection = Obstacle.NONE
self.boost_en = True
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.virtual_create = factory.create_virtual_create()
self.odometry = odometry.Odometry()
# For Simulation Testing
# self.p_map = lab8_map.Map("final_map.json")
# self.map = lab10_map.Map("final_map_config.png")
# For Physical Testing
self.p_map = lab8_map.Map("finalproject_map3.json")
self.map = lab10_map.Map("finalproject_map3_config.png")
self.path = lab10.Run(factory, self.map)
# TODO identify good PID controller gains
self.pidTheta = pid_controller.PIDController(200, 0, 100, [-10, 10], [-50, 50], is_angle=True)
# TODO identify good particle filter parameters
self.pf = particle_filter.ParticleFilter(self.p_map, 1000, 0.06, 0.15, 0.2)
self.joint_angles = np.zeros(7)
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.pdTheta = pd_controller.PDController(500, 40, -200, 200)
self.pidTheta = pid_controller.PIDController(200,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.map = lab8_map.Map("lab8_map.json")
self.base_speed = 100
self.odometry = odometry.Odometry()
self.particle_filter = ParticleFilter(
map=self.map,
virtual_create=self.virtual_create,
num_particles=100,
distance=STRAIGHT_DISTANCE,
sigma_sensor=0.1,
sigma_theta=0.05,
sigma_distance=0.01,
)
_ = self.create.sim_get_position()
def __init__(self, create, time, sonar, servo):
self.create = create
self.time = time
self.sonar = sonar
self.servo = servo
self.odometry = odometry.Odometry()
# get path from A* path finding algorithm
self.m = np.loadtxt("map14.txt", delimiter=",")
self.start_location = (9, 1)
# Without dynamic obstacles
#self.goal_location = (3, 9)
#self.goal_location = (9, 9)
#self.goal_location = (7, 3)
# With dynamic obstacles
self.goal_location = (2, 1)
#self.goal_location = (1, 1)
self.path = aStar.a_star(self.m, self.start_location,
self.goal_location)
# robot initial orientation
self.orientation = "east"
self.theta = 0
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
# self.pidTheta = pd_controller2.PDController(500, 100, -200, 200, is_angle=True)
self.pidTheta = pid_controller.PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True)
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = odometry.Odometry()
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
self.pidTheta = pid_controller.PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True)
self.map = lab10_map.Map("lab10.png")
self.rrt = rrt.RRT(self.map)
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.odometry = odometry.Odometry()
self.base_speed = 60
# sd_{x,y,theta} and rate are only for simulation to change the noise and update rate respectively.
# They are ignored on the robot.
self.tracker = factory.create_tracker(1, sd_x=0.01, sd_y=0.01, sd_theta=0.01, rate=10)
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
self.pdTheta = pd_controller.PDController(self.theta_kp, self.theta_kd,
-200, 200)
# self.pidTheta = pid_controller.PIDController(self.theta_kp, self.theta_kd, self.theta_ki, -200, 200)
self.pidXY = pid_controller.PIDController(self.xy_kp, self.xy_kd,
self.xy_ki, -200, 200)
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
# self.pdTheta = pd_controller.PDController(500, 100, -200, 200)
self.pidTheta = pid_controller.PIDController(500, 20, 1, -100, 100,
-100, 100)
self.pidTheta_dist = pid_controller.PIDController(
500, 20, 1, -100, 100, -100, 100)
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.map = lab10_map.Map("lab10.png")
self.map.thicker_border = 10
self.odometry = odometry.Odometry()
self.pidTheta = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.penholder = factory.create_pen_holder()
self.base_speed = 400
self.pidTheta = pid_controller.PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True)
self.pidDistance = pid_controller.PIDController(1000, 0, 50, [0, 0], [-200, 200], is_angle=False)
self.odometry = odometry.Odometry()
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.odometry = odometry.Odometry()
# self.pidTheta = pd_controller2.PDController(500, 100, -200, 200, is_angle=True)
self.pidTheta = pid_controller.PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True)
# self.pidDistance = pid_controller.PIDController(1000, 0, 50, [0, 0], [-200, 200], is_angle=False)
self.pdWF = pid_controller.PIDController(200, 50, 0, [0,0], [-50, 50])
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.odometry = odometry.Odometry()
self.particle_filter = ParticleFilter()
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
# Add the IP-address of your computer here if you run on the robot
self.virtual_create = factory.create_virtual_create()
self.map = lab8_map.Map("lab8_map.json")
self.robot = MyRobot(self.create, self.time, base_speed=0.1)
self.odometry = odometry.Odometry()
def __init__(self, create, time, sonar, servo):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = create
self.time = time
self.sonar = sonar
self.servo = servo
self.odometry = odometry.Odometry()
# self.pidTheta = pd_controller2.PDController(500,4 100, -200, 200, is_angle=True)
self.pidTheta = pid_controller.PIDController(200, 25, 5, [-1, 1], [-200, 200], is_angle=True)
self.pidDistance = pid_controller.PIDController(100, 15, 5, [-10, 10], [-200, 200], is_angle=False)
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.map = lab10_map.Map("lab10.png")
self.odometry = odometry.Odometry()
self.pidTheta = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.pidDistance = pid_controller.PIDController(1000,
0,
50, [0, 0],
[-200, 200],
is_angle=False)
def __init__(self, client_id):
"""Constructor.
Args:
client_id (integer): V-REP client id.
"""
self._clientID = client_id
# query objects
rc, self._joint = vrep.simxGetObjectHandle(self._clientID, "Prismatic_joint", vrep.simx_opmode_oneshot_wait)
print(rc, self._joint)
# self.servo = Servo(number)
self.odometry = odometry.Odometry()
self.length = 0.08
self.radius = 0.1175
self.arm = self.radius+self.length
print("created pen holder")
def __init__(self, create, time, sonar, servo):
self.create = create
self.time = time
self.sonar = sonar
self.servo = servo
self.odometry = odometry.Odometry()
# get path from A* path finding algorithm
m = np.loadtxt("map14.txt", delimiter=",")
self.start_location = (9, 1)
self.goal_location = (3, 9)
#self.goal_location = (7, 3)
#self.goal_location = (1, 1)
#self.goal_location = (9, 9)
#self.goal_location = (5, 1)
self.path = aStar.a_star(m, self.start_location, self.goal_location)
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.virtual_create = factory.create_virtual_create()
self.odometry = odometry.Odometry()
self.pidTheta = pid_controller.PIDController(200,
0,
100, [-10, 10], [-50, 50],
is_angle=True)
self.map = lab8_map.Map("lab8_map.json")
self.pf = particle_filter.ParticleFilter(self.map, 1000, 0.01, 0.05,
0.1)
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.map = lab11_map.Map("finalproject_map2_config.png")
self.T = None
self.start = (180, 268)
self.end = (50, 150)
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
self.pidTheta = pid_controller.PIDController(150, 5, 50, [-10, 10], [-200, 200], is_angle=True)
self.pidDistance = pid_controller.PIDController(500, 0, 50, [0, 0], [-200, 200], is_angle=False)
self.waypoints = []
def __init__(self, factory):
"""Constructor.
Args:
factory (factory.FactoryCreate)
"""
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.servo = factory.create_servo()
self.sonar = factory.create_sonar()
self.arm = factory.create_kuka_lbr4p()
self.virtual_create = factory.create_virtual_create()
# self.virtual_create = factory.create_virtual_create("192.168.1.XXX")
self.odometry = odometry.Odometry()
self.mapJ = lab8_map.Map("lab8_map.json")
self.map = rrt_map.Map("configuration_space.png")
self.rrt = rrt.RRT(self.map)
self.path = []
self.is_localized = False
# TODO identify good PID controller gains
self.pd_controller = pd_controller.PDController(1000, 100, -75, 75)
self.pdWF = pid_controller.PIDController(200, 50, 0, [0, 0], [-50, 50])
self.pidTheta = pid_controller.PIDController(200,
0,
100, [-10, 10], [-50, 50],
is_angle=True)
self.pidTheta2 = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.pidDistance = pid_controller.PIDController(1000,
0,
50, [0, 0],
[-200, 200],
is_angle=False)
# TODO identify good particle filter parameters
self.pf = particle_filter.ParticleFilter(self.mapJ, 1000, 0.06, 0.15,
0.2)
self.joint_angles = np.zeros(7)
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
# self.pidTheta = pd_controller2.PDController(500, 100, -200, 200, is_angle=True)
self.pidTheta = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-300, 300],
is_angle=True)
self.pidDistance = pid_controller.PIDController(1000,
0,
50, [0, 0],
[-300, 300],
is_angle=False)
self.pidWallFollow = pid_controller.PIDController(1000,
0,
100, [-75, 75],
[-300, 300],
is_angle=False)
def __init__(self, factory):
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.map = lab10_map.Map("configuration_space.png")
self.RRTree = RRTree(1, self.map)
self.create = factory.create_create()
self.time = factory.create_time_helper()
self.sonar = factory.create_sonar()
self.servo = factory.create_servo()
self.odometry = odometry.Odometry()
# self.pidTheta = pd_controller2.PDController(500, 100, -200, 200, is_angle=True)
self.pidTheta = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.pidDistance = pid_controller.PIDController(1000,
0,
50, [0, 0],
[-200, 200],
is_angle=False)
