def __init__(self, name, X0, Dt, proc_cov, meas_cov, Hk=None,
num=1000, L=100):
"""
INPUTS:
name ~ string identifying filter
X0 ~ initial state
Dt ~ timestep
proc_cov ~ process covariance
meas_cov ~ measurement covariance
Hk ~ matrix for measurement model
num ~ number of particles
L ~ number of timesteps
"""
self.name = name
self.X0 = X0
self.num = num
self.dim = len(X0)
self.meas_dim = len(meas_cov)
self.Dt = Dt
self.proc_cov = proc_cov
self.meas_cov = meas_cov
self.est_cov = copy.deepcopy(proc_cov)
if Hk != None:
self.Hk = Hk
self.L = L
self.index = 0
self.t = 0
self.tvec = zeros((self.L+1, 1))
self.xfilt = zeros((self.L+1, self.dim))
self.xpred = zeros((self.L+1, self.dim))
self.covs = zeros((self.L+1, self.dim, self.dim))
self.xfilt[0] = X0
self.xpred[0] = X0
self.covs[0] = self.est_cov
self.measurements = zeros((self.L+1, self.meas_dim))
self.measurements[0] = tools.matmult(self.Hk, self.X0)
self.xkk = X0
self.Qk = self.proc_cov
self.Rk = self.meas_cov
self.Pkk = copy.deepcopy(self.est_cov)
def step_filter(self, zt, Winc=None, *args, **kwargs):
if Winc == None:
Winc = random.multivariate_normal(zeros(self.dim),
self.proc_cov)
self.index += 1
self.t += self.Dt
# prediction
xkkm1 = self.state_trans(Winc, *args, **kwargs)
Fkm1 = self.proc_linearize(xkkm1)
Pkkm1 = tools.matmult(Fkm1,self.Pkk,Fkm1.T) + self.Qk
# update
yk = zt - tools.matmult(self.Hk, xkkm1)
Sk = tools.matmult(self.Hk, Pkkm1, self.Hk.T) + self.Rk
Kk = tools.matmult(Pkkm1, self.Hk.T, linalg.inv(Sk))
self.xkk = xkkm1 + tools.matmult(Kk, yk)
self.Pkk = tools.matmult((eye(self.dim) -
tools.matmult(Kk,self.Hk)), Pkkm1)
# store data
self.xfilt[self.index] = self.xkk
self.xpred[self.index] = xkkm1
self.covs[self.index] = self.Pkk
self.tvec[self.index] = self.t
self.measurements[self.index] = zt
return
def fullcb(self, data):
rospy.logdebug("Full controller callback triggered")
op_cond = self.operating_condition
if op_cond is OperatingCondition.CALIBRATE:
self.send_initial_config()
# self.first_flag = False
return
elif op_cond is OperatingCondition.EMERGENCY:
# we need to stop robots!:
self.stop_robots()
return
elif op_cond is not OperatingCondition.RUN:
# we are not running, keep setting initializations
self.first_flag = True
self.callback_count = 0
self.ekf.index = 0
# clear trajectories:
self.mass_ref_vec.clear()
self.mass_filt_vec.clear()
return
if self.first_flag:
rospy.loginfo("Beginning Trajectory")
# publish start time:
self.time_pub.publish(data.header.stamp)
self.send_initial_config()
self.tbase = data.header.stamp
self.first_flag = False
self.callback_count = 0
# reset filter:
self.ekf.index = 0
self.ekf.xkk = self.ekf.X0
self.ekf.est_cov = copy.deepcopy(self.ekf.proc_cov)
# send reference traj U and store:
self.Uprev = self.X0[2:4]
self.Ukey = self.Uopt[0]
# publish filtered and reference:
self.publish_state_and_config(data, self.ekf.xkk, self.X0)
else:
self.callback_count += 1
zk = tools.config_to_array(self.system, data)
# get updated estimates
self.ekf.step_filter(zk, Winc=np.zeros(self.dsys.nX), u=self.Uprev)
# set index value:
if self.callback_count > len(self.Uopt)-1:
index = len(self.Uopt)-1
else:
index = self.callback_count
# publish filtered and reference:
self.publish_state_and_config(data, self.ekf.xkk, self.Xref[index])
# calculate controls:
self.Ukey = self.Uopt[index] + \
tools.matmult(self.Kstab[index], self.Xopt[index] - self.ekf.xkk)
# send controls to robot:
self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey)
# add path information:
self.add_to_path_vectors(data, self.Xref[index], self.ekf.xkk)
# store data:
self.Uprev = self.Ukey
# check for the end of the trajectory:
if self.callback_count > len(self.tvec)-1:
rospy.loginfo("Trajectory complete!")
try:
self.op_change_client(OperatingCondition(OperatingCondition.STOP))
except rospy.ServiceException, e:
rospy.loginfo("Service did not process request: %s"%str(e))
# now we can stop the robots
self.stop_robots()
def lqrcb(self, data):
rospy.logdebug("LQR control callback triggered")
op_cond = self.operating_condition
if op_cond is OperatingCondition.CALIBRATE:
self.send_initial_config()
return
elif op_cond is OperatingCondition.EMERGENCY:
# we need to stop robots!:
self.stop_robots()
return
elif op_cond is not OperatingCondition.RUN:
# we are not running, keep setting initializations
self.first_flag = True
self.wait_flag = True
self.callback_count = 0
self.ekf.index = 0
# clear trajectories:
self.mass_ref_vec.clear()
self.mass_filt_vec.clear()
# reset
if self.interactive_bool:
self.RM.reset_interps()
return
if self.first_flag:
# let's enforce a few delays here to ensure that we build up enough
# reference traj:
if self.interactive_bool and self.wait_flag:
if self.callback_count == 0:
# publish start time:
self.time_pub.publish(data.header.stamp)
self.send_initial_config()
self.tbase = data.header.stamp
elif self.callback_count > self.n_win:
self.wait_flag = False
self.callback_count += 1
return
rospy.loginfo("Beginning Trajectory")
self.first_flag = False
self.callback_count = 0
# reset filter:
self.ekf.index = 0
self.ekf.xkk = self.ekf.X0
self.ekf.est_cov = copy.deepcopy(self.ekf.proc_cov)
# get reference traj after initial dt:
r = self.RM.calc_reference_traj(self.dsys, [0, self.dt, 2*self.dt])
if r:
Xtmp,Utmp = r
else:
rospy.logwarn("waiting for more reference in first_flag of lqrcb")
self.first_flag = True
return
# send reference traj U and store:
self.convert_and_send_input(Xtmp[0][2:4], Xtmp[1][2:4])#self.Uprev, self.Ukey)
self.Uprev = Utmp[0]
self.Ukey = Utmp[1]
# publish filtered and reference:
self.publish_state_and_config(data, self.ekf.xkk, self.X0)
else:
self.callback_count += 1
zk = tools.config_to_array(self.system, data)
# get updated estimate
self.ekf.step_filter(zk, Winc=np.zeros(self.dsys.nX), u=self.Uprev)
# get reference
r = self.RM.calc_reference_traj(self.dsys, [self.callback_count*self.dt])
if r:
xref,uref = r
else:
rospy.logwarn("reference trajectory returned None for lqr current state!")
return
# publish filtered and reference:
self.publish_state_and_config(data, self.ekf.xkk, xref[0])
# calculate controls:
self.Ukey = xref[0][2:4] + tools.matmult(self.Kstab, xref[0] - self.ekf.xkk)
# send controls to robot:
self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey)
# add path information:
self.add_to_path_vectors(data, xref[0], self.ekf.xkk)
# store data:
self.Uprev = self.Ukey
# check for the end of the trajectory:
if self.callback_count >= len(self.tvec):
rospy.loginfo("Trajectory complete!")
try:
self.op_change_client(OperatingCondition(OperatingCondition.STOP))
except rospy.ServiceException, e:
rospy.loginfo("Service did not process request: %s"%str(e))
# now we can stop the robots
self.stop_robots()
def proc_linearize(self, xlin):
return (eye(self.dim) + tools.matmult(
self.Dt*self.A(xlin + 0.5*self.Dt*self.f(xlin)),
eye(self.dim) + 0.5*self.Dt*self.A(xlin)))
def state_trans(self, Winc, *args, **kwargs):
return self.xkk + self.Dt*tools.matmult(self.A, self.xkk)
def fullcb(self, data):
rospy.logdebug("Full controller callback triggered")
op_cond = self.operating_condition
if op_cond is OperatingCondition.CALIBRATE:
self.send_initial_config()
# self.first_flag = False
return
elif op_cond is OperatingCondition.EMERGENCY:
# we need to stop robots!:
self.stop_robots()
return
elif op_cond is not OperatingCondition.RUN:
# we are not running, keep setting initializations
self.first_flag = True
self.callback_count = 0
self.ekf.index = 0
# clear trajectories:
self.mass_ref_vec.clear()
self.mass_filt_vec.clear()
return
if self.first_flag:
rospy.loginfo("Beginning Trajectory")
# publish start time:
self.time_pub.publish(data.header.stamp)
self.send_initial_config()
self.tbase = data.header.stamp
self.first_flag = False
self.callback_count = 0
# reset filter:
self.ekf.index = 0
self.ekf.xkk = self.ekf.X0
self.ekf.est_cov = copy.deepcopy(self.ekf.proc_cov)
# send reference traj U and store:
self.Uprev = self.X0[2:4]
self.Ukey = self.Uopt[0]
# publish filtered and reference:
self.publish_state_and_config(data, self.ekf.xkk, self.X0)
else:
self.callback_count += 1
zk = tools.config_to_array(self.system, data)
# get updated estimates
self.ekf.step_filter(zk, Winc=np.zeros(self.dsys.nX), u=self.Uprev)
# set index value:
if self.callback_count > len(self.Uopt) - 1:
index = len(self.Uopt) - 1
else:
index = self.callback_count
# publish filtered and reference:
self.publish_state_and_config(data, self.ekf.xkk, self.Xref[index])
# calculate controls:
self.Ukey = self.Uopt[index] + \
tools.matmult(self.Kstab[index], self.Xopt[index] - self.ekf.xkk)
# send controls to robot:
self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey)
# add path information:
self.add_to_path_vectors(data, self.Xref[index], self.ekf.xkk)
# store data:
self.Uprev = self.Ukey
# check for the end of the trajectory:
if self.callback_count > len(self.tvec) - 1:
rospy.loginfo("Trajectory complete!")
try:
self.op_change_client(
OperatingCondition(OperatingCondition.STOP))
except rospy.ServiceException, e:
rospy.loginfo("Service did not process request: %s" %
str(e))
# now we can stop the robots
self.stop_robots()
def lqrcb(self, data):
rospy.logdebug("LQR control callback triggered")
op_cond = self.operating_condition
if op_cond is OperatingCondition.CALIBRATE:
self.send_initial_config()
return
elif op_cond is OperatingCondition.EMERGENCY:
# we need to stop robots!:
self.stop_robots()
return
elif op_cond is not OperatingCondition.RUN:
# we are not running, keep setting initializations
self.first_flag = True
self.wait_flag = True
self.callback_count = 0
self.ekf.index = 0
# clear trajectories:
self.mass_ref_vec.clear()
self.mass_filt_vec.clear()
# reset
if self.interactive_bool:
self.RM.reset_interps()
return
if self.first_flag:
# let's enforce a few delays here to ensure that we build up enough
# reference traj:
if self.interactive_bool and self.wait_flag:
if self.callback_count == 0:
# publish start time:
self.time_pub.publish(data.header.stamp)
self.send_initial_config()
self.tbase = data.header.stamp
elif self.callback_count > self.n_win:
self.wait_flag = False
self.callback_count += 1
return
rospy.loginfo("Beginning Trajectory")
self.first_flag = False
self.callback_count = 0
# reset filter:
self.ekf.index = 0
self.ekf.xkk = self.ekf.X0
self.ekf.est_cov = copy.deepcopy(self.ekf.proc_cov)
# get reference traj after initial dt:
r = self.RM.calc_reference_traj(self.dsys,
[0, self.dt, 2 * self.dt])
if r:
Xtmp, Utmp = r
else:
rospy.logwarn(
"waiting for more reference in first_flag of lqrcb")
self.first_flag = True
return
# send reference traj U and store:
self.convert_and_send_input(Xtmp[0][2:4],
Xtmp[1][2:4]) #self.Uprev, self.Ukey)
self.Uprev = Utmp[0]
self.Ukey = Utmp[1]
# publish filtered and reference:
self.publish_state_and_config(data, self.ekf.xkk, self.X0)
else:
self.callback_count += 1
zk = tools.config_to_array(self.system, data)
# get updated estimate
self.ekf.step_filter(zk, Winc=np.zeros(self.dsys.nX), u=self.Uprev)
# get reference
r = self.RM.calc_reference_traj(self.dsys,
[self.callback_count * self.dt])
if r:
xref, uref = r
else:
rospy.logwarn(
"reference trajectory returned None for lqr current state!"
)
return
# publish filtered and reference:
self.publish_state_and_config(data, self.ekf.xkk, xref[0])
# calculate controls:
self.Ukey = xref[0][2:4] + tools.matmult(self.Kstab,
xref[0] - self.ekf.xkk)
# send controls to robot:
self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey)
# add path information:
self.add_to_path_vectors(data, xref[0], self.ekf.xkk)
# store data:
self.Uprev = self.Ukey
# check for the end of the trajectory:
if self.callback_count >= len(self.tvec):
rospy.loginfo("Trajectory complete!")
try:
self.op_change_client(
OperatingCondition(OperatingCondition.STOP))
except rospy.ServiceException, e:
rospy.loginfo("Service did not process request: %s" %
str(e))
# now we can stop the robots
self.stop_robots()
