the copyJointGroupPositions function failed to get joint_values

asked 2017-11-24 05:48:33 -0500

tengfei gravatar image

Hi all. I'm trying to use moveit API. I get down the code from moveit github.Here are the code that I executed.

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
  ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

  robot_state::RobotStatePtr kinematic_state( new robot_state::RobotState(kinematic_model));
  const robot_state::JointModelGroup *joint_model_group = kinematic_model->getJointModelGroup("jakaUr");

  const std::vector<std::string> &joint_names =joint_model_group->getVariableNames();

  std::vector<double> joint_values;
  kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);

  for (std::size_t i = 0; i < joint_names.size(); ++i) {
    ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
  return 0;

I roslaunch my robot in rviz and rosrun thenode above but the output is:

[ INFO] [1511520740.005112918, 83778.954000000]: Model frame: /world
[ INFO] [1511520740.005171370, 83778.954000000]: Joint joint_1: 0.000000
[ INFO] [1511520740.005185353, 83778.954000000]: Joint joint_2: 0.000000
[ INFO] [1511520740.005193060, 83778.954000000]: Joint joint_3: 0.000000
[ INFO] [1511520740.005201980, 83778.954000000]: Joint joint_4: 0.000000
[ INFO] [1511520740.005208495, 83778.954000000]: Joint joint_5: 0.000000
[ INFO] [1511520740.005215132, 83778.954000000]: Joint joint_6: 0.000000

No matter how I move the robot in Rviz, the output was always 0 from joint_1 to joint_6. I check the Documentation of the function but there is just a explanation: For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. So why the output is incorrect?

edit retag flag offensive close merge delete