In the MoveIt tutorial for the [kinematic part](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/kinematics/src/doc/kinematic_model_tutorial.html), the sample code does the following to get the robot joint position:
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("right_arm");
const std::vector&joint_names = joint_model_group->getVariableNames();
std::vector joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
The full code is available from [link](https://github.com/ros-planning/moveit_tutorials/blob/kinetic-devel/doc/pr2_tutorials/kinematics/src/kinematic_model_tutorial.cpp).
However, this doesn't give the current robot joint position. If I print out the position using `RobotState::printStateInfo`, the position array is always the same no matter how the robot joint positions are.
I also tried `RobotState::update(true)`, but it is the same.
I wonder if the robot state requires any initialisation?
↧