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()