Hi all. I'm trying to use moveit API. I get down the code from [moveit github](https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/pr2_tutorials/kinematics/src).Here are the code that I executed.
#include
#include
#include
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&joint_names =joint_model_group->getVariableNames();
std::vector 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?
↧