示例#1
0
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import matplotlib.pyplot as plt
from math import pi
from chassis import *

mpl.rcParams['legend.fontsize'] = 10
fig = plt.figure()
ax = fig.gca(projection='3d')

cha = chassis()

segments = cha.getChassisSegments()

X = []
Y = []
Z = []

for seg in segments:
  X.append(seg[1][0])
  Y.append(seg[1][1])
  Z.append(seg[1][2])

def plotSegments(ax, segments):
  for seg in segments:
    ax.plot([seg[0][0], seg[1][0]],[seg[0][1],seg[1][1]],[seg[0][2],seg[1][2]])

def plotBoundingBox(ax, X, Y, Z):
  max_range = max([max(X)-min(X), max(Y)-min(Y), max(Z)-min(Z)])
  Xb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][0].flatten() + 0.5*(max(X)+min(X))
示例#2
0
def car_control():
    start_time = time.time()
    x = 1
    counter = 0
    num=0

    a = chassis()

    cap=cv2.VideoCapture(1)
    while(1):  
        ret, frame = cap.read()  
        height, width, _ = frame.shape
        j = 0
        i = 0
        pt1 = []
        pt2 = []
        area=0
        max_x=0
        max_y=0
        max_area=0    
    
        result = tfnet.return_predict(frame)
 
        for j in range(len(result)):
            if(result[j]['label'] == 'person'):
                pt1.append((result[j]['topleft']['x'],result[j]['topleft']['y']))
                pt2.append((result[j]['bottomright']['x'],result[j]['bottomright']['y']))
       
        for i in range(len(pt1)):
            cv2.rectangle(frame,pt1[i],pt2[i],(255,255,255),4)
            cv2.putText(frame, 'person', pt1[i], cv2.FONT_HERSHEY_SIMPLEX,0.8, (0,255,0),2)
            area=(pt1[i][1]-pt2[i][1])*(pt2[i][0]-pt2[i][0])
            if(area>=max_area):
                max_x=(pt1[i][0]+pt2[i][0])/2
                max_y=(pt1[i][1]+pt2[i][1])/2
                max_area=area
        counter += 1
        
        if (time.time() - start_time) > x:
            cv2.putText(frame, 'FPS {0}'.format(str(counter / (time.time() - start_time))), (20,20),cv2.FONT_HERSHEY_SIMPLEX,0.5, (0,255,0),1)
            counter = 0
            start_time = time.time()
        
        ret, jpeg = cv2.imencode('.jpg', frame)
        global scene
        scene = jpeg.tobytes()
        
        if max_x==0:
            num=num+1
        else: num=0
        if num==0:
            if max_area>60000:
                a.moveStepBackward(0.1)    
            elif max_x > 0.3 * width and max_x < 0.7* width:
                a.moveStepForward(0.2)
            elif max_x<0.3*width:
                a.moveStepLeft(0.0007*(0.3*width-max_x))
                a.moveStepForward(0.1)
            elif max_x>0.7*width:
                a.moveStepRight(0.0007*(max_x-0.7*width))
                a.moveStepForward(0.1)
        elif num!=0:
                a.moveStop()
示例#3
0
from chassis import *
from chassisParams import chassisParams
from tripod import *
from pose import pose
from math import *

cp = chassisParams()
c = chassis(cp)

start = pose((0, 0, 1.5), (0, 0, 0))
end = pose((0, 0, 1.5), (pi / 2, 0, 0))
c.chassisPose = start

