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)
