在这里插入图片描述

RobotModel和RobotState类

该RobotModel和RobotState类是核心类,让你获得了机器人的运动。

该RobotModel类包含的各个环节和关节,包括从URDF加载它们的联合极限性质之间的关系。RobotModel还将机器人的链接和关节分离为SRDF中定义的计划组。可以在此处找到有关URDF和SRDF的单独教程:URDF和SRDF教程

所述RobotState包含在某一时间点有关机器人的信息,存储的关节位置和速度和加速度的向量。此信息可用于获取有关机器人的运动信息,该信息取决于机器人的当前状态,例如末端执行器的雅可比行列式。

RobotState还包含帮助程序功能,用于根据末端执行器位置(笛卡尔姿态)设置手臂位置并计算笛卡尔轨迹。

在此示例中,我们将逐步介绍如何将这些类与Panda机械臂一起使用。

运行代码

本教程中的所有代码都可以从moveit_tutorials MoveIt设置中包含的程序包中进行编译和运行 。

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.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的共享指针。您应该尽可能使用它。在此示例中,我们将从此类共享指针开始,仅讨论基本API。您可以查看这些类的实际代码API,以获取有关如何使用这些类提供的更多功能的更多信息。

我们将从实例化 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();

获得关节价值

我们可以检索存储在状态中的熊猫关节当前关节值集。

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()本身不会强制执行联合限制,但是调用forceBounds()可以做到这一点。

/* 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”的姿势,它是机器人“ panda_arm”组中最远端的链接。

kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Isometry3d& 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)。要解决IK,我们将需要以下内容:

末端执行器的所需姿势(默认情况下,这是“ panda_arm”链中的最后一个链接):我们在上一步中计算出的end_effector_state。
超时:0.1 s

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

获取雅可比

我们还可以从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文件:
将panda URDF和SRDF加载到参数服务器上,并将由MoveIt Setup Assistant生成的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>

调试机器人状态

为了帮助调试机械手状态,命令行工具将帮助您对系统进行自检:

rosrun moveit_ros_planning moveit_print_planning_model_info
Logo

北京人形旗下天工造物具身智能开源社区,聚焦具身天工与慧思开物两大平台

更多推荐