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
from NAOcontrol.msg import Arm
from NAOcontrol.msg import ArmAngles
from std_msgs.msg import String
from sys import argv, exit
from math import *
import numpy
from numpy import matrix
import threading
from threading import Thread
if __name__ == '__main__':
angleStep = .1
lock = threading.Lock()
rospy.init_node('naoIk', anonymous=False)
currArmAngles = ArmAngles()
TO_RAD = 3.14 / 180.0
currArmAngles.armAngles = [
80.0 * TO_RAD, -40.0 * TO_RAD, 80.0 * TO_RAD, 80.0 * TO_RAD,
0.0 * TO_RAD, 1.0
]
currArmAngles.duration = 2.0
pub = rospy.Publisher('RArmAngles', ArmAngles)
pub2 = rospy.Publisher('RArmPose', Arm)
minAngles = [-2.094, -1.658, -2.094, 0.0, -1.832, 0.0]
maxAngles = [2.094, 0.0, 2.094, 1.658, 2.617, 1.0]
rospy.sleep(2.0)
pub.publish(currArmAngles)
rospy.sleep(2.0)
currArmAngles.duration = .1
t3 = matrix([[0], [0], [1]])