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))
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()
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)
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
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)
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):
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)
