def setup_integrator(self):
self.system = mda.build_system()
self.sactrep = mda.build_system()
self.mvi = trep.MidpointVI(self.system)
# get the position of the omni in the trep frame
if self.listener.frameExists(SIMFRAME) and self.listener.frameExists(CONTFRAME):
t = self.listener.getLatestCommonTime(SIMFRAME, CONTFRAME)
try:
position, quaternion = self.listener.lookupTransform(SIMFRAME, CONTFRAME, t)
except (tf.Exception):
rospy.logerr("Could not transform from "\
"{0:s} to {1:s}".format(SIMFRAME,CONTFRAME))
return
else:
rospy.logerr("Could not find required frames "\
"for transformation from {0:s} to {1:s}".format(SIMFRAME,CONTFRAME))
return
#update position and velocity arrays
self.prevq = np.vstack((SCALE*np.array([position[0],position[1]]),self.prevq))
self.prevq = np.delete(self.prevq, -1,0)
self.prevdq = np.vstack([self.system.dq[2:4],self.prevdq])
self.prevdq = np.delete(self.prevdq, -1,0)
self.q0 = np.array([-0.1, 0.,SCALE*position[0],SCALE*position[1]])#X=[th,xc,yc]
self.dq0 = np.zeros(self.system.nQd)
x0=np.array([self.q0[0],self.q0[1],self.q0[2],self.q0[3],0.,0.,0.,0.])
[self.KStabil, self.dsys, self.xBar]=mda.build_LQR(self.mvi, self.system, x0)
self.mvi.initialize_from_state(0, self.q0, self.dq0)
self.u=self.mvi.q2[2:4]
#compute LQR control
x=np.array([self.system.q[0],self.system.q[1],self.system.q[2],self.system.q[3], \
self.system.dq[0],self.system.dq[1],self.system.dq[2],self.system.dq[3]])
xTilde = x - self.xBar # Compare to desired state
utemp = -dot(self.KStabil, xTilde) # Calculate input
utemp = fb.sat_u(utemp-self.mvi.q2[2:4])
self.LQpos=utemp+self.prevq[0]
#convert kinematic acceleration to new velocity&position
self.LQvel = utemp/DT
#self.LQpos = self.u
self.wall = self.prevq[0]
#reset score values
self.i = 0.
self.n = 0.
self.prevq = np.zeros([5,2])
self.prevdq = np.zeros([20,2])
return
def timerLQ(self,data):
if not self.running_flag:
return
#compute LQR control
x=np.array([self.system.q[0],self.system.q[1],self.system.q[2],self.system.q[3], \
self.system.dq[0],self.system.dq[1],self.system.dq[2],self.system.dq[3]])
xTilde = x - self.xBar # Compare to desired state
utemp = -dot(self.KStabil, xTilde) # Calculate input
utemp = fb.sat_u(utemp-self.mvi.q2[2:4])
self.LQpos=utemp+self.mvi.q2[2:4]
#convert kinematic acceleration to new velocity&position
veltemp = utemp/DT
self.u = self.mvi.v2
#if np.sign(self.LQvel) != np.sign(veltemp):#update wall if sac changes direction
self.wall = self.prevq[0,0]
self.LQvel = veltemp
return
[KStabilize, dsys, xBar]=mda.build_LQR(mvi, system,X0)
# Simulate the system forward
T = [mvi.t1] # List to hold time values
Q = [mvi.q1] # List to hold configuration values
#Q.append(mvi.q1[1])
X = [dsys.xk] # List to hold state values
U = [mvi.v2] # List to hold input values
u=mvi.q1[2:4]
while mvi.t1 < TF-DT:
x = dsys.xk # Grab current state
xTilde = x - xBar # Compare to desired state
utemp = -dot(KStabilize, xTilde) # Calculate input
utemp = fb.sat_u(utemp-mvi.q2[2:4])
u=utemp+mvi.q2[2:4]
dsys.step(u) # Step the system forward by one time step
T.append(mvi.t1) # Update lists
Q.append(mvi.q1)
X.append(x)
U.append(mvi.v2)
if np.abs(mvi.t1%1)<DT:
print "time = ",mvi.q1
# Visualize the system in action
trep.visual.visualize_3d([ trep.visual.VisualItem3D(system, T, Q) ])
# Plot results
f,ax = plt.subplots(2, sharex=True)
#ax[0].title("Linear Feedback Controller")
