def send():
motion = ALProxy("ALMotion","127.0.0.1",9559)
rospy.init_node('observations')
headpub = rospy.Publisher('headLoc', Head)
armpub = rospy.Publisher('RArmLoc',Arm)
rarm_angle_pub = rospy.Publisher('RArmAnglesLoc',ArmAngles)
while not rospy.is_shutdown():
x = int((motion.getAngle("HeadYaw")+.7)*256/1.4)
y = int((motion.getAngle("HeadPitch")+.7)*256/1.4)
head = Head()
head.x = x
head.y = y
headpub.publish(head)
stiff = motion.getChainStiffnesses("RArm")
rarm = motion.getChainAngles("RArm")
armAngles = ArmAngles()
armAngles.duration = 0.0
for i in range(len(rarm)):
armAngles.armAngles[i] = rarm[i]
if (stiff[0] < .1):
motion.gotoChainAngles("RArm",rarm,0.2,motion.INTERPOLATION_SMOOTH)
armPos = motion.getPosition("RArm",0)
arm = Arm()
for i in range(len(armPos)):
arm.armPose[i] = armPos[i]
arm.hand = motion.getAngle("RHand")
armpub.publish(arm)
rarm_angle_pub.publish(armAngles)
rospy.sleep(.1)
pass
def send():
motion = ALProxy("ALMotion", "127.0.0.1", 9559)
rospy.init_node('observations')
headpub = rospy.Publisher('headLoc', Head)
armpub = rospy.Publisher('RArmLoc', Arm)
rarm_angle_pub = rospy.Publisher('RArmAnglesLoc', ArmAngles)
while not rospy.is_shutdown():
x = int((motion.getAngle("HeadYaw") + .7) * 256 / 1.4)
y = int((motion.getAngle("HeadPitch") + .7) * 256 / 1.4)
head = Head()
head.x = x
head.y = y
headpub.publish(head)
stiff = motion.getChainStiffnesses("RArm")
rarm = motion.getChainAngles("RArm")
armAngles = ArmAngles()
armAngles.duration = 0.0
for i in range(len(rarm)):
armAngles.armAngles[i] = rarm[i]
if (stiff[0] < .1):
motion.gotoChainAngles("RArm", rarm, 0.2,
motion.INTERPOLATION_SMOOTH)
armPos = motion.getPosition("RArm", 0)
arm = Arm()
for i in range(len(armPos)):
arm.armPose[i] = armPos[i]
arm.hand = motion.getAngle("RHand")
armpub.publish(arm)
rarm_angle_pub.publish(armAngles)
rospy.sleep(.1)
pass
def do_ik(arm): global currPose if(not lock.acquire(0)): return target = arm.armPose if(arm.hand < .3): currArmAngles.armAngles[5] = 0.0 print "closing hand" elif(arm.hand > .7): arm.hand = 1.0 #print "Current Pose" #print currPose tarMat = matrix([[target[0]],[target[1]],[target[2]]]) #print tarMat score = [0.0]*8 for i in xrange(4): a[i] = a[i]+angleStep r1 = matrix([[1,0,0],[0,cos(a[1]),-sin(a[1])],[0,sin(a[1]),cos(a[1])]]) r2 = matrix([[cos(a[0]),0,sin(a[0])],[0,1,0],[-sin(a[0]),0,cos(a[0])]]) r3 = matrix([[1,0,0],[0,cos(a[2]),-sin(a[2])],[0,sin(a[2]),cos(a[2])]]) r4 = matrix([[cos(a[3]),-sin(a[3]),0],[sin(a[3]),cos(a[3]),0],[0,0,1]]) tmp = r3*r4*t2; newPose = r1*r2*(t1+tmp) score[i] = sum(numpy.square(tarMat -newPose)) a[i] = a[i] -angleStep for i in xrange(4): a[i] = a[i]-angleStep r1 = matrix([[1,0,0],[0,cos(a[1]),-sin(a[1])],[0,sin(a[1]),cos(a[1])]]) r2 = matrix([[cos(a[0]),0,sin(a[0])],[0,1,0],[-sin(a[0]),0,cos(a[0])]]) r3 = matrix([[1,0,0],[0,cos(a[2]),-sin(a[2])],[0,sin(a[2]),cos(a[2])]]) r4 = matrix([[cos(a[3]),-sin(a[3]),0],[sin(a[3]),cos(a[3]),0],[0,0,1]]) tmp = r3*r4*t2; newPose = r1*r2*(t1+tmp) score[i+4] = sum(numpy.square(tarMat -newPose)) a[i] = a[i] +angleStep mi = -1 mn = sum(numpy.square(tarMat - currPose)) #print "Current error" #print mn #print "new scores" #print score for i in xrange(4): if (score[i] < mn and a[i]+angleStep < maxAngles[i]): mn = score[i] mi = i pass for i in xrange(4): if (score[i+4] < mn and a[i]-angleStep > minAngles[i]): mn = score[i+4] mi = i+4 pass #print mi if(mi < 0): pass elif(mi < 4): currArmAngles.duration = .1 a[mi] = a[mi] + angleStep currArmAngles.armAngles = a else: currArmAngles.duration = .1 a[mi-4] = a[mi-4] - angleStep currArmAngles.armAngles = a r1 = matrix([[1,0,0],[0,cos(a[1]),-sin(a[1])],[0,sin(a[1]),cos(a[1])]]) r2 = matrix([[cos(a[0]),0,sin(a[0])],[0,1,0],[-sin(a[0]),0,cos(a[0])]]) r3 = matrix([[1,0,0],[0,cos(a[2]),-sin(a[2])],[0,sin(a[2]),cos(a[2])]]) r4 = matrix([[cos(a[3]),-sin(a[3]),0],[sin(a[3]),cos(a[3]),0],[0,0,1]]) tmp = r3*r4*t2; currPose = r1*r2*(t1+tmp) zaxis = r1*r2*r3*r4*t3 wx = atan2(zaxis[2],sqrt(zaxis[1]*zaxis[1]+zaxis[0]*zaxis[0])) if(wx > minAngles[4] and wx < maxAngles[4]): currArmAngles.armAngles[4] = wx arm2 = Arm() for i in xrange(3): arm2.armPose[i] = currPose[i] arm2.hand = currArmAngles.armAngles[5] print arm2.armPose pub2.publish(arm2) if( mi > 0): pub.publish(currArmAngles) rospy.sleep(.1) lock.release()
def do_ik(arm):
global currPose
if (not lock.acquire(0)):
return
target = arm.armPose
if (arm.hand < .3):
currArmAngles.armAngles[5] = 0.0
print "closing hand"
elif (arm.hand > .7):
arm.hand = 1.0
#print "Current Pose"
#print currPose
tarMat = matrix([[target[0]], [target[1]], [target[2]]])
#print tarMat
score = [0.0] * 8
for i in xrange(4):
a[i] = a[i] + angleStep
r1 = matrix([[1, 0, 0], [0, cos(a[1]), -sin(a[1])],
[0, sin(a[1]), cos(a[1])]])
r2 = matrix([[cos(a[0]), 0, sin(a[0])], [0, 1, 0],
[-sin(a[0]), 0, cos(a[0])]])
r3 = matrix([[1, 0, 0], [0, cos(a[2]), -sin(a[2])],
[0, sin(a[2]), cos(a[2])]])
r4 = matrix([[cos(a[3]), -sin(a[3]), 0], [sin(a[3]),
cos(a[3]), 0], [0, 0,
1]])
tmp = r3 * r4 * t2
newPose = r1 * r2 * (t1 + tmp)
score[i] = sum(numpy.square(tarMat - newPose))
a[i] = a[i] - angleStep
for i in xrange(4):
a[i] = a[i] - angleStep
r1 = matrix([[1, 0, 0], [0, cos(a[1]), -sin(a[1])],
[0, sin(a[1]), cos(a[1])]])
r2 = matrix([[cos(a[0]), 0, sin(a[0])], [0, 1, 0],
[-sin(a[0]), 0, cos(a[0])]])
r3 = matrix([[1, 0, 0], [0, cos(a[2]), -sin(a[2])],
[0, sin(a[2]), cos(a[2])]])
r4 = matrix([[cos(a[3]), -sin(a[3]), 0], [sin(a[3]),
cos(a[3]), 0], [0, 0,
1]])
tmp = r3 * r4 * t2
newPose = r1 * r2 * (t1 + tmp)
score[i + 4] = sum(numpy.square(tarMat - newPose))
a[i] = a[i] + angleStep
mi = -1
mn = sum(numpy.square(tarMat - currPose))
#print "Current error"
#print mn
#print "new scores"
#print score
for i in xrange(4):
if (score[i] < mn and a[i] + angleStep < maxAngles[i]):
mn = score[i]
mi = i
pass
for i in xrange(4):
if (score[i + 4] < mn and a[i] - angleStep > minAngles[i]):
mn = score[i + 4]
mi = i + 4
pass
#print mi
if (mi < 0):
pass
elif (mi < 4):
currArmAngles.duration = .1
a[mi] = a[mi] + angleStep
currArmAngles.armAngles = a
else:
currArmAngles.duration = .1
a[mi - 4] = a[mi - 4] - angleStep
currArmAngles.armAngles = a
r1 = matrix([[1, 0, 0], [0, cos(a[1]), -sin(a[1])],
[0, sin(a[1]), cos(a[1])]])
r2 = matrix([[cos(a[0]), 0, sin(a[0])], [0, 1, 0],
[-sin(a[0]), 0, cos(a[0])]])
r3 = matrix([[1, 0, 0], [0, cos(a[2]), -sin(a[2])],
[0, sin(a[2]), cos(a[2])]])
r4 = matrix([[cos(a[3]), -sin(a[3]), 0], [sin(a[3]),
cos(a[3]), 0], [0, 0, 1]])
tmp = r3 * r4 * t2
currPose = r1 * r2 * (t1 + tmp)
zaxis = r1 * r2 * r3 * r4 * t3
wx = atan2(zaxis[2], sqrt(zaxis[1] * zaxis[1] + zaxis[0] * zaxis[0]))
if (wx > minAngles[4] and wx < maxAngles[4]):
currArmAngles.armAngles[4] = wx
arm2 = Arm()
for i in xrange(3):
arm2.armPose[i] = currPose[i]
arm2.hand = currArmAngles.armAngles[5]
print arm2.armPose
pub2.publish(arm2)
if (mi > 0):
pub.publish(currArmAngles)
rospy.sleep(.1)
lock.release()
