def openloopcb(self, data):
rospy.logdebug("openloop 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.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 openloopcb")
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 = 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)
# send old value to robot:
self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey)
# publish filtered and 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 openloop initial condition!")
return
self.publish_state_and_config(data, self.ekf.xkk, xref[0])
# get prediction of where we will be in +dt seconds
self.dsyssim.set(self.ekf.xkk, self.Ukey, 0)
Xstart = self.dsyssim.f()
# get reference traj
ttmp = self.twin + (self.callback_count + 1)*self.dt
r = self.RM.calc_reference_traj(self.dsys, ttmp)
if r:
Xref, Uref = r
else:
rospy.logwarn("reference trajectory returned None for openloop traj !")
return
# get initial guess
X0, U0 = op.calc_initial_guess(self.dsys, Xstart, Xref, Uref)
# add path information:
self.add_to_path_vectors(data, Xref[0], Xstart)
# store data:
self.Uprev = self.Ukey
self.Ukey = U0[0]
# 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 openloopcb(self, data):
rospy.logdebug("openloop 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.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 openloopcb")
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 = 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)
# send old value to robot:
self.convert_and_send_input(self.ekf.xkk[2:4], self.Ukey)
# publish filtered and 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 openloop initial condition!"
)
return
self.publish_state_and_config(data, self.ekf.xkk, xref[0])
# get prediction of where we will be in +dt seconds
self.dsyssim.set(self.ekf.xkk, self.Ukey, 0)
Xstart = self.dsyssim.f()
# get reference traj
ttmp = self.twin + (self.callback_count + 1) * self.dt
r = self.RM.calc_reference_traj(self.dsys, ttmp)
if r:
Xref, Uref = r
else:
rospy.logwarn(
"reference trajectory returned None for openloop traj !")
return
# get initial guess
X0, U0 = op.calc_initial_guess(self.dsys, Xstart, Xref, Uref)
# add path information:
self.add_to_path_vectors(data, Xref[0], Xstart)
# store data:
self.Uprev = self.Ukey
self.Ukey = U0[0]
# 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 feedforwardcb(self, data):
rospy.logdebug("Feedforward 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]
# 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 feedforwardcb(self, data):
rospy.logdebug("Feedforward 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]
# 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()
