MoveIt! 中的运动学模型

参考Kinematic Model Tutorial

RobotModel 与 RobotState类

RobotModel 与 RobotState类是访问运动学的核心类。这个例子中我们使用PR2的右臂来走通使用这两个类的过程。

开始设置

初始化一个RobotModelLoader对象，它将在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可以构建一个RobotModel变量，保存机器人的配置信息。我们设机器人各个关节为默认值。可以得到一个JointModelGroup表示机器人的运动组，比如“right_arm”。

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);
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}

关节限制

/* Set one joint in the right arm outside its joint limit */
joint_values[0] = 1.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"));

正向运动学

通过一组随机的关节值实现正向运动学。

kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("r_wrist_roll_link");

/* Print end-effector pose. Remember that this is in the model frame */
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());

逆运动学inverse kinematics (IK)

bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);

Now, we can print out the IK solution (if found):

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");
}

获得雅克比矩阵

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: " << jacobian);

启动文件

启动文件之前要先做两件事：

第一个是将PR2的URDF与SRDF上传到参数服务器

第二个是将kinematics_solver配置文件上传到参数服务器。

<launch>
<include file="$(find pr2_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>

<node name="kinematic_model_tutorial"
pkg="moveit_tutorials"
type="kinematic_model_tutorial"
respawn="false" output="screen">
<rosparam command="load"
file="$(find pr2_moveit_config)/config/kinematics.yaml"/>
</node>
</launch>

运行

roslaunch moveit_tutorials kinematic_model_tutorial.launch

结果