skeliton joint angles from openni_tracker

I'm trying to read human joint angles using tf messages posted by openni_tracker. Tracker posts tf messages and it seems these are normally read with a tf::TransformListener object. I got this part to work, and I am receiving the transforms, but I have a few problems. (Has anyone done this already? Is there a stack/package or code?)

1): The TransformListener.lookupTransform() method takes two strings for the frames to get the transform between. I've been using things like "/right_shoulder_1" to indicate the right shoulder of the first skeleton the tracker sees. The problem is that if the tracker looses the first person, all these frames are deleted and it starts using "/right_shoulder_2" for the next person. How can I just get the transforms for whatever skeleton is currently being tracked?

2) To get the angle between limbs, I've basically been trying to get the 3D vectors for each joint, compute the vectors between them representing limbs between two joints (taking the difference between joint vectors), and calculating the angle between these limb vectors. Doing this through the TransformListner seems very inefficient - I don;t need any information other than the relative vector positions of the joints. Is there a better way?

Thanks!

Here's the code I have so far for just printing the elbow angle. (Doesn't work, seems to always print the same angle of 1.546 (~88.6 degrees))

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include "std_msgs/String.h"
#include <tf/transform_listener.h>

#define my_pi 3.141592653589793 // Define the PI number for vector calculations

void getTransform(tf::StampedTransform t, tf::TransformListener *l, const std::string &from, const std::string &to){
try{
(*l).lookupTransform(   from,           //from frame
to,     //to frame
ros::Time(0),       //latest
t);     //object to store result in
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
}

void printTransform(tf::StampedTransform t){
//output angle
printf("%lf", t.getOrigin().x());
printf("\t");
printf("%lf", t.getOrigin().y());
printf("\t");
printf("%lf", t.getOrigin().z());
printf("\n");
}

int main(int argc, char **argv){

ros::init(argc, argv, "kinect_control");

ros::NodeHandle node;

tf::TransformListener listener;

double elbowAngle=0.0;

//main loop
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform rShoulderToRElbow;
getTransform(rShoulderToRElbow,&listener,"/right_shoulder_2","/right_elbow_2");

tf::StampedTransform rElbowToRHand;
getTransform(rShoulderToRElbow,&listener,"/right_elbow_2","/right_hand_2");

//math
elbowAngle=(rShoulderToRElbow.getOrigin().angle(rElbowToRHand.getOrigin()));

printf("%lf", elbowAngle);
printf("\n");

rate.sleep();
}

//ros::spin();

return 0;
}

edit retag close merge delete

Sort by » oldest newest most voted

There are two methods I can think of that you could follow. The first is you modify the Openni_tracker package and output the users presently being published and the second is to use the getstringframes() function from the transform listener class and see which users are being published.

more

I agree with both methods, but I would recommend the second one :-)

( 2013-03-19 22:05:49 -0500 )edit

Hello! I'm trying to do something similar: to calculate the joint angles of hip, knee and ankle. I've got the image in rviz from openni_tracker and I can see the coordinates, but I want to receive them to use them as an entry of my package (a new one that calculates de angles). I'm new with ROS and I really need help here... I will appreciate any help! :)

more