def getVelocityIntervalWithoutForceField(self, env, W): self.topp = TOPPInterface(W, env, zeroForce=True) if self.topp.ReparameterizeTrajectory(): return self.topp.traj0 else: #### without a force field any path should be executable #### at near zero speed => catch it if not print "WARNING1: without force field, TOPP couldn't find a valid \ velocity profile. Path not continuous or system not STLC" print "discrtimestep=", self.topp.discrtimestep self.topp.SaveToFile('clc2') CP = self.topp.getCriticalPoint() traj = self.topp.traj0 q = np.array( [traj.Eval(t) for t in np.linspace(0, traj.duration, 1e5)]).T plot(q[0, :], q[1, :], '-r', linewidth=2) plot(W[0, :], W[1, :], 'ok', markersize=2) plot(W[0, CP], W[1, CP], 'og', markersize=10) plt.show() sys.exit(1)
def getCriticalPointFromWaypoints(self, env, W, oldNc=0): self.topp = TOPPInterface(W, env) Nc = self.topp.getCriticalPoint() - 1 if Nc < 0: print "return oldNc=", oldNc Nc = oldNc return Nc
def PlotParametrization(self, env): self.topp = TOPPInterface(W, env) if self.topp.ReparameterizeTrajectory(): self.topp.PlotTrajectory(env) print self.topp else: print "Trajectory has no ReParameterization"
def getTOPPTrajectoryWithoutForceField(self, env, W): self.topp = TOPPInterface(W, env, zeroForce=True) if self.topp.ReparameterizeTrajectory(): return self.topp else: self.info() print "WARNING2: without force field, TOPP couldn't find a valid \ velocity profile. Path not continuous or system not STLC" print W.shape print W sys.exit(1)
def getCriticalPointFromWaypoints(self, env, W, oldNc = 0): self.topp = TOPPInterface(W, env) Nc = self.topp.getCriticalPoint()-1 if Nc < 0: print "return oldNc=",oldNc Nc = oldNc return Nc
def getVelocityIntervalWithoutForceField(self, env, W): self.topp = TOPPInterface(W, env, zeroForce=True) if self.topp.ReparameterizeTrajectory(): return self.topp.traj0 else: #### without a force field any path should be executable #### at near zero speed => catch it if not print "WARNING1: without force field, TOPP couldn't find a valid \ velocity profile. Path not continuous or system not STLC" print "discrtimestep=",self.topp.discrtimestep self.topp.SaveToFile('clc2') CP = self.topp.getCriticalPoint() traj = self.topp.traj0 q = np.array([traj.Eval(t) for t in np.linspace(0,traj.duration,1e5)]).T plot(q[0,:],q[1,:],'-r',linewidth=2) plot(W[0,:],W[1,:],'ok',markersize=2) plot(W[0,CP],W[1,CP],'og',markersize=10) plt.show() sys.exit(1)
class Trajectory(): __metaclass__ = abc.ABCMeta DEBUG = 0 DISCRETIZATION_TIME_STEP = 0.01 SMOOTH_CONSTANT = 0 ##TODO: do not change to >0 => problems with PPoly POLYNOMIAL_DEGREE = 1 MIN_NUMBER_WAYPOINTS = 5 rave_traj = [] traj = [] waypoints = [] handle = [] #env_ptr = [] ##drawing parameters ptsize = 0.03 critical_pt_size = 0.08 show_tangent_vector = False show_orientation_vector = False lw_path = 10 lw_tangent = 3 lw_orientation = 3 FONT_SIZE = 20 dVECTOR_LENGTH = 0.4 trajectory_orientation_color = np.array((0.9,0.9,0.9,0.3)) trajectory_tangent_color = np.array((0.9,0.2,0.9,0.3)) trajectory_color_deformation = np.array((0.9,0.2,0.9,0.9)) tangent_zoffset = -0.02 trajectory_color_feasible = np.array((0.2,0.9,0.2,0.9)) trajectory_color_infeasible = np.array((0.9,0.2,0.2,0.9)) trajectory_color = np.array((0.9,0.2,0.2,0.9)) critical_pt_color = np.array((0.9,0.2,0.2,0.9)) bspline = [] Ndim = 0 ### waypoints: real matrix of dimensionality Ndim x Nwaypoints def __init__(self, waypoints_in): self.new_from_waypoints(waypoints_in) def new_from_waypoints(self, waypoints_in): self.waypoints = self.prettify(waypoints_in) self.Ndim = self.waypoints.shape[0] self.Nwaypoints = self.waypoints.shape[1] self.bspline = self.computeSplineFromWaypoints(self.waypoints) [self.waypoints,dW,ddW] = self.get_waypoints_second_order() def save(self, filename): np.save(filename, self.waypoints) def load(self, filename): W = np.load(filename+'.npy') return W def evaluate_at(self, t, der=1): f = np.zeros((self.Ndim)) df = np.zeros((self.Ndim)) if der>1: ddf = np.zeros((self.Ndim)) for i in range(0,self.Ndim): f[i] = splev(t,self.bspline[i]) df[i] = splev(t,self.bspline[i],der=1) if der>1: ddf[i] = splev(t,self.bspline[i],der=2) df[2] = 0 if der>1: ddf[2] = 0 return [f,df,ddf] else: return [f,df] def prettify(self, W): if W.ndim <= 1: print "cannot create trajectory with only one waypoint" sys.exit(1) W = self.removeDuplicateWaypoints(W) Nwaypoints = W.shape[1] if Nwaypoints <= 3: W = self.addMinimalWaypoints(W) Nwaypoints = W.shape[1] return W def removeDuplicateWaypoints(self, W): ACCURACY_DUPLICATE = self.DISCRETIZATION_TIME_STEP/1e10 Nwaypoints = W.shape[1] if Nwaypoints == 1: print "cannot create trajectory with only one waypoint" sys.exit(1) if Nwaypoints == 2: if np.linalg.norm(W[:,0]-W[:,1])<ACCURACY_DUPLICATE: print "only two identical waypoints. abort" sys.exit(1) return W assert(Nwaypoints>2) if np.linalg.norm(W[:,0]-W[:,1])<ACCURACY_DUPLICATE: print "deleting waypoint" W = np.delete(W,0,axis=1) if np.linalg.norm(W[:,-2]-W[:,-1])<ACCURACY_DUPLICATE: print "deleting waypoint" W = np.delete(W,-1,axis=1) return W def addMinimalWaypoints(self, W): Nwaypoints = W.shape[1] if Nwaypoints >= self.MIN_NUMBER_WAYPOINTS: return W Wtmp = W for i in range(0,Nwaypoints-1): print "adding waypoint" Wnew = W[:,i]+0.5*(W[:,i+1]-W[:,i]) Wtmp = np.insert(Wtmp, 2*i+1, values=Wnew, axis=1) W = Wtmp return self.addMinimalWaypoints(W) ### SYSTEM DYNAMICS def getControlMatrix(self, W): import parameters_dynamical_system as params #AM = 1 amin = params.amin amax = params.amax [Ndim,Nwaypoints] = self.getWaypointDim(W) R = params.ControlPerWaypoint(W, Ndim, Nwaypoints) return [R,amin,amax] def info(self): print "#### TRAJECTORY CLASS ######" print "LENGTH: ", self.get_length() print "DISCRETIZATION:",self.DISCRETIZATION_TIME_STEP print "WAYPOINTS: ", self.waypoints.shape[1] [f0,df0] = np.around(self.evaluate_at(0),2) [f1,df1] = np.around(self.evaluate_at(1),2) print "START : ", f0, " dir: ", df0 print "GOAL : ", f1, " dir: ", df1 print "WAYPOINTS : " print " [ T ] [ WAYPOINT ]" for tt in np.linspace(0,1,10): [f,df]=self.evaluate_at(tt) print " [ ",np.around(tt,1)," ] ",np.around(f,decimals=2) def plot_reachableset(self, env): thetavec = [-pi/2,-pi/4,0,pi/4,pi/2] thetavec = [0] for theta in thetavec: ##sailboat force = np.array((2.5,-3.5,0,5.0)) dp = np.array((1,0.0,0,0.2)) ##car ds= 0.01 ### extra large dp p =np.array( [-2.608897298112661, -0.2047366548132623, 0.1000000008651387, -2.987014267911084] ) dp =np.array( [-16.487757154509474, -4.837209926510647, 0.0, 4.899656054155726] ) force =np.array( [0.0, 3.0, 0.0, -1.4821147977289577] ) speed= 3.86342505425 ### no force, no dp p =np.array( [-2.644855145592143, 1.1657167012260294, 0.10000000000235312, -3.7242126663188597] ) dp =np.array( [-1.1866896124736256e-20, 1.910882585951823e-18, 1.4346537045461183e-27, -9.702129945467387e-19] ) force =np.array( [0.0, 0.0, 0.0, 0.0] ) speed= 0.341259539422 ### no force, no dp p =np.array( [-2.644855145592143, 1.1657167012260294, 0.10000000000235312, -3.7242126663188597] ) dp =np.array( [0.0, 0.0, 0.0, 0.0] ) force =np.array( [0.0, 0.0, 0.0, 0.0] ) speed= 0.0 ### close to large dp p =np.array( [-4.113833032000303, -0.0026206376782298216, 0.0999999999999477, -3.139440349128482] ) dp =np.array( [-982166.3702744454, -1372.8293479195447, -8.057068259496254e-08, 1124.7453741523088] ) force =np.array( [0.0, 0.0, 0.0, 0.0] ) speed= 1.07398453113 p = np.array( [ -5.05e+00, -4.08e-03, 1.00e-01, -3.14e+00] ) dp = np.array([ -2.97e+08, -4.32e+05, 5.76e-06, 3.48e+05] ) F = np.array( [ 0. , 0. , 0. , 0.]) speed = 1.07398453113 #ds 1.19162456455 #dt 2.19793777714e-11 p =np.array( [-2.5974191119628514, -0.0004997370983040311, 0.0999999999999989, -3.141179458965877] ) dp =np.array( [-1.2469468647720259, -0.002999767139199421, 0.0, 0.0017880891982438346] ) force =np.array( [0.0, 0.0, 0.0, 0.0] ) speed= 1.51000061035 #ds: 0.00305489294111 ### nice vis p = np.array((0,1e-4,0,0)) force = np.array((0.5,-3.5,0,1.0)) dp = np.array((1,0.1,0,0.2)) speed = 0.2 ##### error ###p =np.array( [-6.012902723369949, -0.005123021667037449, 0.1, -3.1373188699666956] ) ###force =np.array( [0.0, 0.0, 0.0, 0.0] ) ###dp =np.array( [-344.35813853264824, -0.46681954515138263, 0.0, 0.389387904541571] ) ###speed= 1.19162642333 #from reachable_set import ReachableSet #self.reach = ReachableSet( p, s, dp, force, R[:,:,0], amin, amax) #self.reach.Plot() [R,amin,amax] = self.getControlMatrix(p) from reachable_set3d import ReachableSet3D self.reach = ReachableSet3D(env) dnp = dp/np.linalg.norm(dp) dpnormal = -0.001*(force - np.dot(force,dnp)*dnp) self.reach.PlotSingleSet(self.DISCRETIZATION_TIME_STEP, p, dp, speed, force, R[:,:,0], amin, amax) #self.reach.PlotTotalSet(self.DISCRETIZATION_TIME_STEP, # p, dp, speed, force, # R[:,:,0], amin, amax) #self.reach.PlotMoveAgainstWrenchField(self.DISCRETIZATION_TIME_STEP, # p, dpnormal, dp, speed, force, # R[:,:,0], amin, amax) #self.reach.PlotStretch(self.DISCRETIZATION_TIME_STEP, # p, dp, speed, force, # R[:,:,0], amin, amax) def get_dimension(self): [F,dF] = self.evaluate_at(0) return F.shape[0] def waypoint_to_force(self, env, W): Ndims = W.shape[0] pt = np.array(((W[0],W[1],-0.1,W[3]))) F = np.zeros((Ndims)) F[0:3] = env.GetForceAtX(pt) r = 0.3 theta = W[3] rcom = r * np.dot(Rz(theta),ex) torque = np.cross(rcom,F[0:2]) ### TORQUE application F[3] = np.sign(torque[2])*np.linalg.norm(torque) return F def IsInCollision(self, env, W): [Ndim,Nwaypoints] = self.getWaypointDim(W) for i in range(0,Nwaypoints): if env.CheckCollisionAtX(W[:,i]): return True return False def GetFirstCollisionPointIdx(self, env, W): [Ndim,Nwaypoints] = self.getWaypointDim(W) for i in range(0,Nwaypoints): if env.CheckCollisionAtX(W[:,i]): return i return None def get_tangent_forces_at_waypoints(self, W, dW, env): F = self.get_forces_at_waypoints(W, env) if F.ndim>1: Nwaypoints = F.shape[1] else: Nwaypoints = 1 FT = copy.copy(F) for i in range(0,Nwaypoints): T = dW[:,i]/np.linalg.norm(dW[:,i]) FT[:,i] = np.dot(F[:,i],T)*T return FT def get_normal_forces_at_waypoints(self, W, dW, env): F = self.get_forces_at_waypoints(W, env) if F.ndim>1: Nwaypoints = F.shape[1] else: Nwaypoints = 1 FN = copy.copy(F) for i in range(0,Nwaypoints): T = dW[:,i]/np.linalg.norm(dW[:,i]) FN[:,i] = F[:,i] - np.dot(F[:,i],T)*T return FN def get_forces_at_waypoints(self, W, env): Ndim = W.shape[0] if W.ndim>1: Nwaypoints = W.shape[1] else: F = self.waypoint_to_force(env,W) return F F = np.zeros((Ndim,Nwaypoints)) for i in range(0,Nwaypoints): F[:,i] = self.waypoint_to_force(env, W[:,i]) return F def getWaypointDim(self, W): Ndim = W.shape[0] if W.ndim>1: Nwaypoints = W.shape[1] else: Nwaypoints = 1 return [Ndim, Nwaypoints] def IsReparametrizable(self, env): S = self.reparametrize(env, ploting=False) if S is None: return False else: return True def PlotParametrization(self, env): self.topp = TOPPInterface(W, env) if self.topp.ReparameterizeTrajectory(): self.topp.PlotTrajectory(env) print self.topp else: print "Trajectory has no ReParameterization" def getCriticalPointFromWaypoints(self, env, W, oldNc = 0): self.topp = TOPPInterface(W, env) Nc = self.topp.getCriticalPoint()-1 if Nc < 0: print "return oldNc=",oldNc Nc = oldNc return Nc def getCriticalPoint(self, env): #[W,dW,ddW] = self.get_waypoints_second_order() N = self.getCriticalPointFromWaypoints(env, self.waypoints) if N is None: print "No critical point found" sys.exit(1) return N def getVelocityIntervalWithoutForceField(self, env, W): self.topp = TOPPInterface(W, env, zeroForce=True) if self.topp.ReparameterizeTrajectory(): return self.topp.traj0 else: #### without a force field any path should be executable #### at near zero speed => catch it if not print "WARNING1: without force field, TOPP couldn't find a valid \ velocity profile. Path not continuous or system not STLC" print "discrtimestep=",self.topp.discrtimestep self.topp.SaveToFile('clc2') CP = self.topp.getCriticalPoint() traj = self.topp.traj0 q = np.array([traj.Eval(t) for t in np.linspace(0,traj.duration,1e5)]).T plot(q[0,:],q[1,:],'-r',linewidth=2) plot(W[0,:],W[1,:],'ok',markersize=2) plot(W[0,CP],W[1,CP],'og',markersize=10) plt.show() sys.exit(1) def getTOPPTrajectoryWithoutForceField(self, env, W): self.topp = TOPPInterface(W, env, zeroForce=True) if self.topp.ReparameterizeTrajectory(): return self.topp else: self.info() print "WARNING2: without force field, TOPP couldn't find a valid \ velocity profile. Path not continuous or system not STLC" print W.shape print W sys.exit(1) #def getTOPPTrajectory(self, env, W, dW, F): # self.topp = TOPPInterface(W, env) # if self.topp.ReparameterizeTrajectory(): # return self.topp # else: # print W.shape # print W # self.info() # sys.exit(1) # return None @classmethod def from_ravetraj(cls, ravetraj): N = ravetraj.GetNumWaypoints() W=[] for i in range(0,N): w = np.array((ravetraj.GetWaypoint(i)[0],ravetraj.GetWaypoint(i)[1],ravetraj.GetWaypoint(i)[2],ravetraj.GetWaypoint(i)[3])) W.append((w)) W = np.array(W).T return cls(W) @classmethod def from_file(cls, filename): W = np.load(filename+'.npy') return cls(W) def get_number_waypoints(self): dt = self.DISCRETIZATION_TIME_STEP L = self.get_length() N = int(L/dt) return N def get_waypoints_second_order(self, N=None): ############################################################### ### obtain waypoints along the trajectory at constant spacing of ### DISCRETIZATION_TIME_STEP ############################################################### if N is None: N = self.get_number_waypoints() #[Ndim,Nwaypoints] = self.getWaypointDim(self.waypoints) #N = Nwaypoints ############################################################### K = self.get_dimension() pts = np.zeros((K,N)) dpts = np.zeros((K,N)) ddpts = np.zeros((K,N)) ctr = 0 for t in np.linspace(0.0, self.get_length(), num=N): [f0,df0] = self.evaluate_at(t,der=1) pts[:,ctr] = f0 dpts[:,ctr] = df0 #ddpts[:,ctr] = ddf0 ctr = ctr+1 return [pts,dpts,ddpts] def get_waypoints(self, N = None): [pts,dpts,ddpts]=self.get_waypoints_second_order(N) return [pts,dpts] def get_length(self): return self.length #dd = 0.0 #T = np.linspace(0.0, 1.0, num=self.waypoints.shape[1]) #for i in range(0,len(T)-1): # t = T[i] # tdt = T[i+1] # [ft,df0] = self.evaluate_at(t) # [ftdt,df0] = self.evaluate_at(tdt) # dd = dd + np.linalg.norm(ft-ftdt) #return dd def get_handle_draw_waypoints(self, env, W, dW, ddW): Ndims = W.shape[0] Nwaypoints = W.shape[1] tmp_handle = [] if Nwaypoints > 0: with env.env: Wext = np.zeros((3, 2*Nwaypoints)) Wtheta = np.zeros((3, 3*Nwaypoints)) for i in range(0,Nwaypoints): pt = np.array(((W[0,i],W[1,i],W[2,i]+self.tangent_zoffset))) dpt = np.array(((dW[0,i],dW[1,i],dW[2,i]+self.tangent_zoffset))) dpt = self.dVECTOR_LENGTH*dpt/np.linalg.norm(dpt) Wext[:,2*i] = pt Wext[:,2*i+1] = np.array( (pt[0]+dpt[0],pt[1]+dpt[1],pt[2]+dpt[2]) ) #### orientation of system theta = W[3,i] etheta = self.dVECTOR_LENGTH*np.dot(Rz(theta),ex) etheta[2] += self.tangent_zoffset Wtheta[:,3*i] = pt Wtheta[:,3*i+1] = np.array( (pt[0]+etheta[0],pt[1]+etheta[1],pt[2]+etheta[2]) ) Wtheta[:,3*i+2] = pt #Wext = np.array(zip(W[0:3,:],Wdir)).flatten().reshape(Ndim,2*Nwaypoints) if self.show_tangent_vector: h=env.env.drawlinestrip(points=Wext.T,linewidth=self.lw_tangent,colors=self.trajectory_tangent_color) tmp_handle.append(h) if self.show_orientation_vector: h=env.env.drawlinestrip(points=Wtheta.T,linewidth=self.lw_orientation,colors=self.trajectory_orientation_color) tmp_handle.append(h) h=env.env.drawlinestrip(points=W[0:3,:].T,linewidth=self.lw_path,colors=self.trajectory_color) tmp_handle.append(h) return tmp_handle def draw(self, env, keep_handle=True, critical_pt = None, DrawRobot=False): [W,dW,ddW] = self.get_waypoints_second_order() t1 = time.time() if critical_pt == None: N = self.getCriticalPointFromWaypoints(env, W) else: N = critical_pt t2 = time.time() tmp_handle = [] self.trajectory_color = self.trajectory_color_feasible tmp_handle.append(self.get_handle_draw_waypoints(env, W[:,0:N], dW[:,0:N], ddW[:,0:N])) self.trajectory_color = self.trajectory_color_infeasible tmp_handle.append(self.get_handle_draw_waypoints(env, W[:,N:], dW[:,N:], ddW[:,N:])) t3 = time.time() if self.DEBUG: print "draw loop: ",t3-t1," topp:",t2-t1," draw:",t3-t2 if keep_handle: self.handle = tmp_handle else: return tmp_handle def draw_delete(self): self.handle = [] def draw_robot_along_path(self, env, robot, N=10): xt = self.topp.traj0 with env.env: robot.GetLinks()[0].SetStatic(True) env.env.StopSimulation() env.MakeRobotVisible() ictr=0 print "before:",env.env.GetRobots() R = env.env.GetRobot("clone"+str(ictr)) while R is not None: env.env.Remove(R) ictr+=1 R = env.env.GetRobot("clone"+str(ictr)) print "after deleting:",env.env.GetRobots() with env.env: ictr=0 for t in np.linspace(0,xt.duration,N): print "t",t,"/",xt.duration robot_t = RaveCreateRobot(env.env,robot.GetXMLId()) robot_t.Clone(robot,0) robot_t.SetName("clone"+str(ictr)) env.env.AddRobot(robot_t,True) #env.ChangeTransparencyRobot(robot_t, 1.0) robot_t.SetDOFValues(xt.Eval(t)) ictr+=1 #robot_t.SetDOFValues(xt.Eval(t)) print "after adding:",env.env.GetRobots() def execute(self, env, robot, tsleep=0.01, stepping=False): tstep = 0.01 xt = self.topp.traj0 with env.env: robot.GetLinks()[0].SetStatic(True) env.env.StopSimulation() t = 0.0 tstep = 0.01 robot.SetDOFValues(xt.Eval(t)) env.MakeRobotVisible() q = xt.Eval(0) dq = xt.Evald(0) while t < xt.duration: #q = xt.Eval(t) #dq = xt.Evald(t) ddq = xt.Evaldd(t) #qn = q + tstep*dq + 0.5*tstep*tstep*ddq #robot.SetDOFValues(qn) q = q + tstep*dq + 0.5*tstep*tstep*ddq dq = dq + tstep*ddq robot.SetDOFValues(q) env.env.StepSimulation(tstep) time.sleep(tsleep) t += tstep if stepping: raw_input('Press Key to Step. Time: '+str(t)+'/'+str(xt.duration)) def computeSplineFromWaypoints(self,W): Ndim = W.shape[0] Nwaypoints = W.shape[1] bspline=[] tvec = np.linspace(0,1,Nwaypoints) for i in range(Nwaypoints-1): tvec[i+1] = tvec[i]+linalg.norm(W[:,i]-W[:,i+1]) self.length = tvec[i+1] for i in range(0,Ndim): WP = W[i,:] trajectory = splrep(tvec,WP,k=self.POLYNOMIAL_DEGREE,s=self.SMOOTH_CONSTANT) bspline.append(trajectory) return bspline def GetIntervalIndex(self, W): Ndim = W.shape[0] Nwaypoints = W.shape[1] idx = [] epsilon = 1e-1 i = 0 idx.append(0) while i < Nwaypoints-1: k = 1 d = 0.0 while d < epsilon: if i+k > Nwaypoints-1: k = Nwaypoints-i break #print "i",i,"k",k,"d",d,Nwaypoints q0 = W[:,i] q1 = W[:,i+k] d = np.linalg.norm(q0-q1) k+=1 k=k-1 i = i + k idx.append(i) return np.array(idx) def InsertMissingWaypoints(self,Wnext,max_dist): ictr=0 for i in range(0,Wnext.shape[1]-1): d = np.linalg.norm(Wnext[:,i]-Wnext[:,i+1]) if d > max_dist: ## insert some points dW = Wnext[:,i+1]-Wnext[:,i] dstep = max_dist dall = np.linalg.norm(dW) Nnew = int(dall/dstep) - 1 dcur = 0.0 for k in range(1,Nnew): Wstep = (Wnext[:,i+k]-Wnext[:,i]) Wstep /= np.linalg.norm(Wstep) Wnew = Wnext[:,i] + k*dstep*Wstep Wnext = np.insert(Wnext, i+k, Wnew, axis=1) ictr+=1 print "added",ictr return Wnext def RepairTrajectory(self,W,max_dist): ictr=0 i=0 while i < W.shape[1]-1: d = np.linalg.norm(W[:,i+1]-W[:,i]) istep = 1 if d > max_dist: dstep = max_dist Nnew = int(d/dstep) dcur = 0.0 #print i,d,"/",max_dist,"d/dstep",d/dstep,Nnew for k in range(1,Nnew): Wstep = (W[:,i+k]-W[:,i]) Wstep /= np.linalg.norm(Wstep) Wnew = W[:,i] + k*dstep*Wstep W= np.insert(W, i+k, Wnew, axis=1) ictr+=1 istep = Nnew i = i+istep print "new points:",ictr return W
class Trajectory(): __metaclass__ = abc.ABCMeta DEBUG = 0 DISCRETIZATION_TIME_STEP = 0.01 SMOOTH_CONSTANT = 0 ##TODO: do not change to >0 => problems with PPoly POLYNOMIAL_DEGREE = 1 MIN_NUMBER_WAYPOINTS = 5 rave_traj = [] traj = [] waypoints = [] handle = [] #env_ptr = [] ##drawing parameters ptsize = 0.03 critical_pt_size = 0.08 show_tangent_vector = False show_orientation_vector = False lw_path = 10 lw_tangent = 3 lw_orientation = 3 FONT_SIZE = 20 dVECTOR_LENGTH = 0.4 trajectory_orientation_color = np.array((0.9, 0.9, 0.9, 0.3)) trajectory_tangent_color = np.array((0.9, 0.2, 0.9, 0.3)) trajectory_color_deformation = np.array((0.9, 0.2, 0.9, 0.9)) tangent_zoffset = -0.02 trajectory_color_feasible = np.array((0.2, 0.9, 0.2, 0.9)) trajectory_color_infeasible = np.array((0.9, 0.2, 0.2, 0.9)) trajectory_color = np.array((0.9, 0.2, 0.2, 0.9)) critical_pt_color = np.array((0.9, 0.2, 0.2, 0.9)) bspline = [] Ndim = 0 ### waypoints: real matrix of dimensionality Ndim x Nwaypoints def __init__(self, waypoints_in): self.new_from_waypoints(waypoints_in) def new_from_waypoints(self, waypoints_in): self.waypoints = self.prettify(waypoints_in) self.Ndim = self.waypoints.shape[0] self.Nwaypoints = self.waypoints.shape[1] self.bspline = self.computeSplineFromWaypoints(self.waypoints) [self.waypoints, dW, ddW] = self.get_waypoints_second_order() def save(self, filename): np.save(filename, self.waypoints) def load(self, filename): W = np.load(filename + '.npy') return W def evaluate_at(self, t, der=1): f = np.zeros((self.Ndim)) df = np.zeros((self.Ndim)) if der > 1: ddf = np.zeros((self.Ndim)) for i in range(0, self.Ndim): f[i] = splev(t, self.bspline[i]) df[i] = splev(t, self.bspline[i], der=1) if der > 1: ddf[i] = splev(t, self.bspline[i], der=2) df[2] = 0 if der > 1: ddf[2] = 0 return [f, df, ddf] else: return [f, df] def prettify(self, W): if W.ndim <= 1: print "cannot create trajectory with only one waypoint" sys.exit(1) W = self.removeDuplicateWaypoints(W) Nwaypoints = W.shape[1] if Nwaypoints <= 3: W = self.addMinimalWaypoints(W) Nwaypoints = W.shape[1] return W def removeDuplicateWaypoints(self, W): ACCURACY_DUPLICATE = self.DISCRETIZATION_TIME_STEP / 1e10 Nwaypoints = W.shape[1] if Nwaypoints == 1: print "cannot create trajectory with only one waypoint" sys.exit(1) if Nwaypoints == 2: if np.linalg.norm(W[:, 0] - W[:, 1]) < ACCURACY_DUPLICATE: print "only two identical waypoints. abort" sys.exit(1) return W assert (Nwaypoints > 2) if np.linalg.norm(W[:, 0] - W[:, 1]) < ACCURACY_DUPLICATE: print "deleting waypoint" W = np.delete(W, 0, axis=1) if np.linalg.norm(W[:, -2] - W[:, -1]) < ACCURACY_DUPLICATE: print "deleting waypoint" W = np.delete(W, -1, axis=1) return W def addMinimalWaypoints(self, W): Nwaypoints = W.shape[1] if Nwaypoints >= self.MIN_NUMBER_WAYPOINTS: return W Wtmp = W for i in range(0, Nwaypoints - 1): print "adding waypoint" Wnew = W[:, i] + 0.5 * (W[:, i + 1] - W[:, i]) Wtmp = np.insert(Wtmp, 2 * i + 1, values=Wnew, axis=1) W = Wtmp return self.addMinimalWaypoints(W) ### SYSTEM DYNAMICS def getControlMatrix(self, W): import parameters_dynamical_system as params #AM = 1 amin = params.amin amax = params.amax [Ndim, Nwaypoints] = self.getWaypointDim(W) R = params.ControlPerWaypoint(W, Ndim, Nwaypoints) return [R, amin, amax] def info(self): print "#### TRAJECTORY CLASS ######" print "LENGTH: ", self.get_length() print "DISCRETIZATION:", self.DISCRETIZATION_TIME_STEP print "WAYPOINTS: ", self.waypoints.shape[1] [f0, df0] = np.around(self.evaluate_at(0), 2) [f1, df1] = np.around(self.evaluate_at(1), 2) print "START : ", f0, " dir: ", df0 print "GOAL : ", f1, " dir: ", df1 print "WAYPOINTS : " print " [ T ] [ WAYPOINT ]" for tt in np.linspace(0, 1, 10): [f, df] = self.evaluate_at(tt) print " [ ", np.around(tt, 1), " ] ", np.around(f, decimals=2) def plot_reachableset(self, env): thetavec = [-pi / 2, -pi / 4, 0, pi / 4, pi / 2] thetavec = [0] for theta in thetavec: ##sailboat force = np.array((2.5, -3.5, 0, 5.0)) dp = np.array((1, 0.0, 0, 0.2)) ##car ds = 0.01 ### extra large dp p = np.array([ -2.608897298112661, -0.2047366548132623, 0.1000000008651387, -2.987014267911084 ]) dp = np.array([ -16.487757154509474, -4.837209926510647, 0.0, 4.899656054155726 ]) force = np.array([0.0, 3.0, 0.0, -1.4821147977289577]) speed = 3.86342505425 ### no force, no dp p = np.array([ -2.644855145592143, 1.1657167012260294, 0.10000000000235312, -3.7242126663188597 ]) dp = np.array([ -1.1866896124736256e-20, 1.910882585951823e-18, 1.4346537045461183e-27, -9.702129945467387e-19 ]) force = np.array([0.0, 0.0, 0.0, 0.0]) speed = 0.341259539422 ### no force, no dp p = np.array([ -2.644855145592143, 1.1657167012260294, 0.10000000000235312, -3.7242126663188597 ]) dp = np.array([0.0, 0.0, 0.0, 0.0]) force = np.array([0.0, 0.0, 0.0, 0.0]) speed = 0.0 ### close to large dp p = np.array([ -4.113833032000303, -0.0026206376782298216, 0.0999999999999477, -3.139440349128482 ]) dp = np.array([ -982166.3702744454, -1372.8293479195447, -8.057068259496254e-08, 1124.7453741523088 ]) force = np.array([0.0, 0.0, 0.0, 0.0]) speed = 1.07398453113 p = np.array([-5.05e+00, -4.08e-03, 1.00e-01, -3.14e+00]) dp = np.array([-2.97e+08, -4.32e+05, 5.76e-06, 3.48e+05]) F = np.array([0., 0., 0., 0.]) speed = 1.07398453113 #ds 1.19162456455 #dt 2.19793777714e-11 p = np.array([ -2.5974191119628514, -0.0004997370983040311, 0.0999999999999989, -3.141179458965877 ]) dp = np.array([ -1.2469468647720259, -0.002999767139199421, 0.0, 0.0017880891982438346 ]) force = np.array([0.0, 0.0, 0.0, 0.0]) speed = 1.51000061035 #ds: 0.00305489294111 ### nice vis p = np.array((0, 1e-4, 0, 0)) force = np.array((0.5, -3.5, 0, 1.0)) dp = np.array((1, 0.1, 0, 0.2)) speed = 0.2 ##### error ###p =np.array( [-6.012902723369949, -0.005123021667037449, 0.1, -3.1373188699666956] ) ###force =np.array( [0.0, 0.0, 0.0, 0.0] ) ###dp =np.array( [-344.35813853264824, -0.46681954515138263, 0.0, 0.389387904541571] ) ###speed= 1.19162642333 #from reachable_set import ReachableSet #self.reach = ReachableSet( p, s, dp, force, R[:,:,0], amin, amax) #self.reach.Plot() [R, amin, amax] = self.getControlMatrix(p) from reachable_set3d import ReachableSet3D self.reach = ReachableSet3D(env) dnp = dp / np.linalg.norm(dp) dpnormal = -0.001 * (force - np.dot(force, dnp) * dnp) self.reach.PlotSingleSet(self.DISCRETIZATION_TIME_STEP, p, dp, speed, force, R[:, :, 0], amin, amax) #self.reach.PlotTotalSet(self.DISCRETIZATION_TIME_STEP, # p, dp, speed, force, # R[:,:,0], amin, amax) #self.reach.PlotMoveAgainstWrenchField(self.DISCRETIZATION_TIME_STEP, # p, dpnormal, dp, speed, force, # R[:,:,0], amin, amax) #self.reach.PlotStretch(self.DISCRETIZATION_TIME_STEP, # p, dp, speed, force, # R[:,:,0], amin, amax) def get_dimension(self): [F, dF] = self.evaluate_at(0) return F.shape[0] def waypoint_to_force(self, env, W): Ndims = W.shape[0] pt = np.array(((W[0], W[1], -0.1, W[3]))) F = np.zeros((Ndims)) F[0:3] = env.GetForceAtX(pt) r = 0.3 theta = W[3] rcom = r * np.dot(Rz(theta), ex) torque = np.cross(rcom, F[0:2]) ### TORQUE application F[3] = np.sign(torque[2]) * np.linalg.norm(torque) return F def IsInCollision(self, env, W): [Ndim, Nwaypoints] = self.getWaypointDim(W) for i in range(0, Nwaypoints): if env.CheckCollisionAtX(W[:, i]): return True return False def GetFirstCollisionPointIdx(self, env, W): [Ndim, Nwaypoints] = self.getWaypointDim(W) for i in range(0, Nwaypoints): if env.CheckCollisionAtX(W[:, i]): return i return None def get_tangent_forces_at_waypoints(self, W, dW, env): F = self.get_forces_at_waypoints(W, env) if F.ndim > 1: Nwaypoints = F.shape[1] else: Nwaypoints = 1 FT = copy.copy(F) for i in range(0, Nwaypoints): T = dW[:, i] / np.linalg.norm(dW[:, i]) FT[:, i] = np.dot(F[:, i], T) * T return FT def get_normal_forces_at_waypoints(self, W, dW, env): F = self.get_forces_at_waypoints(W, env) if F.ndim > 1: Nwaypoints = F.shape[1] else: Nwaypoints = 1 FN = copy.copy(F) for i in range(0, Nwaypoints): T = dW[:, i] / np.linalg.norm(dW[:, i]) FN[:, i] = F[:, i] - np.dot(F[:, i], T) * T return FN def get_forces_at_waypoints(self, W, env): Ndim = W.shape[0] if W.ndim > 1: Nwaypoints = W.shape[1] else: F = self.waypoint_to_force(env, W) return F F = np.zeros((Ndim, Nwaypoints)) for i in range(0, Nwaypoints): F[:, i] = self.waypoint_to_force(env, W[:, i]) return F def getWaypointDim(self, W): Ndim = W.shape[0] if W.ndim > 1: Nwaypoints = W.shape[1] else: Nwaypoints = 1 return [Ndim, Nwaypoints] def IsReparametrizable(self, env): S = self.reparametrize(env, ploting=False) if S is None: return False else: return True def PlotParametrization(self, env): self.topp = TOPPInterface(W, env) if self.topp.ReparameterizeTrajectory(): self.topp.PlotTrajectory(env) print self.topp else: print "Trajectory has no ReParameterization" def getCriticalPointFromWaypoints(self, env, W, oldNc=0): self.topp = TOPPInterface(W, env) Nc = self.topp.getCriticalPoint() - 1 if Nc < 0: print "return oldNc=", oldNc Nc = oldNc return Nc def getCriticalPoint(self, env): #[W,dW,ddW] = self.get_waypoints_second_order() N = self.getCriticalPointFromWaypoints(env, self.waypoints) if N is None: print "No critical point found" sys.exit(1) return N def getVelocityIntervalWithoutForceField(self, env, W): self.topp = TOPPInterface(W, env, zeroForce=True) if self.topp.ReparameterizeTrajectory(): return self.topp.traj0 else: #### without a force field any path should be executable #### at near zero speed => catch it if not print "WARNING1: without force field, TOPP couldn't find a valid \ velocity profile. Path not continuous or system not STLC" print "discrtimestep=", self.topp.discrtimestep self.topp.SaveToFile('clc2') CP = self.topp.getCriticalPoint() traj = self.topp.traj0 q = np.array( [traj.Eval(t) for t in np.linspace(0, traj.duration, 1e5)]).T plot(q[0, :], q[1, :], '-r', linewidth=2) plot(W[0, :], W[1, :], 'ok', markersize=2) plot(W[0, CP], W[1, CP], 'og', markersize=10) plt.show() sys.exit(1) def getTOPPTrajectoryWithoutForceField(self, env, W): self.topp = TOPPInterface(W, env, zeroForce=True) if self.topp.ReparameterizeTrajectory(): return self.topp else: self.info() print "WARNING2: without force field, TOPP couldn't find a valid \ velocity profile. Path not continuous or system not STLC" print W.shape print W sys.exit(1) #def getTOPPTrajectory(self, env, W, dW, F): # self.topp = TOPPInterface(W, env) # if self.topp.ReparameterizeTrajectory(): # return self.topp # else: # print W.shape # print W # self.info() # sys.exit(1) # return None @classmethod def from_ravetraj(cls, ravetraj): N = ravetraj.GetNumWaypoints() W = [] for i in range(0, N): w = np.array( (ravetraj.GetWaypoint(i)[0], ravetraj.GetWaypoint(i)[1], ravetraj.GetWaypoint(i)[2], ravetraj.GetWaypoint(i)[3])) W.append((w)) W = np.array(W).T return cls(W) @classmethod def from_file(cls, filename): W = np.load(filename + '.npy') return cls(W) def get_number_waypoints(self): dt = self.DISCRETIZATION_TIME_STEP L = self.get_length() N = int(L / dt) return N def get_waypoints_second_order(self, N=None): ############################################################### ### obtain waypoints along the trajectory at constant spacing of ### DISCRETIZATION_TIME_STEP ############################################################### if N is None: N = self.get_number_waypoints() #[Ndim,Nwaypoints] = self.getWaypointDim(self.waypoints) #N = Nwaypoints ############################################################### K = self.get_dimension() pts = np.zeros((K, N)) dpts = np.zeros((K, N)) ddpts = np.zeros((K, N)) ctr = 0 for t in np.linspace(0.0, self.get_length(), num=N): [f0, df0] = self.evaluate_at(t, der=1) pts[:, ctr] = f0 dpts[:, ctr] = df0 #ddpts[:,ctr] = ddf0 ctr = ctr + 1 return [pts, dpts, ddpts] def get_waypoints(self, N=None): [pts, dpts, ddpts] = self.get_waypoints_second_order(N) return [pts, dpts] def get_length(self): return self.length #dd = 0.0 #T = np.linspace(0.0, 1.0, num=self.waypoints.shape[1]) #for i in range(0,len(T)-1): # t = T[i] # tdt = T[i+1] # [ft,df0] = self.evaluate_at(t) # [ftdt,df0] = self.evaluate_at(tdt) # dd = dd + np.linalg.norm(ft-ftdt) #return dd def get_handle_draw_waypoints(self, env, W, dW, ddW): Ndims = W.shape[0] Nwaypoints = W.shape[1] tmp_handle = [] if Nwaypoints > 0: with env.env: Wext = np.zeros((3, 2 * Nwaypoints)) Wtheta = np.zeros((3, 3 * Nwaypoints)) for i in range(0, Nwaypoints): pt = np.array( ((W[0, i], W[1, i], W[2, i] + self.tangent_zoffset))) dpt = np.array( ((dW[0, i], dW[1, i], dW[2, i] + self.tangent_zoffset))) dpt = self.dVECTOR_LENGTH * dpt / np.linalg.norm(dpt) Wext[:, 2 * i] = pt Wext[:, 2 * i + 1] = np.array( (pt[0] + dpt[0], pt[1] + dpt[1], pt[2] + dpt[2])) #### orientation of system theta = W[3, i] etheta = self.dVECTOR_LENGTH * np.dot(Rz(theta), ex) etheta[2] += self.tangent_zoffset Wtheta[:, 3 * i] = pt Wtheta[:, 3 * i + 1] = np.array( (pt[0] + etheta[0], pt[1] + etheta[1], pt[2] + etheta[2])) Wtheta[:, 3 * i + 2] = pt #Wext = np.array(zip(W[0:3,:],Wdir)).flatten().reshape(Ndim,2*Nwaypoints) if self.show_tangent_vector: h = env.env.drawlinestrip( points=Wext.T, linewidth=self.lw_tangent, colors=self.trajectory_tangent_color) tmp_handle.append(h) if self.show_orientation_vector: h = env.env.drawlinestrip( points=Wtheta.T, linewidth=self.lw_orientation, colors=self.trajectory_orientation_color) tmp_handle.append(h) h = env.env.drawlinestrip(points=W[0:3, :].T, linewidth=self.lw_path, colors=self.trajectory_color) tmp_handle.append(h) return tmp_handle def draw(self, env, keep_handle=True, critical_pt=None, DrawRobot=False): [W, dW, ddW] = self.get_waypoints_second_order() t1 = time.time() if critical_pt == None: N = self.getCriticalPointFromWaypoints(env, W) else: N = critical_pt t2 = time.time() tmp_handle = [] self.trajectory_color = self.trajectory_color_feasible tmp_handle.append( self.get_handle_draw_waypoints(env, W[:, 0:N], dW[:, 0:N], ddW[:, 0:N])) self.trajectory_color = self.trajectory_color_infeasible tmp_handle.append( self.get_handle_draw_waypoints(env, W[:, N:], dW[:, N:], ddW[:, N:])) t3 = time.time() if self.DEBUG: print "draw loop: ", t3 - t1, " topp:", t2 - t1, " draw:", t3 - t2 if keep_handle: self.handle = tmp_handle else: return tmp_handle def draw_delete(self): self.handle = [] def draw_robot_along_path(self, env, robot, N=10): xt = self.topp.traj0 with env.env: robot.GetLinks()[0].SetStatic(True) env.env.StopSimulation() env.MakeRobotVisible() ictr = 0 print "before:", env.env.GetRobots() R = env.env.GetRobot("clone" + str(ictr)) while R is not None: env.env.Remove(R) ictr += 1 R = env.env.GetRobot("clone" + str(ictr)) print "after deleting:", env.env.GetRobots() with env.env: ictr = 0 for t in np.linspace(0, xt.duration, N): print "t", t, "/", xt.duration robot_t = RaveCreateRobot(env.env, robot.GetXMLId()) robot_t.Clone(robot, 0) robot_t.SetName("clone" + str(ictr)) env.env.AddRobot(robot_t, True) #env.ChangeTransparencyRobot(robot_t, 1.0) robot_t.SetDOFValues(xt.Eval(t)) ictr += 1 #robot_t.SetDOFValues(xt.Eval(t)) print "after adding:", env.env.GetRobots() def execute(self, env, robot, tsleep=0.01, stepping=False): tstep = 0.01 xt = self.topp.traj0 with env.env: robot.GetLinks()[0].SetStatic(True) env.env.StopSimulation() t = 0.0 tstep = 0.01 robot.SetDOFValues(xt.Eval(t)) env.MakeRobotVisible() q = xt.Eval(0) dq = xt.Evald(0) while t < xt.duration: #q = xt.Eval(t) #dq = xt.Evald(t) ddq = xt.Evaldd(t) #qn = q + tstep*dq + 0.5*tstep*tstep*ddq #robot.SetDOFValues(qn) q = q + tstep * dq + 0.5 * tstep * tstep * ddq dq = dq + tstep * ddq robot.SetDOFValues(q) env.env.StepSimulation(tstep) time.sleep(tsleep) t += tstep if stepping: raw_input('Press Key to Step. Time: ' + str(t) + '/' + str(xt.duration)) def computeSplineFromWaypoints(self, W): Ndim = W.shape[0] Nwaypoints = W.shape[1] bspline = [] tvec = np.linspace(0, 1, Nwaypoints) for i in range(Nwaypoints - 1): tvec[i + 1] = tvec[i] + linalg.norm(W[:, i] - W[:, i + 1]) self.length = tvec[i + 1] for i in range(0, Ndim): WP = W[i, :] trajectory = splrep(tvec, WP, k=self.POLYNOMIAL_DEGREE, s=self.SMOOTH_CONSTANT) bspline.append(trajectory) return bspline def GetIntervalIndex(self, W): Ndim = W.shape[0] Nwaypoints = W.shape[1] idx = [] epsilon = 1e-1 i = 0 idx.append(0) while i < Nwaypoints - 1: k = 1 d = 0.0 while d < epsilon: if i + k > Nwaypoints - 1: k = Nwaypoints - i break #print "i",i,"k",k,"d",d,Nwaypoints q0 = W[:, i] q1 = W[:, i + k] d = np.linalg.norm(q0 - q1) k += 1 k = k - 1 i = i + k idx.append(i) return np.array(idx) def InsertMissingWaypoints(self, Wnext, max_dist): ictr = 0 for i in range(0, Wnext.shape[1] - 1): d = np.linalg.norm(Wnext[:, i] - Wnext[:, i + 1]) if d > max_dist: ## insert some points dW = Wnext[:, i + 1] - Wnext[:, i] dstep = max_dist dall = np.linalg.norm(dW) Nnew = int(dall / dstep) - 1 dcur = 0.0 for k in range(1, Nnew): Wstep = (Wnext[:, i + k] - Wnext[:, i]) Wstep /= np.linalg.norm(Wstep) Wnew = Wnext[:, i] + k * dstep * Wstep Wnext = np.insert(Wnext, i + k, Wnew, axis=1) ictr += 1 print "added", ictr return Wnext def RepairTrajectory(self, W, max_dist): ictr = 0 i = 0 while i < W.shape[1] - 1: d = np.linalg.norm(W[:, i + 1] - W[:, i]) istep = 1 if d > max_dist: dstep = max_dist Nnew = int(d / dstep) dcur = 0.0 #print i,d,"/",max_dist,"d/dstep",d/dstep,Nnew for k in range(1, Nnew): Wstep = (W[:, i + k] - W[:, i]) Wstep /= np.linalg.norm(Wstep) Wnew = W[:, i] + k * dstep * Wstep W = np.insert(W, i + k, Wnew, axis=1) ictr += 1 istep = Nnew i = i + istep print "new points:", ictr return W