[MoveIt! Pick & Place Demo][MoveIt! Pick & Place Demo] 1. 基本介绍

[MoveIt! Pick & Place Demo] 1. 基本介绍

引言

当前研究机械臂，MoveIt!是一个非常流行的软件架构，官方教程看过一遍之后，基本上有一个大体的了解，但是对于实现一个较为完整的应用仍然较为困难。另一个杜宇初学者比较友好的就是ROS-Industrial及其教程，从Linux基本命令到ROS基础，再到MoveIt相关内容，是一套不错的比较完整的教程。

由于本人之前使用过一段时间Kinova Jaco2机械臂，而且官方github网站也发布了相关的示例代码, 因此本系列博客打算以Jaco机械臂为载体，实现一个较为完整的Pick&Place Demo。目前仅以仿真环境为主，在Rviz和Gazebo中实现，编程语言为C++。

环境搭建

首先在自己的工作空间内下载Kinova官方ROS驱动包，然后编译。官方推荐的系统版本是 ROS Indigo 和64位的 Ubuntu 14.04，我在 Ubuntu 16.04 和 ROS kinetic 上也简单测试过，可以运行demo，但是MoveIt!两个版本很多API还是不同的。

我们主要用到的包是kinova_arm_moveit_demo，主要程序是pick_place.cpp，里面定义了pick_place节点，可以执行一系列的动作。下面通过pick_place.h头文件可以简单看一下整个程序结构。后面的博客将会进行更深入分析。

class PickPlace
    {
    public:
        PickPlace(ros::NodeHandle &nh);
        ~PickPlace();



    private:
        ros::NodeHandle nh_;

        // open&close fingers: gripper_group_.plan not alway have a solution
        actionlib::SimpleActionClient<kinova_msgs::SetFingersPositionAction>* finger_client_;

        moveit::planning_interface::MoveGroup* group_;
        moveit::planning_interface::MoveGroup* gripper_group_;
        robot_model::RobotModelPtr robot_model_;
//        robot_state::RobotStatePtr robot_state_;

        planning_scene::PlanningScenePtr planning_scene_;
        planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;

        // work scene
        moveit_msgs::CollisionObject co_;
        moveit_msgs::AttachedCollisionObject aco_;
        moveit_msgs::PlanningScene planning_scene_msg_;


        ros::Publisher pub_co_;
        ros::Publisher pub_aco_;
        ros::Publisher pub_planning_scene_diff_;
        ros::Subscriber sub_pose_;
        ros::Subscriber sub_joint_;

        //
        std::vector<std::string> joint_names_;
        std::vector<double> joint_values_;

        // use Kinova Inverse Kinematic model to generate joint value, then setJointTarget().
        bool use_KinovaInK_;

        // check some process if success.
        bool result_;
        // wait for user input to continue: cin >> pause_;
        std::string pause_;
        std::string robot_type_;
        bool robot_connected_;

        // update current state and pose
        boost::mutex mutex_state_;
        boost::mutex mutex_pose_;
        sensor_msgs::JointState current_state_;
        geometry_msgs::PoseStamped current_pose_;


        // define pick_place joint value and pose
        std::vector<double> start_joint_;
        std::vector<double> grasp_joint_;
        std::vector<double> pregrasp_joint_;
        std::vector<double> postgrasp_joint_;

        geometry_msgs::PoseStamped start_pose_;
        geometry_msgs::PoseStamped grasp_pose_;
        geometry_msgs::PoseStamped can_pose_;
        geometry_msgs::PoseStamped pregrasp_pose_;
        geometry_msgs::PoseStamped postgrasp_pose_;


        void build_workscene();
        void add_obstacle();
        void add_complex_obstacle();
        void clear_obstacle();
        void clear_workscene();
        void add_attached_obstacle();
        void add_target();

        void define_joint_values();
        void define_cartesian_pose();
        geometry_msgs::PoseStamped generate_gripper_align_pose(geometry_msgs::PoseStamped targetpose_msg, double dist, double azimuth, double polar, double rot_gripper_z);
        void setup_constrain(geometry_msgs::Pose target_pose, bool orientation, bool position);
        void check_constrain();

        bool my_pick();
        bool my_place();

        void get_current_state(const sensor_msgs::JointStateConstPtr &msg);
        void get_current_pose(const geometry_msgs::PoseStampedConstPtr &msg);
        // TODO: use Kinova inverse kinematic solution instead of from ROS.
        void getInvK(geometry_msgs::Pose &eef_pose, std::vector<double> &joint_value);
        void check_collision();
        void evaluate_plan(moveit::planning_interface::MoveGroup &group);
        bool gripper_action(double gripper_rad);
    };
}