川崎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[0] = 0.4;
  primitive1.dimensions[1] = 0.5;
  primitive1.dimensions[2] = 0.7;

  geometry_msgs::Pose box_pose1;
  box_pose1.orientation.w = 1.0;
  box_pose1.position.x = 0.75;
  box_pose1.position.y = 0.0;
  box_pose1.position.z = 0.45;

  collision_table1.primitives.push_back(primitive1);
  collision_table1.primitive_poses.push_back(box_pose1);
  collision_table1.operation = collision_table1.ADD;
  collision_objects.push_back(collision_table1);

  // define table2----------------------
  moveit_msgs::CollisionObject collision_table2;
  collision_table2.header.frame_id = move_group1.getPlanningFrame();
  collision_table2.id = "box2";

  shape_msgs::SolidPrimitive primitive2;
  primitive2.type = primitive2.BOX;
  primitive2.dimensions.resize(3);
  primitive2.dimensions[0] = 0.4;
  primitive2.dimensions[1] = 0.5;
  primitive2.dimensions[2] = 0.7;

  geometry_msgs::Pose box_pose2;
  box_pose2.orientation.w = 1.0;
  box_pose2.position.x = -0.75;
  box_pose2.position.y = 0.0;
  box_pose2.position.z = 0.45;

  collision_table2.primitives.push_back(primitive2);
  collision_table2.primitive_poses.push_back(box_pose2);
  collision_table2.operation = collision_table2.ADD;
  collision_objects.push_back(collision_table2);

  // define table3----------------------
  moveit_msgs::CollisionObject collision_table3;
  collision_table3.header.frame_id = move_group1.getPlanningFrame();
  collision_table3.id = "box3";

  shape_msgs::SolidPrimitive primitive3;
  primitive3.type = primitive3.BOX;
  primitive3.dimensions.resize(3);
  primitive3.dimensions[0] = 1;
  primitive3.dimensions[1] = 0.4;
  primitive3.dimensions[2] = 0.7;

  geometry_msgs::Pose box_pose3;
  box_pose3.orientation.w = 1.0;
  box_pose3.position.x = 0;
  box_pose3.position.y = 0.65;
  box_pose3.position.z = 0.45;

  collision_table3.primitives.push_back(primitive3);
  collision_table3.primitive_poses.push_back(box_pose3);
  collision_table3.operation = collision_table3.ADD;
  collision_objects.push_back(collision_table3);

  // define object1----------------------
  moveit_msgs::CollisionObject collision_object1;
  collision_object1.header.frame_id = move_group1.getPlanningFrame();
  collision_object1.id = "box4";

  shape_msgs::SolidPrimitive primitive4;
  primitive4.type = primitive4.BOX;
  primitive4.dimensions.resize(3);
  primitive4.dimensions[0] = 0.1;
  primitive4.dimensions[1] = 0.1;
  primitive4.dimensions[2] = 0.1;

  geometry_msgs::Pose box_pose4;
  box_pose4.orientation.w = 1.0;         
  box_pose4.position.x = 0.75;           
  box_pose4.position.y = 0.00;              
  box_pose4.position.z = 0.86;         

  collision_object1.primitives.push_back(primitive4);
  collision_object1.primitive_poses.push_back(box_pose4);
  collision_object1.operation = collision_object1.ADD;
  collision_objects.push_back(collision_object1);

  // define object2----------------------
  moveit_msgs::CollisionObject collision_object2;
  collision_object2.header.frame_id = move_group2.getPlanningFrame();
  collision_object2.id = "box5";

  shape_msgs::SolidPrimitive primitive5;
  primitive5.type = primitive5.BOX;
  primitive5.dimensions.resize(3);
  primitive5.dimensions[0] = 0.1;
  primitive5.dimensions[1] = 0.1;
  primitive5.dimensions[2] = 0.1;

  geometry_msgs::Pose box_pose5;
  box_pose5.orientation.w = 1.0;  
  box_pose5.position.x = -0.75;        
  box_pose5.position.y = 0.00;            
  box_pose5.position.z = 0.86;         

  collision_object2.primitives.push_back(primitive5);
  collision_object2.primitive_poses.push_back(box_pose5);
  collision_object2.operation = collision_object2.ADD;
  collision_objects.push_back(collision_object2);

  // Add an object into the world
  ROS_INFO_NAMED("tutorial", "Add an object into the world");
  planning_scene_interface.addCollisionObjects(collision_objects);
  sleep(1);
  
  // step1:left arm move--------------------------------------------------------
  // move lower_pose1(up)
  geometry_msgs::Pose lower_pose1;
  lower_pose1.orientation.w = 1.0;
  lower_pose1.position.x = 0.2544;
  lower_pose1.position.y = 0.6546;
  lower_pose1.position.z = 1.10;  
  move_group1.setPoseTarget(lower_pose1);

  success = (move_group1.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group1.execute(move_plan);

  // move lower_pose2 (go above the object )
  geometry_msgs::Pose lower_pose2;
  lower_pose2.orientation.w = 1.0;
  lower_pose2.position.x = 0.749;
  lower_pose2.position.y = 0.01;
  lower_pose2.position.z = 1.10;  
  move_group1.setPoseTarget(lower_pose2);

  success = (move_group1.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group1.execute(move_plan);

  // move lower_pose3(down)
  geometry_msgs::Pose lower_pose3;
  lower_pose3.orientation.w = 1.0;
  lower_pose3.position.x = 0.749;
  lower_pose3.position.y = 0.01;
  lower_pose3.position.z = 0.97;
  move_group1.setPoseTarget(lower_pose3);

  success = (move_group1.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group1.execute(move_plan);

  //catch the object
  move_group1.attachObject(collision_object1.id,"lower_link_j4");

  // step2:right arm move--------------------------------------------------------
  // move upper_pose1(up)
  geometry_msgs::Pose upper_pose1;
  upper_pose1.orientation.w = 1.0;
  upper_pose1.position.x = -0.2545;
  upper_pose1.position.y =  0.6546;
  upper_pose1.position.z =  1.10; 
  move_group2.setPoseTarget(upper_pose1);

  success = (move_group2.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group2.execute(move_plan);

  // move upper_pose2(go above the object )
  geometry_msgs::Pose upper_pose2;
  upper_pose2.orientation.w = 1.0;
  upper_pose2.position.x = -0.749;
  upper_pose2.position.y =  0.01;
  upper_pose2.position.z =  1.10;
  move_group2.setPoseTarget(upper_pose2);

  success = (move_group2.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group2.execute(move_plan);

  // move upper_pose3(down)
  geometry_msgs::Pose upper_pose3;
  upper_pose3.orientation.w = 1.0;
  upper_pose3.position.x = -0.749;
  upper_pose3.position.y =  0.01;
  upper_pose3.position.z =  0.965;
  move_group2.setPoseTarget(upper_pose3);

  success = (move_group2.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group2.execute(move_plan);
  
  //catch the object
  move_group2.attachObject(collision_object2.id,"upper_link_j4");

  //step3:left arm forward move--------------------------------------------------------
  // move lower_pose4 (up)
  geometry_msgs::Pose lower_pose4;
  lower_pose4.orientation.w = 1.0;
  lower_pose4.position.x = 0.749;
  lower_pose4.position.y = 0.01;
  lower_pose4.position.z = 1.10; 
  move_group1.setPoseTarget(lower_pose4);

  success = (move_group1.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group1.execute(move_plan);

  // move lower_pose5(forward)
  geometry_msgs::Pose lower_pose5;
  lower_pose5.orientation.w = 1.0;
  lower_pose5.position.x = 0.3553;
  lower_pose5.position.y = 0.6632;
  lower_pose5.position.z = 1.10;
  move_group1.setPoseTarget(lower_pose5);

  success = (move_group1.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group1.execute(move_plan);  

  //step4:right arm forward move--------------------------------------------------------
  // move upper_pose4 (up)
  geometry_msgs::Pose upper_pose4;
  upper_pose4.orientation.w = 1.0;
  upper_pose4.position.x = -0.749;
  upper_pose4.position.y =  0.01;
  upper_pose4.position.z =  1.10;
  move_group2.setPoseTarget(upper_pose4);

  success = (move_group2.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group2.execute(move_plan);

  // move upper_pose4 (forward)
  geometry_msgs::Pose upper_pose5;
  upper_pose5.orientation.w = 1.0;
  upper_pose5.position.x = -0.333;
  upper_pose5.position.y =  0.678;
  upper_pose5.position.z =  1.10;
  move_group2.setPoseTarget(upper_pose5);

  success = (move_group2.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group2.execute(move_plan);

  //step5:lift arm put down object--------------------------------------------------------
  // move lower_pose6 (down)
  geometry_msgs::Pose lower_pose6;
  lower_pose6.orientation.w = 1.0;
  lower_pose6.position.x = 0.3553;
  lower_pose6.position.y = 0.6632;
  lower_pose6.position.z = 0.97;
  move_group1.setPoseTarget(lower_pose6);

  success = (move_group1.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group1.execute(move_plan);

  move_group1.detachObject(collision_object1.id);
  sleep(1);

  // move lower_pose7 (up)
  move_group1.setPoseTarget(lower_pose5);
  success = (move_group1.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group1.execute(move_plan);


  //step6:right arm put down object--------------------------------------------------------
  geometry_msgs::Pose upper_pose6;
  upper_pose6.orientation.w = 1.0;
  upper_pose6.position.x = -0.333;
  upper_pose6.position.y =  0.678;
  upper_pose6.position.z =  0.97;
  move_group2.setPoseTarget(upper_pose6);

  success = (move_group2.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group2.execute(move_plan);
  
  move_group2.detachObject(collision_object2.id);
  sleep(1);
  move_group2.setPoseTarget(upper_pose5);
  success = (move_group2.plan(move_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group2.execute(move_plan);

  // step7: Return to the initial position--------------------------------------------------------
  moveit::planning_interface::MoveGroupInterface::Plan two_arms_plan;
  geometry_msgs::Pose lower_pose7;
  lower_pose7.orientation.w = 1.0;
  lower_pose7.position.x = 0.2544;
  lower_pose7.position.y = 0.6546;
  lower_pose7.position.z = 0.965;

  geometry_msgs::Pose upper_pose7;
  upper_pose7.orientation.w = 1.0;
  upper_pose7.position.x = -0.2545;
  upper_pose7.position.y =  0.6546;
  upper_pose7.position.z =  0.965;

  two_arms_move_group.setPoseTarget(lower_pose7, "lower_link_j4");
  two_arms_move_group.setPoseTarget(upper_pose7, "upper_link_j4");
  success = (two_arms_move_group.plan(two_arms_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  two_arms_move_group.execute(two_arms_plan);

  return 0;
}

cmake.txt

cmake_minimum_required(VERSION 2.8.3)
project(khi_duaro_gazebo)

add_compile_options(-std=c++11)

find_package(Eigen3 REQUIRED)

if(NOT EIGEN3_INCLUDE_DIRS)
  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
endif()

find_package(catkin REQUIRED COMPONENTS
 interactive_markers
 moveit_core
 moveit_visual_tools
 moveit_ros_planning
 moveit_ros_planning_interface
 pluginlib
 geometric_shapes
 tf2_ros
 tf2_eigen
 tf2_geometry_msgs
)

catkin_package(
  LIBRARIES
    demo
    demo_dual
  CATKIN_DEPENDS
    moveit_core
    moveit_visual_tools
    moveit_ros_planning_interface
    interactive_markers
  DEPENDS
    EIGEN3
)

include_directories(SYSTEM ${THIS_PACKAGE_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS})
include_directories(${catkin_INCLUDE_DIRS})
link_directories(${catkin_LIBRARY_DIRS})

add_executable(demo
  src/demo.cpp
)

target_link_libraries(demo ${catkin_LIBRARIES})

install(TARGETS demo DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(DIRECTORY launch controller DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

install(DIRECTORY config launch script
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
  USE_SOURCE_PERMISSIONS )

package.xml

<?xml version="1.0"?>
<package>
  <name>khi_duaro_gazebo</name>
  <version>1.1.2</version>
  <description>The khi_duaro_gazebo package</description>

  <author email="nakamichi_d@khi.co.jp">nakamichi_d</author>
  <maintainer email="nakamichi_d@khi.co.jp">nakamichi_d</maintainer>

  <license>BSD</license>

  <url type="website">http://ros.org/wiki/khi_robot</url>
  <url type="repository">https://github.com/Kawasaki-Robotics/khi_robot</url>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>gazebo_ros_control</build_depend>
  <build_depend>pluginlib</build_depend>
  <build_depend>eigen</build_depend>
  <build_depend>moveit_core</build_depend>
  <build_depend>moveit_ros_planning_interface</build_depend>
  <build_depend>moveit_ros_perception</build_depend>
  <build_depend>interactive_markers</build_depend>
  <build_depend>geometric_shapes</build_depend>
  <build_depend>moveit_visual_tools</build_depend>
  <build_depend>tf2_ros</build_depend>
  <build_depend>tf2_eigen</build_depend>
  <build_depend>tf2_geometry_msgs</build_depend>

  <run_depend>gazebo_ros_control</run_depend>
  <run_depend>pluginlib</run_depend>
  <run_depend>eigen</run_depend>
  <run_depend>moveit_core</run_depend>
  <run_depend>moveit_ros_planning_interface</run_depend>
  <run_depend>moveit_ros_perception</run_depend>
  <run_depend>interactive_markers</run_depend>
  <run_depend>geometric_shapes</run_depend>
  <run_depend>moveit_visual_tools</run_depend>
  <run_depend>tf2_ros</run_depend>
  <run_depend>tf2_eigen</run_depend>
  <run_depend>tf2_geometry_msgs</run_depend>

  <run_depend>catkin</run_depend>
  <run_depend>controller_manager</run_depend>
  <run_depend>effort_controllers</run_depend>
  <run_depend>gazebo_ros</run_depend>
  <run_depend>joint_state_controller</run_depend>
  <run_depend>joint_trajectory_controller</run_depend>
  <run_depend>positon_controllers</run_depend>
  <run_depend>robot_state_publisher</run_depend>
  <run_depend>rostopic</run_depend>
  <run_depend>catkin</run_depend>
  <run_depend>khi_duaro_description</run_depend>

  <export></export>

</package>

rviz_demo.launch

其实就是moveit_config文件夹中的demo.launch

<launch>
  <arg name="db" default="false" />
  <arg name="db_path" default="$(find khi_duaro_moveit_config)/default_warehouse_mongo_db" />
  <arg name="debug" default="false" />
  <arg name="use_gui" default="false" />

  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find khi_duaro_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="$(arg use_gui)"/>
    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
  </node>

  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find khi_duaro_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find khi_duaro_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- If database loading was enabled, start mongodb as well -->
  <include file="$(find khi_duaro_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
  </include>

</launch>