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
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]])
t2 = matrix([[0.11],[0],[0]])
t1 = matrix([[0.09],[0],[0]])
a = currArmAngles.armAngles;
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])]])
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]])
t2 = matrix([[0.11], [0], [0]])
t1 = matrix([[0.09], [0], [0]])
a = currArmAngles.armAngles
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],