print getNextStep(0, c, end, 0)
#print getNextStep(1, c, end, 0)
#print getNextStep(2, c, end, 0)
#print getNextStep(3, c, end, 0)
#print getNextStep(4, c, end, 0)
#print getNextStep(5, c, end, 0)
示例#4
0
class animated_model():

    #Parameters
    groundZ = -2.5
    tripod = ((0, 2, 4), (1, 3, 5))
    ripple = ((3, ), (0, ), (4, ), (2, ), (5, ), (1, ))
    tripple = ((0, ), (4, ), (2, ), (), (), (), (5, ), (1, ), (3, ), (), (), ()
               )  #Time stretched tripod
    wave = ((0, ), (1, ), (2, ), (3, ), (4, ), (5, ))  #Shitty gait

    maxStepSize = 2.  #adjust meeee
    minStepSize = .001  #adjust meeee tooooo
    gait = tripod

    #Local state
    cp = chassisParams()
    c = chassis(cp)
    numLegs = len(c.legs)
    targets = [
        getNextStep(i, c, c.chassisPose, groundZ) for i in xrange(numLegs)
    ]

    gaitStep = 0
    stepProgress = 0

    def step(self):
        self.c.updatePose((self.velocity[0], self.velocity[1], 0),
                          self.angularVelocity, 0, 0)

        if self.stepProgress > 1:
            legsToMove = self.gait[self.gaitStep]
            oldPose = self.c.chassisPose
            newPose = pose((oldPose.position[0] + self.dir[0] * self.stepSize,
                            oldPose.position[1] + self.dir[1] * self.stepSize,
                            oldPose.position[2]),
                           (oldPose.yaw + self.angularVelocity * self.gaitTime,
                            oldPose.pitch, oldPose.roll))

            for i in legsToMove:
                self.targets[i] = getNextStep(i, self.c, newPose, self.groundZ)

            self.gaitStep += 1
            self.stepProgress = 0
            self.gaitStep %= self.numBins

        else:
            self.stepProgress += 1. / self.gaitBinTime

        newThetas = [
            self.c.getAngles(self.targets[i], i) for i in xrange(self.numLegs)
        ]
        return self.c.getChassisSegments(newThetas)

    def updateVelocity(self, velocity, angularVelocity):
        self.velocity = velocity
        self.angularVelocity = angularVelocity
        self.speed = sqrt(velocity[0]**2 + velocity[1]**2)
        if self.speed == 0:
            self.dir = (0, 0)
        else:
            self.dir = (velocity[0] / self.speed, velocity[1] / self.speed)

        self.numBins = len(self.gait)
        self.gaitBinTime = 20 / self.numBins  #Initial gaitBinTime determines all free parameters. Does change.
        self.stepSize = self.speed * self.numBins * self.gaitBinTime

        #Adjust the stepRate and stepSize if stepSize is out of limits
        if self.stepSize > self.maxStepSize:
            stepRate = self.speed / self.maxStepSize
            self.gaitBinTime = max(1, floor(1. / (stepRate * self.numBins)))
            self.stepSize = self.speed * self.gaitBinTime * self.numBins

        elif self.stepSize < self.minStepSize:
            stepRate = self.speed / self.minStepSize
            self.gaitBinTime = max(1, floor(1. / (stepRate * self.numBins)))
            self.stepSize = self.speed * self.gaitBinTime * self.numBins

        self.gaitTime = self.numBins * self.gaitBinTime
示例#5
0
class animatedChassis():
  updatesPerSecond = 60
  updatePeriod = 1.0/updatesPerSecond
  cont = controller.controller()

  #For keeping track of update times
  lastUpdateTime = 0

  cp = chassisParams()
  c = chassis(cp)

  #Last actual step:
  lastStepPose = c.chassisPose

  stepRate = 5
  stepSize = 1

  #Reset timers and vars for the beginning of the next step
  def initNextStep(self):
  #Feet to pick up
  pass

  



  #Main Update Loop
  def update(self, time):
    if time-self.lastUpdateTime < updatePeriod: return

    #We are updating this time
    self.lastUpdateTime = time
    controlVector = cont.getControl()
    oldPose = self.c.chassisPose

    newPose = 


  #Calculates the position on a cycloid given the start and end positions
  # the progress of the step [0,1] and the height of the cycloid
  def interpolateStep(startPos, endPos, progress, height):
    #X
    xRadius = (startPos[0]-endPos[0])/2
    xMid = startPos[0]+xRadius
    X = xMid - xRadius*cos(progress*pi)

    #Y
    yRadius = (startPos[1]-endPos[1])/2
    yMid = startPos[1]+yRadius
    Y = yMid - yRadius*cos(progress*pi)

    #Z
    Z = (height/2)*(1-cos(progress*2*pi))+(endPos[2]-startPos[2])*progress

    return (X,Y,Z)

#Get intersection angle of Dyn with Stat
#Dyn is based at origin
#Stat is based on positive X axis
#Angle 0 is down the x axis
def getIntersectionAngle(baseDist, statAng, statWidth, dynWidth, statLen, dynLen):
  if (dynWidth + statWidth) > baseDist:
    print "Legs too wide for base positioning"
    return None

  statEnd = (baseDist + statLen*cos(statAng) + statWidth*cos(statAng+pi/2), statLen*sin(statAng) + statWidth*sin(statAng+pi/2))
  dynRadiusEff = sqrt(dynLen**2+dynWidth**2) #Effective radius of dynamic leg

  statVector = np.array([statWidth, cos(statAng-pi/2)*statLen, sin(statAng-pi/2)*statLen])
  dynEnd = (dynRadiusEff*cos(dynAng),dynRadiusEff*sin(dynAng))
  #This uses the dot product as in perceptron algorithm to get distance between and offset line and a point
  #Solve this for dynAng
  dist = -statWidth + statVector[1]*dynRadiusEff*cos(dynAng) + statVector[2]*dynRadiusEff*sin(dynAng)
示例#6
0
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation

from math import pi
from chassis import *
from random import random

mpl.rcParams['legend.fontsize'] = 10
fig = plt.figure()
ax = fig.gca(projection='3d')

