MoveIt!入门教程-运动学模型(Kinematic Model)
MoveIt!入门教程-运动学模型(Kinematic Model)
说明
- 通过 C++ API来应用kinematics
RobotModel和RobotState类
- 核心类为RobotModel和RobotState
- 利用PR2的右臂来演示类的使用
使用
- 开始
设置开始使用RobotModel类是很容易的。在一般情况下，你会发现，最高级的组件将返回一个共享指针给RobotModel类。你应该总是在可能的情况下使用它。在这个示例中，我们将从这样一个共享指针开始，只讨论基本的。您可以查看这些类的实际代码类以获取更多关于如何使用这些类功能的更多信息。
我们开始实例化RobotModelLoader对象，它从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());
- 使用 RobotModel, 我们可构建RobotState维护机器人的配置。我们在状态中设置所有关节为默认值。我们能获得一个 JointModelGroup, 它显示机器人模型在特定组，比如： PR2机器人“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->getJointModelNames();
- 获得关节值
- 我们可以获取保持在状态中右臂的关节的设置值
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]);
}
- 关节限制
- setJointGroupPositions()函数不能强制自己设置关节限制，但可以通过调用enforceBounds()来设置
/* 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"));
- 正向运动学
- 现在，我们可以计算出一组随机关节值的正向运动学。
- 请注意，我们要找到机器人右臂("right_arm")最远端的连接“r_wrist_roll_link”的姿态。
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());
- 逆运动学
- 我们现在可以在PR2机器人的右臂解决逆运动学（IK）。为了解决IK，我们需要以下：
- 末端执行器所需的姿势（默认情况下，这是“right_arm”链最后一个环节）
- end_effector_state为我们上一步计算得到的。
- 试图解决IK的数量：5
- 每个尝试超时：0.1秒
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
- 如果找到，打印出解决方案
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");
}
- 获得Jacobian
- 我们可以从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: " << jacobian);
- 完整代码
- 这里可获得完整代码
- 编译代码
- launch文件
- 运行代码，需要这个launch文件做两样事情：
- 上传PR2 URDF和SRDF到参数服务器
- 放置运动学求解配置（由配置助手生成的）到ROS参数服务器对应节点的命名空间下，这个节点由是教程里实例化的类。
<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>
- 教程里的代码可以从moveit_tutorials包找到，可以进行编译和运行。
- 运行代码
roslaunch moveit_tutorials kinematic_model_tutorial.launch
- 期待的输出
- 输出效果如下，因为使用随机节点，数字会不一样。
[ INFO] [1384819451.749126980]: Model frame: /odom_combined
[ INFO] [1384819451.749228320]: Joint r_shoulder_pan_joint: 0.000000
[ INFO] [1384819451.749268059]: Joint r_shoulder_lift_joint: 0.000000
[ INFO] [1384819451.749298929]: Joint r_upper_arm_roll_joint: 0.000000
[ INFO] [1384819451.749337412]: Joint r_upper_arm_joint: -1.135650
[ INFO] [1384819451.749376593]: Joint r_elbow_flex_joint: 0.000000
[ INFO] [1384819451.749404669]: Joint r_forearm_roll_joint: -1.050000
[ INFO] [1384819451.749480167]: Joint r_forearm_joint: 0.000000
[ INFO] [1384819451.749545399]: Joint r_wrist_flex_joint: 0.000000
[ INFO] [1384819451.749577945]: Joint r_wrist_roll_joint: 0.000000
[ INFO] [1384819451.749660707]: Current state is not valid
[ INFO] [1384819451.749723425]: Current state is valid
[ INFO] [1384819451.750553768]: Translation: 0.0464629
-0.660372
1.08426
[ INFO] [1384819451.750715730]: Rotation: -0.0900434 -0.945724 -0.312248
-0.91467 -0.0455189 0.40163
-0.394045 0.321768 -0.860926
[ INFO] [1384819451.751673044]: Joint r_shoulder_pan_joint: -1.306166
[ INFO] [1384819451.751729266]: Joint r_shoulder_lift_joint: 0.502776
[ INFO] [1384819451.751791355]: Joint r_upper_arm_roll_joint: 0.103366
[ INFO] [1384819451.751853793]: Joint r_upper_arm_joint: -1.975539
[ INFO] [1384819451.751907012]: Joint r_elbow_flex_joint: 2.805000
[ INFO] [1384819451.751963863]: Joint r_forearm_roll_joint: -1.851939
[ INFO] [1384819451.752015895]: Joint r_forearm_joint: -0.414720
[ INFO] [1384819451.752064923]: Joint r_wrist_flex_joint: 0.000000
[ INFO] [1384819451.752117933]: Joint r_wrist_roll_joint: 0.000000
[ INFO] [1384819451.753252155]: Jacobian: 0.472372 0.0327816 -0.28711 0.0708816 1.38778e-17 0 0
0.0964629 -0.120972 -0.0626032 -0.311436 0 0 0
0 -0.381159 -0.0266776 -0.0320097 8.67362e-19 0 0
0 0.965189 0.229185 0.973042 -0.0665617 -0.991369 -0.0900434
0 0.261552 -0.845745 0.212168 -0.116995 0.12017 -0.91467
1 0 -0.48186 0.0904127 0.990899 -0.0524048 -0.394045
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号