Python Vector.plus方法代码示例

# 需要导入模块: from vector import Vector [as 别名]
# 或者: from vector.Vector import plus [as 别名]
class Printer(object):

    def __init__ (self, x, y, r, m, grid, move_units_per_cell):
        self.position = Vector(float(x), float(y))
        self.r = r
        move_unit_pixels = grid.gridsize() / move_units_per_cell
        self.v = Vector(move_unit_pixels, move_unit_pixels)
        self.grid = grid
        self.penDown = False

    def set_position_on_grid(self, xcell, ycell):
        self.position = Vector((xcell * self.grid.gridsize()) + self.grid.gridsize()/2, (ycell * self.grid.gridsize())+ self.grid.gridsize()/2)

    def move (self):
        self.position = self.position.plus(self.v)
              
    def setPenUp(self):
        self.penDown = False
        
    def setPenDown(self):
        self.penDown = True

    def simulate(self):
        if self.move_is_valid():
            self.move()
            if self.penDown:
                position = (self.position.x, self.position.y)
                self.grid.PenDraw(position)

    def move_is_valid(self):
        """ Checks if moving with the given dt will cause collision with
            the boundaries of the grid """
        new_loc = self.position.plus(self.v)
        new_loc = (new_loc.x, new_loc.y)
        return self.grid.inbounds(new_loc)

# 需要导入模块: from vector import Vector [as 别名]
# 或者: from vector.Vector import plus [as 别名]
def LaserData(data):

       scans = []
       for d in data.ranges:
           if d > 0:
               scans.append(d)
           
       angle_min = data.angle_min
       angle_max = data.angle_max
       angle_inc = data.angle_increment
       forceVectors = []
            
       i = 0
       for laser in scans:
           scan_angle = angle_min + (angle_inc*i)
           opposing_angle = scan_angle + math.pi
           i += 1

           if(laser != 0):
               force = 1/(laser**2)
           else:
               force = 0
           forceVectors.append(Vector(force*math.cos(opposing_angle),force*math.sin(opposing_angle)))

            
       outputVector = Vector(0,0)
       for x in forceVectors:
           outputVector = outputVector.plus(x)
       outputVector = outputVector.plus(Vector(1,0))  

       mag = math.sqrt((outputVector.x**2)+(outputVector.y**2))
    
       #print mag
       if(mag < 50):
           event = 'laserclear'
           linear = .1
	   angular = 0
       else:
    	   event = 'laseravoid'
           angular = 5*(outputVector.y/mag)
           linear = 0.06

       if outputVector.y < 0:
           angular = angular * 10
    
       print event
       msg = SteeringMessage()
       msg.event = event
       msg.linear = linear
       msg.angular = angular
       pub = rospy.Publisher('/LaserMon', SteeringMessage, queue_size = 10)
       pub.publish(msg)

# 需要导入模块: from vector import Vector [as 别名]
# 或者: from vector.Vector import plus [as 别名]
class Printer(object):
    """A simple model of a 2d 3d-printer. Can draw in a GridWorld"""

    def __init__ (self, x, y, r, grid, sub_cell_resolution):
        """Constructs a printer which draws in a GridWorld and moves in increments of fractions of cell widths

        x: the x coordinate for the printer start position
        y: the y coordinate for the printer start position
        r: the radius of the printer (does not influence how the printer prints, only the way it is represented if drawn visually)
        grid: the gridworld that the printer can act upon
        sub_cell_resolution: the factor by which cell width is divided by to determine the distance moved by the printer in a single time unit
        """

        self.position = Vector(float(x), float(y))
        self.r = r
        move_unit_pixels = grid.gridsize() / sub_cell_resolution
        self.v = Vector(move_unit_pixels, move_unit_pixels)
        self.grid = grid
        self.penDown = False

    def set_printer_direction(self, leftright, updown):
        """Set the direction the printer will move in on the following time steps.

        leftright: -1 = left motion, 0 = no leftright motion, 1 = right motion
        updown: -1 = down motion, 0 = no updown motion, 1 = up motion"""
        self.v = Vector(leftright, updown)

    def set_position_on_grid(self, xcell, ycell):
        """ Move the printer to the specified cell position on the grid"""
        self.position = Vector((xcell * self.grid.gridsize()) + self.grid.gridsize()/2, (ycell * self.grid.gridsize())+ self.grid.gridsize()/2)

    def setPenUp(self):
        self.penDown = False
        
    def setPenDown(self):
        self.penDown = True

    def simulate(self):
        """Simulate a single time unit for the printer (which will be moving in a particular direction and may or may not have the pen down"""
        if self.move_is_valid():
            self.position = self.position.plus(self.v)
            if self.penDown:
                position = (self.position.x, self.position.y)
                self.grid.PenDraw(position)

    def move_is_valid(self):
        """ Checks if moving with the given dt will cause collision with the boundaries of the grid """
        new_loc = self.position.plus(self.v)
        new_loc = (new_loc.x, new_loc.y)
        return self.grid.inbounds(new_loc)

