MoveIt RobotState initialisation
In the MoveIt tutorial for the kinematic part, 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<std::string> &joint_names = joint_model_group->getVariableNames();
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
The full code is available from link.
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?