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)
]
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]])
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)
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)
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
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)