Question

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 joint1 to joint6. 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?

Asked by tengfei on 2017-11-24 06:48:33 UTC

Answer 1

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]);
  }

Asked by DaveBG on 2022-06-24 04:15:11 UTC

the copyJointGroupPositions function failed to get joint_values

Comments

Answers

Comments