ROS+moveit+jakaminicob仿真运动

news2024/9/22 15:31:13

先浅浅的放一个官方的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