class ArmSim(object): # ===== Paper location input for simulation ===== Rp = Rrpy(pi/2,0,0) # Roll-pitch-yaw rotation parameterization PAPER_BASE = np.identity(4) # Paper frame rigid body transform PAPER_BASE[0:3,0:3] = Rp # Paper rotation matrix PAPER_BASE[0:3,3] = np.asfarray([-Paper.X_SIZE/2,-50,0]) # Paper origin # Arm Location Ra = Rrpy(-pi/2-0.05,pi/2,0) # Arm Roll-Pitch-Yaw base orientation parameterization ARM_BASE = np.identity(4) # Arm fixed-base rigid body transform ARM_BASE[0:3,3] = np.asfarray([250,-50,0]) # Arm origin ARM_BASE[0:3,0:3] = Ra # Set rotation INITIAL_CONFIG = np.asfarray([pi/4,pi/4,0]) # Initial arm joint configuration # ===== Set of waypoints on paper ===== WAYPOINTS = [[[10,0],[10,30]],[[10,15],[20,15]],[[20,0],[20,30]]] def __init__(self): self.arm = Arm(self.ARM_BASE, 100, 160, 200, 1, -20) # TODO: Initialze arm object #self.paperPlot = PaperPlot() #self.forcePlot = ForcePlot() self.paper = Paper(self.PAPER_BASE) # Initialize paper self.armPlot = ArmPlot() # Initialize the 3D plot self.armPlot.plotPaper(self.paper) self.armPlot.plotArm(self.arm) self.controller = DeltaController(self.arm, self.paper) # Run the simulation def run(self, strokes, initialConfig, minStep=100): initialConfig = np.asfarray(initialConfig) current = initialConfig # Current configuration self.worldWaypoints = self.paper.strokesToWorld(strokes) # Loop over waypoints #for i in range(0,len(strokes)): ''' ikConfig = np.append(self.arm.planarIK(strokes[i][0]),[50])# Compute IK print str(ikConfig) nsteps = minStep configs = interpolateLinear(current, ikConfig, nsteps) # Interpolate trajectory print configs.shape''' configs = self.controller.generateTrajectory(strokes) # Wait for replay # Loop over interpolated configurations while(True): for k in range(0, len(configs)): print 'Step', k #print str(configs[k]) self.arm.setConfiguration(self.rx64RoundConfig(configs[k])) # Update arm position self.armPlot.clear() self.armPlot.plotArm(self.arm) # Plot self.armPlot.plotPaper(self.paper) # Plot paper again self.armPlot.plotIdealPath(self.worldWaypoints) #print 'Arm Position', str(self.arm.eePosition()) self.arm.printEEPosition() draw() self.armPlot.fig.show() sleep(0.0001) c = input('Enter some string to continue') #current = ikConfig # Round the configuration to RX64 angles def rx64RoundConfig(self, config, randomness=0): nbits = 10 rx64Range = 5.0*pi/3.0 dConfig = (config/rx64Range) * pow(2.0, nbits) #print 'Discrete configuration:', str(dConfig) dConfig = np.round(dConfig) roundedConfig = dConfig/pow(2.0, nbits) * rx64Range #print 'Rounded Configuration', str(roundedConfig) # Generate Gaussian return roundedConfig