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
import max_demon as mda from max_demon.constants import * import max_demon.force_fb as fb from max_demon import rvizmarks X0 = np.array([0.15,0.15,0.,-0.5,-0.3,0.,0.,10.0])# th,phi,x,y,dth,dphi,dx,dy t0 = 0.0 # Initial time system = mda.build_system() # Create and initialize the variational integrator mvi = trep.MidpointVI(system) mvi.initialize_from_configs(t0, X0[0:4], t0+DT, X0[0:4]) #mvi.initialize_from_state(t0,X0[0:4],X0[4:6]) [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])