Moveit运动学模型

模型获取

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->getJointModelNames();

设置关节值

1）获取关节值
2）设置关节限制
1）获取关节值

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

2）设置关节限制

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

正向运动学

获取一个随机的关节状态，然后求解末端执行器的姿态

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());

逆运动学

给一个末端执行器的位姿，求解次数，还有计算超时时间，得出每一个关节的状态

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

获得雅克比矩阵

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

启动文件

启动文件做过两件事
1）上传模型URDF和SRDF到参数服务器
2）放置运动学求解配置（由配置助手生成的）到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运动学模型相关推荐

无人驾驶运动学模型——线性时变模型预测控制的思路推演过程_百叶书的博客-CSDN博客_线性时变模型预测控制转
无人驾驶运动学模型--线性时变模型预测控制的思路推演过程_百叶书的博客-CSDN博客_线性时变模型预测控制
常见移动机器人运动学模型
参考链接: 常见移动机器人运动学模型总结 (qq.com) 两轮差速驱动机器人图 2.1 TurtleBot 3 图 2.2 两轮差速驱动机器人运动模型正运动学模型逆运动学模型原文 <两 ...
常见移动机器人运动学模型总结（图片版）
关注同名微信公众号"混沌无形",有趣好文! 原文链接:常见移动机器人运动学模型总结 (包含原文PDF百度云下载链接) 精彩的理论论证过程见原文链接(含全文下载链接) 由于网页排版效 ...
麦克纳姆轮全向移动机器人运动学模型应用
关注同名微信公众号"混沌无形",有趣好文! 原文链接:https://mp.weixin.qq.com/s/GWhlXsuY6QYyoZydaSYpjQ(包含原文PDF百度云下载链 ...
无人车系统（一）：运动学模型及其线性化
相对无人机与机械臂来说,无人车系统的运动学模型非常简洁.尽管简洁,无人车的运动学模型也是非线性的.应用于具体控制算法时,有必要对原始运动学模型进行变形或线性化.本篇主要介绍无人车的运动学模型,并对原始 ...
Apollo代码学习(二)—车辆运动学模型
Apollo代码学习-车辆运动学模型前言车辆模型单车模型(Bicycle Model) 车辆运动学模型阿克曼转向几何(Ackerman turning geometry) 小结 Apollo( ...
基于运动学模型的轨迹跟踪控制
章四基于运动学模型的轨迹跟踪控制 MPC(4)基于运动学模型的轨迹跟踪控制器设计无人驾驶车辆模型预测控制(龚建伟)第四章基于运动学模型的轨迹跟踪控制(仿真部分) 无人车辆在惯性坐标系中,车辆必须 ...
ROS学习记录（二）阿克曼转向车运动学模型及在gazebo中搭建仿真环境
前言:该篇是ROS学习记录的第二篇,如果还没关注过之前文章的读者,如有需要可以通过下方传送门去看之前的文章: ROS学习记录(一) Plugin插件这两天关注了古月老师的公众号,看到了其中一篇课程推 ...
二自由度车辆的运动学模型和动力学模型
二自由度车辆的运动学模型和动力学模型最近刚接触自动驾驶相关的知识,得知像LQR.MPC这类基于模型的控制器,若想有不错的控制器性能,那么必须有比较精确的被控对象的数学模型.对于车辆这类被控对象的模型 ...