RobotModel和RobotState类是访问机器人运动学的核心类。
RobotModel类包含所有链接和关节之间的关系，包括它们从 URDF 加载的关节限制属性。RobotModel 还将机器人的链接和关节分成在 SRDF 中定义的规划组。
RobotState包含有关机器人在某个时间点的信息，存储关节位置的向量以及可选的速度和加速度。此信息可用于获取有关机器人的运动学信息，该信息取决于其当前状态，例如末端执行器的雅可比行列式。RobotState 还包含用于根据末端执行器位置（笛卡尔姿势）设置手臂位置和计算笛卡尔轨迹的辅助函数。
Roslaunch 启动文件以直接从 moveit_tutorials 运行代码：
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.400443ros.moveit_tutorials: Rotation:
-0.395039 0.600666 -0.695086
0.299981 -0.630807 -0.715607
-0.868306 -0.491205 0.0690048ros.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
1.我们将首先实例化一个 RobotModelLoader 对象，该对象将在 ROS 参数服务器上查找机器人描述并构造一个 RobotModel供我们使用。
代码如下（示例）：
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
使用RobotModel，我们可以构造一个 RobotState来维护机器人的配置。我们将状态下的所有关节设置为其默认值。然后我们可以得到一个 JointModelGroup，它代表特定组的机器人模型，例如Panda机器人的”panda_arm”。
moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
检索存储在 Panda 手臂状态中的当前关节值集。
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.联合限制
etJointGroupPositions() 本身不会强制执行联合限制，但调用 enforceBounds() 会执行此操作
joint_values[0] = 5.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
3.正向运动学
现在，可以计算一组随机关节值的正向运动学。请注意，我们希望找到”panda_link8″的位姿，它是机器人”panda_arm”组中最远端的链接。
kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");
ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");
4.逆运动学
现在可以求解 Panda 机器人的逆运动学 (IK)。要解决 IK，我们将需要以下内容：
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, timeout);
现在，我们可以打印出 IK 解决方案（如果找到）：
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");
}
5.获取雅可比行列式
我们还可以从RobotState获取雅可比行列式。
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");
要运行代码，您需要一个启动文件，它可以做两件事：
将 Panda URDF 和 SRDF 加载到参数服务器上，然后将 MoveIt 设置助手生成的 kinematics_solver 配置放到实例化本教程中的类的节点命名空间中的 ROS 参数服务器上。
<launch>
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
include>
<node name="robot_model_and_robot_state_tutorial"
pkg="moveit_tutorials"
type="robot_model_and_robot_state_tutorial"
respawn="false" output="screen">
<rosparam command="load"
file="$(find panda_moveit_config)/config/kinematics.yaml"/>
node>
launch>
Original: https://blog.csdn.net/qq_27545821/article/details/123037585
Author: 菜鸟来求学
Title: 机器人模型和机器人状态
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/679618/
转载文章受原作者版权保护。转载请注明原作者出处！