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):
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)
self.pidWallFollow = pid_controller.PIDController(1000, 0, 100, [-75, 75], [-200, 200], is_angle=False)
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):
"""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, 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, 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()
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.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 run_pid_test(self,
constants,
initial_X,
goal,
show_graph=True,
rpm=True,
iterations=4000):
"""This method runs a PID controller test on the system. Constants should be
an array with P, I, D constants in that order. deg refers to whether or not
you want to convert to degrees on the graph, it will be in radians if set to
false. Iterations means how many loops to run. For a dt = 0.005, 4000 iterations
is 20 seconds. A different dt will create a different length test for a given
number of iterations. Smaller dt run more precisely, but consume more resources.
Larger dt run with less resources, but are less accurate."""
self.X = initial_X
controller = pid_controller.PIDController(constants[0], constants[1],
constants[2], self.dt,
goal[0, 0])
t = []
omega = []
error = []
u = []
for i in range(iterations):
if rpm:
U = [controller.Update(self.X[0, 0] * 30.0 / math.pi)]
else:
U = [controller.Update(self.X[0, 0])]
U = numpy.clip(U, self.minU, self.maxU)
if rpm:
omega.append(self.X[0, 0] * 30.0 / math.pi)
error.append((goal[0, 0] - self.X[0, 0]) * 30.0 / math.pi)
else:
omega.append(self.X[0, 0])
error.append(goal[0, 0] - self.X[0, 0])
self.update(U)
t.append(i * self.dt)
u.append(U[0, 0])
if show_graph:
fig_size = [12, 9]
pyplot.rcParams["figure.figsize"] = fig_size
pyplot.subplot(2, 1, 1)
pyplot.plot(t, omega, label='angular velocity')
print("Final State: ")
if rpm:
print(self.X * 30.0 / math.pi)
else:
print(self.X)
print("\n")
#pyplot.plot(t, error, label='error')
pyplot.legend()
pyplot.subplot(2, 1, 2)
pyplot.plot(t, u, label='voltage')
pyplot.legend()
pyplot.show()
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, name):
self._action_name = name
self._as = actionlib.SimpleActionServer('/j2s7s300/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start = False)
self._feedback_sub = rospy.Subscriber('/j2s7s300_driver/trajectory_controller/state', control_msgs.msg.FollowJointTrajectoryFeedback, self.feedback_cb)
self._controller = pid_controller.PIDController()
self._vel_pub = rospy.Publisher('/j2s7s300_driver/in/joint_velocity',
kinova_msgs.msg.JointVelocity,
queue_size=1)
self._as.start()
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):
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.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.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.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)
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)
self.pidDistance = pid_controller.PIDController(1000, 0, 50, [0, 0], [-200, 200], is_angle=False)
self.tracker = factory.create_tracker(1, sd_x=0.01, sd_y=0.01, sd_theta=0.01, rate=10)
self.nodes = []
self.alpha = 0.9999
self.beta = 1 - self.alpha
# read file
self.img = lab11_image.VectorImage("lab11_img1.yaml")
self.penholder = factory.create_pen_holder()
self.offset = 0.18
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.pd_controller = pd_controller.PDController(1000, 100, -75, 75)
# self.pd_controller = pid_controller.PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True)
# 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,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
# self.pidWallFollowing = pid_controller.PIDController(300, 0, 100, [-75, 75], [-300, 300], is_angle=False)
self.pidWallFollowing = pid_controller.PIDController(
200, 50, 0, [0, 0], [-50, 50])
self.result = np.empty((0, 3))
self.base_speed = 100
self.current = ''
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.pidDistance = pid_controller.PIDController(1000,
0,
50, [0, 0],
[-200, 200],
is_angle=False)
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, create, time, sonar, servo):
self.create = create
self.time = time
self.sonar = sonar
self.servo = servo
self.odometry = odometry.Odometry()
self.pidWall = pid_controller.PIDController(1000,
0,
50, [0, 0], [-150, 150],
is_angle=False)
self.pidTheta = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-150, 150],
is_angle=True)
self.pidDistance = pid_controller.PIDController(1000,
0,
50, [0, 0],
[-150, 150],
is_angle=False)
# get path from A* path finding algorithm
m = np.loadtxt("map14.txt", delimiter=",")
self.path = aStar.a_star(m, (9, 1), (7, 5))
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.pidTheta = pid_controller.PIDController(300,
5,
50, [-10, 10],
[-200, 200],
is_angle=True)
self.pidDistance = pid_controller.PIDController(300,
0,
20, [0, 0],
[-200, 200],
is_angle=False)
# self.pidTheta = pid_controller.PIDController(200, 0, 100, [-10, 10], [-50, 50], is_angle=True)
self.rrt = rrt.RRT(self.map)
self.joint_angles = np.zeros(7)
# the position that the robot returns after localizing
self.localized_x = 0
self.localized_y = 0
self.localized_theta = math.pi / 2
def __init__(self):
# Create a publisher for acceleration data
self.pub1 = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
# set the rate of ros loop
self.rate = rospy.Rate(10) # 10hz
# Create variable for use
self.vel_twist = TwistStamped()
#Creates old time for use with the PIDs
self.old_time = rospy.get_time()
# Create object of PID Controller with tunning arguments
self.pid_methods = pid_controller.PIDController(KP=0.5, KI=0.0, KD=0.0) # PID contants P, I, D
# runs the loop function
self.loop_roomba_location() # Runs roomba subscriber
def run_profiled_test(self,
profile,
constants,
initial_X,
show_graph=True,
iterations=4000):
self.X = initial_X
trap_profile = trapezoidal_profile.TrapezoidalMotionProfile(
profile[0], profile[1], profile[2])
controller = pid_controller.PIDController(
constants[0], constants[1], constants[2], self.dt,
[trap_profile.distance(0),
trap_profile.velocity(0)])
t = []
theta = []
omega = []
u = []
profile_vel = []
for i in range(iterations):
controller.setSetpoint(trap_profile.distance(i * self.dt))
U = [
controller.Update(self.X[0, 0]) +
(12.0 / profile[1]) * trap_profile.velocity(i * self.dt)
]
U = numpy.clip(U, self.minU, self.maxU)
theta.append(self.X[0, 0])
omega.append(self.X[1, 0])
self.update(U)
t.append(i * self.dt)
u.append(U[0, 0])
profile_vel.append(
(12.0 / profile[1]) * trap_profile.velocity(i * self.dt))
if show_graph:
fig_size = [12, 9]
pyplot.rcParams["figure.figsize"] = fig_size
pyplot.subplot(2, 1, 1)
pyplot.plot(t, theta, label='angle')
pyplot.plot(t, omega, label='angular velocity')
pyplot.legend()
pyplot.subplot(2, 1, 2)
pyplot.plot(t, u, label='voltage')
pyplot.plot(t, profile_vel, label='profile velocity')
pyplot.legend()
pyplot.show()
def createController(self):
"""
Creates a new gain scheduling controller based on the current
gain settings.
"""
self.feedForward = velocity_feedforward.VelocityFeedForward()
self.feedForward.coeff = self.ffcoeff
self.controller = pid_controller.PIDController(self.pgain, self.igain,
self.dgain,
self.outputMin,
self.outputMax,
self.feedForward.func)
self.pgainScheduler = GainScheduler(
self.pgainSchedMin * self.pgain,
self.pgain,
self.pgainSchedWid,
)
def run_pid_test(self, constants, initial_X, goal, reality = False, iterations = 4000):
if reality:
dynamics = elevator_dynamics()
self.controller.X = initial_X / 39.3701
controller = pid_controller.PIDController(constants[0], constants[1], constants[2], self.dt, goal[0,0])
t = []
y = []
v = []
e = []
u = []
for i in range(iterations):
U = [controller.Update(self.controller.X[0,0] * 39.3701)]
U = np.clip(U, self.minU, self.maxU)
y.append(self.controller.X[0,0]* 39.3701)
v.append(self.controller.X[1,0]* 39.3701)
e.append((goal[0,0]-self.controller.X[0,0])* 39.3701)
if reality:
self.controller.update(U, dynamics.iterate(i * self.dt, self.controller.X, U))
else:
self.controller.update(U)
t.append(i * self.dt)
u.append(U[0,0])
fig_size = [12, 9]
pyplot.rcParams["figure.figsize"] = fig_size
pyplot.subplot(2, 1, 1)
pyplot.plot(t, y, label='height')
pyplot.plot(t, v, label= 'velocity')
print("Final State: ")
print(self.controller.X * 39.3701)
print("\n")
#pyplot.plot(t, error, label='error')
pyplot.legend()
pyplot.subplot(2, 1, 2)
pyplot.plot(t, u, label='voltage')
pyplot.legend()
pyplot.show()
def __init__(self, name):
rospy.loginfo(
"Joint Trajectory initialize start with name: {}*******************************************"
.format(name))
self._action_name = name
self._as = actionlib.SimpleActionServer(
"/m1n6s300/follow_joint_trajectory",
control_msgs.msg.FollowJointTrajectoryAction,
execute_cb=self.execute_cb,
auto_start=False)
rospy.loginfo("/m1ns6s300/follow_joint_trajectory is up")
self._feedback_sub = rospy.Subscriber(
'/m1ns6s300_driver/trajectory_controller/state',
control_msgs.msg.FollowJointTrajectoryFeedback, self.feedback_cb)
self._controller = pid_controller.PIDController()
self._vel_pub = rospy.Publisher('/m1n6s300_driver/in/joint_velocity',
kinova_msgs.msg.JointVelocity,
queue_size=1)
self._as.start()
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 = lab9_map.Map("finalproject_map2.json")
self.pf = particle_filter.ParticleFilter(self.map, 1000, 0.01, 0.05,
0.1)
self.arm = factory.create_kuka_lbr4p()
