RobotModel 和 RobotState 是核心的两个接口类，用来访问机器人的运动学模型。
RobotModel
- 管理机器人link和joint的关系，从URDF中加载机器人描述，也包括了设置的joint限制。
- 根据SRDF中的定义将link和joint按照规划组planning group分组
RobotState
- 管理机器人状态信息，记录机器人在某个时刻的关节状态 （包括位置，速度，加速度等）。
- 提供用于设置位置和计算路径的辅助方法。
以Panda机械臂为例：
运行程序
案例程序在 https://github.com/ros-planning/moveit_tutorials.git （branch： kinetic-devel ）
roslaunch moveit_tutorials robot_model_and_robot_state_tutorial.launch
结果
运行后可以看到以下结果，注：数值不一定相同，因为是随机的
ros.moveit_tutorials: Model frame: /panda_link0
ros.moveit_tutorials: Joint panda_joint1: 0.000000
ros.moveit_tutorials: Joint panda_joint2: 0.000000
ros.moveit_tutorials: Joint panda_joint3: 0.000000
ros.moveit_tutorials: Joint panda_joint4: 0.000000
ros.moveit_tutorials: Joint panda_joint5: 0.000000
ros.moveit_tutorials: Joint panda_joint6: 0.000000
ros.moveit_tutorials: Joint panda_joint7: 0.000000
ros.moveit_tutorials: Current state is not valid
ros.moveit_tutorials: Current state is valid
ros.moveit_tutorials: Translation:
-0.541498
-0.592805
0.400443
ros.moveit_tutorials: Rotation:
-0.395039 0.600666 -0.695086
0.299981 -0.630807 -0.715607
-0.868306 -0.491205 0.0690048
ros.moveit_tutorials: Joint panda_joint1: -2.407308
ros.moveit_tutorials: Joint panda_joint2: 1.555370
ros.moveit_tutorials: Joint panda_joint3: -2.102171
ros.moveit_tutorials: Joint panda_joint4: -0.011156
ros.moveit_tutorials: Joint panda_joint5: 1.100545
ros.moveit_tutorials: Joint panda_joint6: 3.230793
ros.moveit_tutorials: Joint panda_joint7: -2.651568
ros.moveit_tutorials: Jacobian:
0.592805 -0.0500638 -0.036041 0.366761 -0.0334361 0.128712 -4.33681e-18
-0.541498 -0.0451907 0.0417049 -0.231187 0.0403683 0.00288573 3.46945e-18
0 -0.799172 0.0772022 -0.247151 0.0818336 0.0511662 0
0 0.670056 -0.742222 0.349402 -0.748556 -0.344057 -0.695086
0 -0.74231 -0.669976 -0.367232 -0.662737 0.415389 -0.715607
1 4.89669e-12 0.0154256 0.862009 0.021077 0.842067 0.0690048
完整代码
创建对象
通常不直接手动创建RobotModel对象，我们可以通过高层的封装类的接口得到RobotModel对象的指针。比如通过 RobotModelLoader 中的gerModel()
创建RobotModelLoader对象时传入robot_description参数，会从ROS参数服务器上获取对应的机器人模型描述自动创建RobotModel对象。
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());
用RobotModel创建RobotState对象，设置关节状态为默认值。
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
调用RobotModel的getJointModelGroup(...)可以得到 JointModelGroup对象的指针。JointModelGroup是针对一个规划组的robot model。
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
const std::vector& joint_names = joint_model_group->getVariableNames()
获取关节数值
通过RobotState的setJointGroupPositions(...)可以获取某个model的关节信息
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]);
}
设置关节位置及关节限制
setJointGroupPositions(...)设置关节位置不会管是否满足joint limits，可以通过enforceBounds()迫使满足。
/* Set one joint in the Panda arm outside its joint limit */
joint_values[0] = 5.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
/* Check whether any joint is outside its joint limits */
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
/* Enforce the joint limits for this state and check again*/
kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
正运动学
随机设置关节数值，计算panda_link8的姿态。
kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");
/* Print end-effector pose. Remember that this is in the model frame */
ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");
逆运动学
IK逆求解，传入上一步求出的末端执行器的姿态end_effector_state，设置尝试次数10，超时0.1s，求解成功会把结果存到joint_model_group参数中
std::size_t attempts = 10;
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);
打印结果
if (found_ik)
{
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]);
}
}
else
{
ROS_INFO("Did not find IK solution");
}
雅克比矩阵
从RobotState中获取Jacobian
Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
Eigen::MatrixXd jacobian;
kinematic_state->getJacobian(joint_model_group,
kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
reference_point_position, jacobian);
ROS_INFO_STREAM("Jacobian: \n" << jacobian << "\n");
launch文件
launch文件中包含以下两步：
- 加载URDF和SRDF文件到参数服务器
- 加载运动学求解器kinematics_solver的配置参数（yaml文件）到参数服务器