Get joint values

[Flavian Hautbois]

Hi everyone,

Here is the code I've been trying to test:
 

  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
  planning_scene::PlanningScene planning_scene(kinematic_model);
  robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();

  /* Get and print the name of the coordinate frame in which the transforms for this model are computed*/
  ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

  std::vector<double> joint_values; 
  robot_state::JointStateGroup* joint_state_group = current_state.getJointStateGroup("manipulator");
  joint_state_group->getVariableValues(joint_values);

const std::vector<std::string> &joint_names = joint_state_group->getJointModelGroup()->getJointModelNames();

Everything compiles fine, I am 99.99% sure that the model is loaded as "robot_description" and my end effector group is named "manipulator" (one evidence for that is that joint_names actually contains my joint names).
However, I have my robot connected, rviz launched, the simulation robot has the same pose as the real robot, but "joint_values" contains only zeros (which is absolutely not accurate).

I have been following a tutorial [1] (which by the way has a few mistakes, such as the use of robot_model_loader::RDFLoader and robot_state::RobotState& current_state = planning_scene.getCurrentState(); similar working code is available at [2]).

Thanks

Flavian

------------

[1] http://moveit.ros.org/wiki/Environment_Representation/C%2B%2B_API#Change_the_state
[2] https://github.com/ros-planning/moveit_pr2/blob/groovy-devel/pr2_moveit_tutorials/planning/src/planning_scene_tutorial.cpp

[Flavian Hautbois]

I think I understood the problem better but I still don't know how to solve it. Here I am making a new robot model that I load from the configuration file. That gives me a kinematic model that can be accessed using RobotModelPtr. Then the planning scene creates a new internal state that only has the same kinematic tree as my robot. It is used to decouple the real robot from a path planning. 
However I would like to synchronize my duplicate kinematic model with the robot state, so that my planning path's start point be the same as the current robot position. Since the real robot has a robot_state_publisher which publishes a tf, I should be able to retrieve joint values from the tf topic. 

Should I subscribe to the tf topic? Or is there a simpler way to access the robot model published to tf and get joint values from it?

Thanks 

[Ioan Sucan]

Hello Flavian,

The code you wrote is correct, but there is no connection to the data coming from the robot. PlanninScene is some sense a 'passive' data structure. You need to fill it in.

For that, you should use a PlanningSceneMonitor class and call one or more of the various start methods:
http://moveit.ros.org/doxygen/classplanning__scene__monitor_1_1PlanningSceneMonitor.html

planning_scene_monitor::PlanningSceneMonitor psm("robot_description");

psm.startStateMonitor();

sleep(1);
robot_state::RobotState r = psm.getPlanningScene().getCurrentState();

In a previous e-mail I mentioned the CurrentStateMonitor; you can get an instance of that by calling psm.getStateMonitor().

Ioan

[Flavian Hautbois]

Thanks Ian,

Is the "sleep(1)" mandatory?

2013/9/6 Ioan Sucan <isu...@gmail.com>

--
F. Hautbois

[Ioan Sucan]

Not really. You can use CurrentStateMonitor::waitForCurrentState() to wait until all joint values have been filled.

Ioan

[Flavian Hautbois]

What's the philosophy behind the planning_scene object? Is it problematic if there are two instances of a planning_scene?

By the way, I get an
error: ‘const PlanningScenePtr’ has no member named ‘getCurrentState'

2013/9/6 Ioan Sucan <isu...@gmail.com>

--
F. Hautbois

[Ioan Sucan]

You should use -> instead of . for calling getCurrentState().

You can have as many planning scene objects as you like. You can clone them, have one be just a diff of the other, e.g., scene A is same as B but with this object added/removed/moved, etc.

Ioan