Ejemplo n.º 1
0
class chassisParams():
    #DYNAMIC PARAMETERS(initial setting)
    #Position and orientation of chassis:
    chassisPose = pose((0, 0, 0), (0, 0, 0))

    #STATIC PARAMETERS(should never change)
    #Leg parameters:
    #alpha is the relative angles of rotation for the joints. Coxa is pi/2 off femur, which is 0 off tibia, which is 0 off foot
    numLegs = 6
    alpha = [[pi / 2, 0, 0]] * numLegs

    #Radius is the radius of the joints
    radius = [[.5, 1.5, 2]] * numLegs

    #Displacement is the axial offset, could be the height of a servo horn for example
    #(Be sure to account for left side vs right side)
    displacement = [[0, 0, 0]] * numLegs

    #Angle ranges
    angleRange = [[[-pi / 2, pi / 2], [-pi / 2, pi / 2], [-pi, 0]]] * numLegs

    #The actual leg objects are the only params that make it into the chassis
    legs = [
        leg(alpha[i], radius[i], displacement[i], angleRange[i])
        for i in xrange(numLegs)
    ]

    legAngle = 2 * pi / numLegs
    legRadius = 1.5
    legPose = [
        pose((legRadius * sin(legAngle * i), legRadius * cos(legAngle * i),
              -.5), (legAngle * i, 0, 0)) for i in xrange(numLegs)
    ]
Ejemplo n.º 2
0
def update_lines(num, lines):
  #If we have reached the end of this step
  progress = num/float(updates)
  if num%updates is 0:
    currentPose = c.chassisPose
    (currX,currY,currZ) = currentPose.position
    nextPose = pose((currX+dir[0]*stepSize,currY+dir[1]*stepSize,currZ),(currentPose.yaw+angularVelocity,currentPose.pitch,currentPose.roll))

    targets.targets = np.array([getNextStep(i, c, nextPose, groundZ) if i in [2*j for j in xrange(numLegs/2)] else targets.targets[i] for i in xrange(numLegs)])
  elif num%updates is updates/2:
    currentPose = c.chassisPose
    (currX,currY,currZ) = currentPose.position
    nextPose = pose((currX+dir[0]*stepSize,currY+dir[1]*stepSize,currZ),(currentPose.yaw+angularVelocity,currentPose.pitch,currentPose.roll))

    targets.targets = np.array([getNextStep(i, c, nextPose, groundZ) if i in [2*j+1 for j in xrange(numLegs/2)] else targets.targets[i] for i in xrange(numLegs)])

    

  #Update position
  x = startX + progress*velocity[0]
  y = startY + progress*velocity[1]
  z = startZ

  c.chassisPose = pose((x,y,z),(startPose.yaw+progress*angularVelocity,startPose.pitch,startPose.roll))

  #Get IK
  newThetas = [c.getAngles(targets.targets[i], i) for i in range(numLegs)]

  #Get FK for drawing
  newSegments = c.getChassisSegments(newThetas)
  for line,data in zip(lines,newSegments):
    line.set_data([[data[0][0],data[1][0]],[data[0][1],data[1][1]]])
    line.set_3d_properties([data[0][2],data[1][2]])
Ejemplo n.º 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)
Ejemplo n.º 4
0
while (1):
    best_image = check_matches()
    if (best_image != " "):
        break

# best_image = 'model_images/model_image12.png'

print "task 1: ", time.time() - start_time, "seconds"

# loop for capturing images and estimating the pose of the metallophone
# In this phase a user must move the metallophone according to NAO's guidance
while (1):
    capture_image(robotIP, PORT)
    time.sleep(1.0)
    cx, cy, rot, rotation_vector, translation_vector = pose(
        best_image
    )  # x and y distances between the camera and the center of metallophone in mm

    k = cv2.waitKey(5) & 0xFF
    if k == 27:
        break

    print "cx: ", cx
    print "cy: ", cy
    l_error_y = 0
    r_error_y = 0

    check_temp()  # Always check motors' temperature for safety

    if (abs(cy) >
            175):  # 175mm is a safe max distance (Could be a little bit more)
Ejemplo n.º 5
0
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))

targets = targetHolder(np.array([getNextStep(i, c, nextPose, groundZ) for i in xrange(numLegs)]))

angles = [c.getAngles(targets.targets[i], i) for i in range(numLegs)]
segments = c.getChassisSegments(angles)
lines = [ax.plot([dat[0][0],dat[1][0]],[dat[0][1],dat[1][1]],[dat[0][2],dat[1][2]])[0] for dat in segments]

updates = 20

def update_lines(num, lines):
  #If we have reached the end of this step
  progress = num/float(updates)
  if num%updates is 0:
    currentPose = c.chassisPose
    (currX,currY,currZ) = currentPose.position
Ejemplo n.º 6
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)