# 需要导入模块: from vector import Vector [as 别名]
# 或者: from vector.Vector import plus [as 别名]
def LaserMonitorCB(data):
    	scans = []
    
   		#adding all the scan data into an array called scans
    	for d in data.ranges:
    		if d > 0:
			scans.append(d)
		angle_min = data.angle_min #angle_min data from LaserScan
		angle_max = data.angle_max #angle_max data from LaserScan
		angle_incr = data.angle_increment #angle_increment data from LaserScan
		forceVectors = []
		    
	i = 0
	for laser in scans: #creates an array of force vectors
		scan_angle = angle_min + (angle_incr*i) #increment the angle so that it is equal to the angle scanned at
		opposing_angle = scan_angle + math.pi #make the angle opposite so that the vector is in the opposing direction of the wall
		i += 1

		if(laser != 0): #avoid divide by 0 error
			force = 1/(laser**2) #inverse square relationship
		else:
			force = 0
		forceVectors.append(Vector(force*math.cos(opposing_angle),force*math.sin(opposing_angle)))
	
		    
	outputVector = Vector(0,0)
	for force in forceVectors: #adds all the forcevectors together to get a resulting vector
			outputVector = outputVector.plus(force)
	outputVector = outputVector.plus(Vector(1,0)) #add a vector pointing forwards to take into account wanting to move forward
	magnitude = math.sqrt((outputVector.x**2)+(outputVector.y**2))
	
	msg = SteeringMessage()
	if(magnitude <50):
		msg.event_name = 'laserclear'
		msg.linear = .1
		msg.angular = 0
	else:
		msg.event_name = 'laserobstacle'
    		msg.linear = .1
		msg.angular = 10*(outputVector.y/magnitude)

 	if outputVector.x == 1.0 and outputVector.y ==0:
		msg.angular = math.pi
		msg.linear = -.5	

	pub = rospy.Publisher('/Steering', SteeringMessage)
    	pub.publish(msg)        

# 需要导入模块: from vector import Vector [as 别名]
# 或者: from vector.Vector import plus [as 别名]
    def test_add(self):
        v = Vector([8.218, -9.341])
        w = Vector([-1.129, 2.111])

        self.assertVecEqual(['7.08899999999999996802557689080', '-7.22999999999999909405801190587'], v.plus(w))
        self.assertEqual(v.plus(w), w.plus(v), "Vector addition is commutative!")

# 需要导入模块: from vector import Vector [as 别名]
# 或者: from vector.Vector import plus [as 别名]
from vector import Vector

# Addition
v1 = Vector([8.218, -9.341])
v2 = Vector([-1.129, 2.111])
print ''
print 'Addition'
print v1.plus(v2)

# Subtraction
v1 = Vector([7.119, 8.215])
v2 = Vector([-8.223, 0.878])
print ''
print 'Subtraction'
print v1.minus(v2)

# Scalar Multiplication
s = 7.41
v = Vector([1.671, -1.012, -0.318])
print ''
print 'Scalar Multiplication'
print v.times_scalar(s)

# Magnitude
print ''
print 'Magnitude'
v = Vector([-0.221, 7.437])
print format(v.magnitude(), '.5f')
v = Vector([8.813, -1.331, -6.247])
print format(v.magnitude(), '.5f')