Ask Your Question
0

skeliton joint angles from openni_tracker

asked 2012-12-30 08:19:39 -0600

DanF gravatar image

updated 2014-01-28 17:14:41 -0600

ngrennan gravatar image

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 <arm_navigation_msgs/MoveArmAction.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 flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2013-03-19 06:39:01 -0600

Rookdroid gravatar image

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.

edit flag offensive delete link more

Comments

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

Stephane.M gravatar imageStephane.M ( 2013-03-19 22:05:49 -0600 )edit
0

answered 2015-07-30 14:55:18 -0600

NievsaMolina gravatar image

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! :)

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2012-12-30 08:19:39 -0600

Seen: 807 times

Last updated: Mar 19 '13