c = chassis()

#target = [[random()*3-1.5,random()*3-1.5,random()-3] for i in range(6)]
target = [[random()*3-1.5,random()*3-1.5,random()-3]]*6
angles = [c.getAngles(target[i], i) for i in range(6)]
c.theta = angles
segments = c.getChassisSegments()

X = []
Y = []
Z = []

for seg in segments:
  X.append(seg[1][0])
  Y.append(seg[1][1])
  Z.append(seg[1][2])
from math import pi,cos,sin,sqrt
from random import random

from chassis import *
from chassisParams import chassisParams
from pose import pose
from tripod import *
from targetHolder import *


fig = plt.figure()
ax = p3.Axes3D(fig)

cp = chassisParams()
c = chassis(cp)
numLegs = len(c.legs)
groundZ = -1.5


velocity = (.5,.5)
speed = sqrt(sum(velocity))
if speed == 0:
  dir = (0,0)
else:
  dir = (velocity[0]/speed,velocity[1]/speed)
angularVelocity = 3
stepSize = .5
startPose = c.chassisPose
(startX,startY,startZ) = startPose.position
nextPose = pose((startX+dir[0]*stepSize,startY+dir[1]*stepSize,startZ),(startPose.yaw+angularVelocity,startPose.pitch,startPose.roll))
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import matplotlib.pyplot as plt
from math import pi
from chassis import *

mpl.rcParams["legend.fontsize"] = 10
fig = plt.figure()
ax = fig.gca(projection="3d")

cha = chassis()

segments = cha.getChassisSegments()

X = []
Y = []
Z = []

for seg in segments:
    X.append(seg[1][0])
    Y.append(seg[1][1])
    Z.append(seg[1][2])


def plotSegments(ax, segments):
    for seg in segments:
        ax.plot([seg[0][0], seg[1][0]], [seg[0][1], seg[1][1]], [seg[0][2], seg[1][2]])


def plotBoundingBox(ax, X, Y, Z):
示例#9
0
class animatedChassis():

    #Parameters
    groundZ = -0.5
    velocity = (.110, .00)
    angularVelocity = 0.00
    tripod = ((0, 2, 4), (1, 3, 5))
    ripple = ((3, ), (0, ), (4, ), (2, ), (5, ), (1, ))
    tripple = ((0, ), (4, ), (2, ), (), (), (), (5, ), (1, ), (3, ), (), (), ()
               )  #Time stretched tripod
    wave = ((0, ), (1, ), (2, ), (3, ), (4, ), (5, ))  #Shitty gait

    maxStepSize = 1  #adjust meeee
    minStepSize = 0  #adjust meeee tooooo
    gait = tripod

    #Local state
    cp = chassisParams()
    c = chassis(cp)
    numLegs = len(c.legs)
    time = 0

    #Init procedure
    speed = sqrt(velocity[0]**2 + velocity[1]**2)
    if speed == 0:
        dir = (0, 0)
    else:
        dir = (velocity[0] / speed, velocity[1] / speed)

    numBins = len(gait)
    gaitBinTime = 10  #Initial gaitBinTime determines all free parameters. Does change.
    stepSize = speed * numBins * gaitBinTime
    print stepSize

    #Adjust the stepRate and stepSize if stepSize is out of limits
    if stepSize > maxStepSize:
        stepRate = speed / maxStepSize
        gaitBinTime = ceil(1. / (stepRate * numBins))
        stepSize = speed * gaitBinTime * numBins
    elif stepSize < minStepSize:
        stepRate = speed / minStepSize
        gaitBinTime = ceil(1. / (stepRate * numBins))
        stepSize = speed * gaitBinTime * numBins

    gaitTime = numBins * gaitBinTime

    targets = [
        getNextStep(i, c, c.chassisPose, groundZ) for i in xrange(numLegs)
    ]

    def step(self):
        self.time += 1
        self.c.updatePose((self.velocity[0], self.velocity[1], 0),
                          self.angularVelocity, 0, 0)

        currentStep = self.time % self.gaitTime
        if currentStep % self.gaitBinTime == 0:
            currentBin = int(currentStep) / int(self.gaitBinTime)
            legsToMove = self.gait[currentBin]
            oldPose = self.c.chassisPose
            newPose = pose((oldPose.position[0] + self.dir[0] * self.stepSize,
                            oldPose.position[1] + self.dir[1] * self.stepSize,
                            oldPose.position[2]),
                           (oldPose.yaw + self.angularVelocity * self.gaitTime,
                            oldPose.pitch, oldPose.roll))

            for i in legsToMove:
                self.targets[i] = getNextStep(i, self.c, newPose, self.groundZ)

        newThetas = [
            self.c.getAngles(self.targets[i], i) for i in xrange(self.numLegs)
        ]
        return self.c.getChassisSegments(newThetas)