模型获取
1)从ROS参数中找到机器人模型RobotModel
2)实例化操作接口 RobotState
3)获取一个运动组JointModelGroup,建立运动学模型
1)从ROS参数中找到机器人模型
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());
2)实例化操作接口
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
3)获取一个运动组
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("right_arm");
const std::vector<std::string> &joint_names = joint_model_group-><