How to get Nao robot's joint coordinates?

Question

For my project, a human follows a Nao robot's movements and I have human skeleton joint data in world coordinate as (x,y,z) from a depth sensor. Now I need the robot's joint coordinates to justify human imitation. The depth sensor cannot track the Nao robot.

I have found the robot's sensor positions using the motionProxy.getPosition function. I have also tried finding out the joint names with the deprecated function getJointNames but there is no function for getting the "positions" of these joints.

Should I manually measure the distance between joints and sensors and calculate the joint position? Also, has anyone tried using Vicon or other motion tracking system on a Nao robot?

Answer 1

You can have the position of any joint in cartesian world using ALMotion.getPosition. eg:

ALMotion.getPosition( RShoulderPitch 0 1)
[-0.627288878,-0.0759276226,0.991154313,-0.000606168585,-0.315976441,0.014949495]

ALMotion.getPosition( HeadPitch FRAME_TORSO 1)
[-0.0248374045,-0.000473844644,0.991121054,-0.000527210592,-0.317508608,-0.0489440858]

...