ROS+moveit+jakaminicob仿真运动-编程知识

ROS+moveit+jakaminicob仿真运动

先浅浅的放一个官方的c++文档：

Motion Planning API — moveit_tutorials Melodic documentation

一、实现运动到目标点的程序

二、在rviz里面新建扫描平台

一、实现运动到目标点的程序

（等我得空了补一个c++运行环境部署说明）

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>#include <moveit_visual_tools/moveit_visual_tools.h>#include <motion_planning.h>
#include <iostream>
#include <math.h>#include <vector>
#include <cmath>
#include <Eigen/Dense>#include <fstream>
// #include <inv_2.cpp>using namespace Eigen;int main(int argc, char** argv)
{
//ros初始化需要的几行代码，创建一个 ROS 节点（Node），并启动一个异步的 ROS spinner。
ros::init(argc, argv, "move_group_interface_tutorial_zyw");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();//这是我们操作的关节
static const std::string PLANNING_GROUP = "arm_group";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// // 原始指针经常被用来指代计划组以改进性能
const robot_state::JointModelGroup* joint_model_group =move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// 设置目标位置
geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 0.0654579;
target_pose1.orientation.x = 0.0654572;
target_pose1.orientation.y = 0.704072;
target_pose1.orientation.z = -0.704069;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.1;
move_group.setPoseTarget(target_pose1);
//plan
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if (success) {
//执行move_group.execute(my_plan);
} else {ROS_WARN("Failed to plan and execute the trajectory.");
}return 0;}

二、在rviz里面新建扫描平台

1.效果图：

2.代码：

ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
ros::Rate loop_rate(10);
while (ros::ok()){//创建一个物体moveit_msgs::CollisionObject collision_object;collision_object.header.frame_id = move_group.getPlanningFrame();moveit::planning_interface::PlanningSceneInterface planning_scene_interface;// The id of the object is used to identify it.//id必须设置collision_object.id = "box1";// Define a box to add to the world.shape_msgs::SolidPrimitive primitive;primitive.type= primitive.CYLINDER;primitive.dimensions.resize(2);//dimensions[0]控制高度的primitive.dimensions[0] = 0.1;//dimensions[1]控制半径的primitive.dimensions[1] = 0.1;// Define a pose for the box (specified relative to frame_id)geometry_msgs::Pose box_pose;box_pose.orientation.w = 1.0;//圆柱中心点的距离box_pose.position.x = 0.2;box_pose.position.y = 0.2;box_pose.position.z = 0.05;collision_object.primitives.push_back(primitive);collision_object.primitive_poses.push_back(box_pose);std::vector<moveit_msgs::CollisionObject> collision_objects;collision_objects.push_back(collision_object);//定义操作为添加collision_object.operation = collision_object.ADD;//定义一个PlanningScene消息moveit_msgs::PlanningScene planning_scene;planning_scene.world.collision_objects.push_back(collision_object);planning_scene.is_diff = true;//发布该消息planning_scene_diff_publisher.publish(planning_scene);loop_rate.sleep();}

3.代码的简单说明

这部分代码不包括机械臂的初始化和头文件的引用（如果有小朋友不懂这个，可以去看我在开头引入的官方文档，官方文档给了你两个功能包，moveit_tutorials和panda_moveit_config。你clone到本地打开，在moveit_tutorials/doc/move_group_interface/src/move_group_interface_tutorial.cpp里面有机械臂的初始化代码，然后你自己如过不会写cmakelist文件，可以直接用moveit_tutorials的cmakelist文件，当然你也可以直接用我在一里面写的代码），只是物体的新建和主题的发布。物体的新建可以去看看shape_msgs::SolidPrimitive的官方文档，里面有设置形状、位置等参数的说明。

shape_msgs/SolidPrimitive Documentation