def setup_full_controller(self):
# create mvi and dsys:
mvi = sd.trep.MidpointVI(self.system)
dsys = op.discopt.DSystem(mvi, self.tvec)
# get reference and initial guess:
X0,U0 = op.calc_initial_guess(dsys, self.X0, self.Xref, self.Uref)
cost = op.discopt.DCost(self.Xref, self.Uref, self.Qcost, self.Rcost)
# build optimizer:
optimizer = op.discopt.DOptimizer(dsys, cost)
optimizer.optimize_ic = False
optimizer.descent_tolerance = 1e-6
optimizer.first_method_iterations = 2
# optimize:
finished, self.Xopt, self.Uopt = optimizer.optimize(X0, U0)
Qk = lambda k: self.Qcost
Rk = lambda k: self.Rcost
self.Kstab = dsys.calc_feedback_controller(self.Xopt, self.Uopt, Qk, Rk)
if not finished:
rospy.logwarn("Could not complete full optimization!")
return
def setup_full_controller(self):
# create mvi and dsys:
mvi = sd.trep.MidpointVI(self.system)
dsys = op.discopt.DSystem(mvi, self.tvec)
# get reference and initial guess:
X0, U0 = op.calc_initial_guess(dsys, self.X0, self.Xref, self.Uref)
cost = op.discopt.DCost(self.Xref, self.Uref, self.Qcost, self.Rcost)
# build optimizer:
optimizer = op.discopt.DOptimizer(dsys, cost)
optimizer.optimize_ic = False
optimizer.descent_tolerance = 1e-6
optimizer.first_method_iterations = 2
# optimize:
finished, self.Xopt, self.Uopt = optimizer.optimize(X0, U0)
Qk = lambda k: self.Qcost
Rk = lambda k: self.Rcost
self.Kstab = dsys.calc_feedback_controller(self.Xopt, self.Uopt, Qk,
Rk)
if not finished:
rospy.logwarn("Could not complete full optimization!")
return
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()
