川崎duAro机器人 ROS_moveit demo

说明

demo.cpp

/* Author: hiics */

#include <ros/ros.h>
#include <iostream>
// MoveIt!
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
// moveit_msgs
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_msgs/GetPlannerParams.h>
// moveit_visual_tools
#include <moveit_visual_tools/moveit_visual_tools.h>
// tf2
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
//gazebo
#include <gazebo_msgs/SpawnModel.h>
// Kinematics
#include <moveit_msgs/GetPositionIK.h>

int main(int argc, char** argv)
{
  //init node
  ros::init(argc, argv, "demo");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  //init planning_scene_interface
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  //init lower_arm and initial posture
  static const std::string LOWER_PLANNING_GROUP = "lower_arm";
  moveit::planning_interface::MoveGroupInterface move_group1(LOWER_PLANNING_GROUP);
  const robot_state::JointModelGroup* lower_joint_model_group =
      move_group1.getCurrentState()->getJointModelGroup(LOWER_PLANNING_GROUP);

  moveit::core::RobotStatePtr lower_current_state = move_group1.getCurrentState();
  std::vector<double> lower_joint_group_positions;
  lower_current_state->copyJointGroupPositions(lower_joint_model_group, lower_joint_group_positions);
  lower_joint_group_positions[0] = -0.785;  
  lower_joint_group_positions[1] =  0.785;  
  lower_joint_group_positions[2] =  0.0;  
  lower_joint_group_positions[3] =  0.0;   
  move_group1.setJointValueTarget(lower_joint_group_positions);
  moveit::planning_interface::MoveGroupInterface::Plan move_plan;
  bool success = (move_group1.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group1.execute(move_plan);

  //init upper_arm and initial posture
  static const std::string UPPER_PLANNING_GROUP = "upper_arm";
  moveit::planning_interface::MoveGroupInterface move_group2(UPPER_PLANNING_GROUP); 
  const robot_state::JointModelGroup* upper_joint_model_group =
      move_group2.getCurrentState()->getJointModelGroup(UPPER_PLANNING_GROUP);

  moveit::core::RobotStatePtr upper_current_state = move_group2.getCurrentState();
  std::vector<double> upper_joint_group_positions;
  upper_current_state->copyJointGroupPositions(upper_joint_model_group, upper_joint_group_positions);
  upper_joint_group_positions[0] =  0.785;  
  upper_joint_group_positions[1] = -0.785;  
  upper_joint_group_positions[2] =  0.0;  
  upper_joint_group_positions[3] =  0.0;   
  move_group2.setJointValueTarget(upper_joint_group_positions);
  success = (move_group2.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group2.execute(move_plan);

  // init again
  move_group1.setJointValueTarget(lower_joint_group_positions);
  success = (move_group1.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group1.execute(move_plan);

  //init botharms
  static const std::string BOTH_PLANNING_GROUP = "botharms";
  moveit::planning_interface::MoveGroupInterface two_arms_move_group(BOTH_PLANNING_GROUP); 

  // clear environment
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("marker");
  visual_tools.deleteAllMarkers();
  sleep(1);
  
    //add objest-------------------------------------------------------------------------
  std::vector<moveit_msgs::CollisionObject> collision_objects; 
  // define table1----------------------
  moveit_msgs::CollisionObject collision_table1;
  collision_table1.header.frame_id = move_group1.getPlanningFrame();
  collision_table1.id = "box1";

  shape_msgs::SolidPrimitive primitive1;
  primitive1.type = primitive1.BOX;
  primitive1.dimensions.resize(3);
  primitive1.dimensions