Esempio n. 1
0
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
Esempio n. 2
0
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
Esempio n. 3
0
	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()
Esempio n. 4
0
    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()