先热个身，来个简单的random_pose例程
例程源码get_random_pose.cpp：
（本来想像tutorial代码中那样显示target_pose坐标，但是发现不行，暂时没找到函数，先这样吧）
//需要的头文件
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <rviz_visual_tools/rviz_visual_tools.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <Eigen/Geometry>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "get_random_pose");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
move_group.setRandomTarget();
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
visual_tools.deleteAllMarkers();
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (random pose) %s", success ? "" : "FAILED");
Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z() = 0.65;
//随机的坐标无法展示，终端显示找不到“gripper_frmae”的位置
// geometry_msgs::PoseStamped target_pose;
// target_pose = move_group.getPoseTarget("gripper_frame");
//展示坐标和轨迹线，随机的坐标怎么展示？？？
// visual_tools.publishAxisLabeled(target_pose.pose, "random_pose");
visual_tools.publishText(text_pose, "GetRandomPose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
//move_group.move();//待定
ros::waitForShutdown();
return 0;
}
图示
因为是get random pose，因此在运行例程时会存在目标不可行的情况，终端会出现RRTConnect: Unable to sample any valid states for goal tree，多运行几次例程即可。机器人模型是自建的某宝机器人模型，建模过程可见前面的博文。
例程解析
头文件（貌似最后一行可以不加），遵循用哪些包写哪些头文件的原则。
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <rviz_visual_tools/rviz_visual_tools.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <Eigen/Geometry>
节点初始化，然后开线程并启动线程。注意初始化节点名称需要与程序名称一样。
ros::init(argc, argv, "get_random_pose");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
前面博文Move Group C++ Interface 源码解析中已经写了部分源码的解析。下面代码的顺序是，设置例程中的规划组，声明对象planning_scence_interface，定义一个robot_state::JointModelGroup类型的指针joint_model_group，包含规划组的信息，调用setRandomTarget()将相应的action发送给move_group节点。
static const std::string PLANNING_GROUP = "arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
move_group.setRandomTarget();
MoveItVisualTools包提供了许多功能，可用于可视化RViz中的对象，机器人和轨迹以及调试工具。
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
visual_tools.deleteAllMarkers();
这里的代码主要是为轨迹可视化做准备，plan类的主要作用就是展示运动规划。当规划成功，则在终端显示相应信息。
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) ==
moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (random pose) %s", success ? "" : "FAILED");
下面是标题和轨迹可视化的步骤。
Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z() = 0.65;
//随机的坐标无法展示，终端显示找不到“gripper_frmae”的位置
// geometry_msgs::PoseStamped target_pose;
// target_pose = move_group.getPoseTarget("gripper_frame");
//展示坐标和轨迹线，随机的坐标怎么展示？？？
// visual_tools.publishAxisLabeled(target_pose.pose, "random_pose");
visual_tools.publishText(text_pose, "GetRandomPose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger();
RobotState And RobotModel
先放一波官方文档的解释：
RobotModel类和RobotState是类访问机器人运动学状态的核心部分。
RobotModel类包含所有link和joint之间的关系，包括从URDF加载的关节限制属性。 RobotModel还将机器人的link和joint分离为SRDF中定义的规划组。
RobotState类包含有关机器人在snapshot in time（这个不知道怎么翻译，快照时间下？反正是瞬时状态吧，hahaha）的信息，存储关节位置的矢量以及可选的速度和加速度，可用于获取有关当前状态下机器人的运动信息，例如末端执行器的雅可比矩阵。RobotState还包含辅助函数，用于根据末端执行器位置（笛卡尔姿势）设置手臂位置以及计算笛卡尔轨迹。
例程源码
//#include <ros/ros.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <rviz_visual_tools/rviz_visual_tools.h>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "RobotModelAndRobotState");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface move_group("arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
//RobotModelPtr，robot_model指针
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());
//RobotStatePtr,robot_state指针
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("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 (int i = 0; i < joint_names.size(); i++){
ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
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"));
kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("gripper_frame");
ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
visual_tools.deleteAllMarkers();
std::size_t attempts = 10;
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);
if (found_ik){
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
move_group.setJointValueTarget(joint_values);
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan %s", success ? "" : "FAILED");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
for (int 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: \n" << jacobian << "\n");
ros::shutdown();
return 0;
}
图示
可以看出终端输出如下结果：
例程解析
初始化
首先实例化一个RobotModelLoader对象，该对象将在ROS参数服务器上查找机器人描述并构造一个RobotModel供我们使用。
定义一个指向RobotModel的指针kinematic_model，后面用处很大。
getModelFrame()，顾名思义，获取计算此模型的变换的帧（使用RobotState时）。此框架取决于根关节。因此，帧是从SRDF中提取的，或者假设它是根link的名称，一般根link为base_link。
//设置规划组
moveit::planning_interface::MoveGroupInterface move_group("arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
//RobotModelPtr，robot_model指针
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());
定义一个指向RobotState的指针kinematic_state。
将机器人的状态都设为默认值，根据setToDefaultValues()定义，默认位置为0或者是最大界限和最小界限之间。
定义一个robot_state::JointModelGroup类型的指针joint_model_group，包含规划组arm的信息。
根据getVariableNames，按照它们存储在内存中的顺序获取构成此状态的变量的名称，并将其存放在可变数组joint_name中。
这里进行的一切操作均没有和move_group进行通讯，因此机器人不会动。
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("arm");
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
获取每个关节值
根据copyJointGroupPositions()，按照在规划组中变量的顺序，将当前状态下规划组中的各个关节的位置值复制到另一个地方。注意，这不一定是RobotState本身的连续内存块，因此我们复制而不是返回指针。
然后输出每个关节的名称及其当前状态下的位置值。
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for (int i = 0; i < joint_names.size(); i++){
ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
关节限制
setJointGroupPositions()，更新规划组中各关节的位置，但这个函数不会强制执行关节限制。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"));
正向运动学
根据setToRandomPositions()，目前将规划组中的各个关节值设置为随机值。
然后计算末端执行器（当前规划组中最末端，本文为gripper_frame）在随机关节值状态下的正向运动学齐次变换矩阵（平移+旋转），并输出。
kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("gripper_frame");
ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");
逆运动学
先为可视化做准备。
调用setFromIK解当前规划组arm的逆运动学问题，返回一个bool量。在解决该问题之前，需要如下条件：
- 末端执行器的期望位姿（一般情况下为当前规划组chain的最后一个连杆，本文为gripper_link）：也就是上面已经计算得到的齐次变换矩阵end_effector_state；
- 尝试解决IK的次数：10；
- 每次尝试的时间：0.1s.
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link"); visual_tools.deleteAllMarkers();
std::size_t attempts = 10;
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);
如果IK得到解，则驱动机器人按照计算得到的关节值进行运动，同时，打印计算结果。
if (found_ik){
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
move_group.setJointValueTarget(joint_values);
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan %s", success ? "" : "FAILED");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
for (int 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");
}
计算末端执行器的雅克比矩阵
getJacobian()，先给定一个参考点reference_point_position坐标位置，该坐标位置是相对kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back())（也就是gripper_fame）而言的，，定义一个空的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");
扫一扫
1420
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
