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, 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()
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.odometry = Odometry()
self.pidTheta = PIDController(300,
5,
50, [-10, 10], [-200, 200],
is_angle=True)
# constants
self.base_speed = 100
self.variance_sensor = 0.1
self.variance_distance = 0.01
self.variance_direction = 0.05
self.world_width = 3.0 # the x
self.world_height = 3.0 # the y
self.filter = ParticleFilter(self.virtual_create,
self.variance_sensor,
self.variance_distance,
self.variance_direction,
num_particles=100,
world_width=self.world_width,
world_height=self.world_height)
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, 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):
"""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):
"""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()
self.travel_dist = 0.4
self.current_dist_to_wall = 0
self.base_speed = 100
self.min_dist_to_wall = self.travel_dist + 0.2
self.min_dist_to_localize = 0.45
self.min_theta_to_localize = math.pi / 4
_ = self.create.sim_get_position()
def __init__(self,
xpos,
ypos,
num_particles=3000,
sensor_sigma=0.1,
the_map=None,
cartesian_sigma=0.05,
theta_sigma=np.pi / 48.):
self.num_particles = num_particles
self.drawn_particles = 500
self.sensor_sigma = sensor_sigma
if the_map is None:
import lab8_map
the_map = lab8_map.Map("lab8_map.json")
self.map = the_map
self.cartesian_sigma = cartesian_sigma
self.theta_sigma = theta_sigma
self.xpos = xpos
self.ypos = ypos
self.particles = (np.random.uniform(
low=self.num_particles * [xpos - .5, ypos - .5, 0],
high=self.num_particles * [xpos + .5, 3, 2 * np.pi]).reshape(
self.num_particles, 3))
import numpy as np
import matplotlib.pyplot as plt
import lab8_map
from particle_filter import ParticleFilter
# data = np.random.normal(0.5, 0.1, 100)
# plt.hist(data, 20)
# plt.show()
map = lab8_map.Map("lab8_map.json")
particle_filter = ParticleFilter(map, 1)