Ask Your Question
0

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));
  kinematic_state->setToDefaultValues();
  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]);
  }
  ros::shutdown();
  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

Comments

did you solve the problem

omeranar1 gravatar image omeranar1  ( 2022-01-07 03:17:45 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-06-24 04:15:11 -0500

DaveBG gravatar image

I solved this problem by assuming that the call to kinematic_state was failing and replaced with:

  move_group = new moveit::planning_interface::MoveGroupInterface(PLANNING_GROUP);
  joint_model_group = move_group->getCurrentState()->getJointModelGroup(PLANNING_GROUP);
  const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
  std::vector<double> joint_values;

  moveit::core::RobotStatePtr current_state = move_group->getCurrentState();
  current_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]);
  }
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: 2017-11-24 05:48:33 -0500

Seen: 165 times

Last updated: Nov 24 '17