def control_loop(self,joints,ts): print "running control loop with new trajectory" F = Spline2D(ts, joints) t_start = time() duration = ts[-1] while True: if rospy.is_shutdown(): return if self.stop_requested: self.ctrl_loop_running = False return t_elapsed = time() - t_start if t_elapsed > duration+1: return pos_cur = self.brett.base.get_pose() if t_elapsed > duration: pos_targ = joints[-1] else: pos_targ = F(t_elapsed, nu = 0) pos_targ[2] = ku.closer_ang(pos_targ[2], pos_cur[2]) twist = (pos_targ - pos_cur) twist *= .5 #np.linalg.norm(np.r_[.5,.5,1] / twist, np.inf) a = pos_cur[2] twist[0:2] = np.dot( np.array([[np.cos(a), np.sin(a)], [-np.sin(a), np.cos(a)]]) , twist[0:2]) self.brett.base.set_twist(twist) sleep(.01)
def control_loop(self,joints,ts): print "running control loop with new trajectory" F = Spline2D(ts, joints) t_start = time() duration = ts[-1] prev_err = None prev_time = None kp = 1 kd = .1 use_relative = False frame_id = self.msg.header.frame_id if "base" in frame_id: use_relative = True pos_start = self.brett.base.get_pose("odom_combined") elif "odom_combined" in frame_id or "map" in frame_id: pass else: raise Exception("invalid frame %s for base traj"%frame_id) while True: if rospy.is_shutdown(): return if self.stop_requested: self.ctrl_loop_running = False rospy.loginfo("stop requested--leaving control loop") return t_elapsed = time() - t_start if t_elapsed > duration+5: rospy.loginfo("time elapsed (+1sec)--leaving control loop") return else: if use_relative: # invert transform from orig position pos_cur = self.brett.base.get_pose("odom_combined") pos_cur -= pos_start a = pos_start[2] pos_cur[:2] = np.array([[cos(a), sin(a)],[-sin(a), cos(a)]]).dot(pos_cur[:2]) else: pos_cur = self.brett.base.get_pose("odom_combined") if t_elapsed > duration: pos_targ = joints[-1] else: pos_targ = F(t_elapsed, nu = 0) pos_targ[2] = ku.closer_ang(pos_targ[2], pos_cur[2]) err = (pos_targ - pos_cur) twist = kp*err if prev_err is not None: twist += kd*(err - prev_err)/(t_elapsed - prev_time) prev_err = err prev_time = t_elapsed a = pos_cur[2] twist[0:2] = np.dot( np.array([[np.cos(a), np.sin(a)], [-np.sin(a), np.cos(a)]]) , twist[0:2]) self.brett.base.set_twist(twist) pos_prev = pos_cur sleep(.01)