机器人 笛卡尔运动规划

机器人 笛卡尔运动规划

flyfish

#include <ros/ros.h>
// 包含moveit的API
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <eigen_conversions/eigen_msg.h>
#include <vector>
#include <tf/transform_broadcaster.h>

#include <vector>



int main(int argc, char **argv)
{

    bool cartesian =true; //需要编写成参数形式

    ros::init (argc, argv, "moveit_cartesian_demo",ros::init_options::AnonymousName);
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // ros::NodeHandle nodeHandle;

    //初始化需要使用move group控制的机械臂中的arm group
    moveit::planning_interface::MoveGroupInterface arm("arm");

    ROS_INFO("Planning reference frame: %s", arm.getPlanningFrame().c_str());
    ROS_INFO("End effector reference frame: %s", arm.getEndEffectorLink().c_str());

    //获取终端link的名称
    std::string end_effector_link=arm.getEndEffectorLink();

    //设置目标位置所使用的参考坐标系
    std::string pose_reference_frame="base_link";
    arm.setPoseReferenceFrame(pose_reference_frame);


    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);


    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance( 0.1);
    arm.setGoalOrientationTolerance( 0.1);
    /////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //控制机械臂运动到之前设置的“forward”姿态
    arm.setNamedTarget("forward");
    arm.move();
    //获取当前位姿数据最为机械臂运动的起始位姿
    geometry_msgs::Pose start_pose=arm.getCurrentPose (end_effector_link).pose;

    std::vector<geometry_msgs::Pose > waypoints;
    //waypoints.push_back(start_pose);

    geometry_msgs::Pose wpose=start_pose;
    wpose.position.x -= 0.2;
    wpose.position.y -= 0.2;
    waypoints.push_back(wpose);


    wpose.position.x+=0.05;
    wpose.position.y+=0.15;
    wpose.position.z-=0.15;
    waypoints.push_back(wpose);

    waypoints.push_back(start_pose);

    if(cartesian)
    {
        double fraction = 0.0 ;//  #路径规划覆盖率
        int maxtries = 100 ;//  #最大尝试规划次数
        int attempts = 0;//     #已经尝试规划次数

        //# 设置机器臂当前的状态作为运动初始状态
        arm.setStartStateToCurrentState();
        moveit_msgs::RobotTrajectory    trajectory;
        //# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
        while (fraction < 1.0 && attempts < maxtries)
        {
            //返回一个值，该值介于0.0到1.0之间，表示路径点所描述的实现路径的百分比。如果出错，返回-1.0。

            fraction= arm.computeCartesianPath(
                        waypoints,   //# waypoint poses，路点列表
                        0.01,        //# eef_step，终端步进值
                        0.0,        // # jump_threshold，跳跃阈值
                        trajectory,
                        true,NULL);     //  # avoid_collisions，避障规划



            //  # 尝试次数累加
            attempts += 1;

            //# 打印运动规划进程
            if (attempts % 10 == 0)
                ROS_INFO("still trying after  %d attempts...",attempts);   

        }    

        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
        my_plan.trajectory_=trajectory;  

        //按照规划的运动路径控制机械臂运动

        arm.execute(my_plan);
        //  # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
        if (fraction == 1.0)
        {
            ROS_INFO("Path computed successfully. Moving the arm %f.",fraction);
            arm.move();
            sleep(5);
            ROS_INFO("Path execution complete.");
        }
        //# 如果路径规划失败，则打印失败信息
        else
            ROS_INFO("Path planning failed with only  %f success after %d attempts",fraction,maxtries);
    }



    /////////////////////////////////////////////////////////////////////
    //控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(2);   

    ros::shutdown();

}