基于Moveit编写机械臂运动规划接口函数

机械臂的运动可以分为在关节空间下的运动和笛卡尔空间下的运动。

关节空间下的运动：在关节空间运动中，运动是通过控制每个关节的角度或位置来描述的。每个关节的运动由其对应的关节角度或位置决定。这种描述方式更接近于机器人关节本身的运动，因此在控制机器人关节的运动时很有用。在关节空间中，机器人的动作可以通过关节控制器直接控制。

笛卡尔空间下的运动：在笛卡尔空间运动中，运动是通过物体的位姿（位置和方向）来描述的。这种描述方式更接近于物体在三维空间中的实际运动，因此在描述机器人末端执行器（如机器人手臂末端的工具）的运动时很有用。在笛卡尔空间中，机器人的动作可以直接描述为末端执行器在三维空间中的位姿变化。

总的来说，关节空间运动更适用于描述机器人的关节运动，它不考虑机械臂运动到目标点过程中的姿态，而笛卡尔空间运动更适用于描述末端执行器在三维空间中的运动，当机械臂运动到目标点过程中需要考虑机械臂末端的姿态时，就要使用笛卡尔空间运动。在实际应用中，这两种描述方式常常结合使用，以便更好地控制机器人的运动。

关节空间运动

        首先要创建moveit的控制规划组arm，配置一些规划的参数等等。

void init(void)
{
        //创建了一个名为arm的MoveGroupInterface对象，用于与名为"arm"的MoveGroup进行交互。
        moveit::planning_interface::MoveGroupInterface arm("arm");
       
        arm.setGoalJointTolerance(0.01);//设置关节空间中的目标容差为0.01
        arm.setGoalPositionTolerance(0.001);//设置笛卡尔空间中的位置容差为0.001m
        arm.setGoalOrientationTolerance(0.01);//设置末端执行器的姿态容差为0.01rad

        arm.setMaxAccelerationScalingFactor(0.3);//最大加速度缩放因子设置为0.3
        arm.setMaxVelocityScalingFactor(0.3);//最大速度缩放因子设置为0.3

        arm.setPoseReferenceFrame("base_link");//设置机械臂运动的参考坐标系为"base_link"

        arm.allowReplanning(true);//允许在规划或执行失败时进行重新规划。
        arm.setNumPlanningAttempts(5);//设置尝试规划轨迹的次数为5次。
        arm.setPlanningTime(5.0);//设置规划算法设置最大允许时间为5秒。
        arm.setPlannerId("RRTConnectkConfigDefault");//设置路径规划算法
}

下面是使用moveit开放的c++API接口实现的机械臂关节空间运动的代码。

move_j

        输入参数：joint_values为机械臂各个关节的目标角度值。

bool move_j(const vector<double> &joint_values)
{
    arm.setJointValueTarget(joint_values);//设置目标关节角度
    arm.move();//关节运动
    sleep(0.5);

    return true;
}

move_p 

        输入参数：pose为机械臂运动目标位姿（x，y，z及姿态，姿态用四元数表示）

bool move_p(const geometry_msgs::Pose &pose)
{
    geometry_msgs::Pose target_pose;
    target_pose.position = pose.position;
    target_pose.orientation = pose.orientation;

    arm.setStartStateToCurrentState();//当前状态为开始运动状态
    arm.setPoseTarget(target_pose);//设置目标位姿
    
    //声明了一个Plan对象，该对象将用于存储由MoveGroup生成的机械臂运动计划。
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit::core::MoveItErrorCode success = arm.plan(plan);

    ROS_INFO("move_p:%s",success ? "Success":"Failed");
    if (success)
    {
        arm.execute(plan);//调用execute方法来执行存储在plan对象中的运动计划。
        sleep(1);
        return true;
    }
    return false;
}

笛卡尔空间运动

move_l:直线运动

        输入参数：pose为机械臂运动目标位姿（x，y，z及姿态，姿态用四元数表示）

    bool move_l(const geometry_msgs::Pose &pose)
    {
        vector<geometry_msgs::Pose> waypoints;//创建一个`waypoints`向量来存储路径点
		geometry_msgs::Pose target_pose;
		target_pose.position = pose.position;
        target_pose.orientation = pose.orientation;

		waypoints.push_back(target_pose);
		
		moveit_msgs::RobotTrajectory trajectory;//初始化一个`RobotTrajectory`对象来存储计算出的轨迹。
		const double jump_threshold = 0.0;// 设置`jump_threshold`为0.0，这是确定路径点之间是否存在“跳跃”的阈值。
		const double eef_step = 0.01;//设置`eef_step`为0.01，这是末端执行器（End Effector，EEF）在路径中移动的步长。

		double fraction = 0.0;// 初始化`fraction`变量来跟踪路径计算的成功程度。
		int maxtries = 100; // 设置最大尝试次数`maxtries`为100。
		int attempts = 0;  // 初始化尝试次数`attempts`为0。

		while (fraction < 1.0 && attempts < maxtries)
		{
			fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);//尝试计算路径，并返回路径计算成功的比例（`fraction`）
			attempts++;
		}

		if (fraction == 1)
		{
			ROS_INFO("Path computed successfully. Moving the arm.");
            
            //创建一个`Plan`对象，并将计算出的`trajectory`赋值给它。
			moveit::planning_interface::MoveGroupInterface::Plan plan;
			plan.trajectory_ = trajectory;

			arm.execute(plan);
			sleep(1);
			return true;
		}
		else
		{
			ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);
			return false;
		}
	}

move_p_with_constrains:带有姿态限制的运动，及机械臂一直要保持一个你设定的姿态运动到目标点

        输入参数：pose为机械臂运动目标位姿（x，y，z及姿态，姿态用四元数表示）

 bool move_p_with_constrains(const geometry_msgs::Pose &pose)
    {
        geometry_msgs::Pose target_pose;
        target_pose.position = pose.position;
        target_pose.orientation = pose.orientation;
       
        ocm.link_name = "link6";//设置约束应用于`base_link`
        ocm.header.frame_id = "base_link";//设置约束的参考坐标系也为`base_link`
        ocm.orientation = pose.orientation;//将目标方向设置为`pose.orientation
        //设置每个轴（x, y, z）的绝对容差为0.1。
        ocm.absolute_x_axis_tolerance = 0.1;
        ocm.absolute_y_axis_tolerance = 0.1;
        ocm.absolute_z_axis_tolerance = 0.1;
        ocm.weight = 1;// 设置约束的权重为1。
        
        //创建一个`Constraints`消息`ocm_constraints`，并将`ocm`添加到其方向约束列表中
        moveit_msgs::Constraints ocm_constraints;
        ocm_constraints.orientation_constraints.push_back(ocm);
       
        arm.setPathConstraints(ocm_constraints);//使用`setPathConstraints`方法将约束应用到MoveGroup

        arm.setStartStateToCurrentState();
        arm.setPoseTarget(target_pose);

        arm.setPlanningTime(10.0);
        
        moveit::planning_interface::MoveGroupInterface::Plan plan;
        moveit::core::MoveItErrorCode success = arm.plan(plan);
        ROS_INFO("move_p:%s",success ? "Success":"Failed");

        arm.setPlanningTime(5.0);
        arm.clearPathConstraints();//清除约束
        
        if (success)
        {
            arm.execute(plan);
            sleep(1);
            return true;
        }
        return false;

    }

move_c:圆周运动（以一个给定半径，作圆周运动 ）

        输入参数：radius为圆周半径

bool RML_63_CONTROL::move_c(double radius)
{
    geometry_msgs::Pose start_pose = arm.getCurrentPose().pose;//获取机械臂当前位姿
    std::vector<geometry_msgs::Pose> waypoints;
    waypoints.push_back(start_pose);

    //在x,y平面内生成圆周路点位姿
    double a=  start_pose.position.x;
    double b = start_pose.position.y;

    //使用循环在0到2π（6.28）之间以0.01为步长迭代，计算圆周上的点，并将它们添加到waypoints向量中。
    for(double th=0.0; th<6.28; th=th+0.01)
    {
        start_pose.position.x = a + (radius - radius * cos(th));
        start_pose.position.y = b + radius * sin(th);
        waypoints.push_back(start_pose);
    }
    // //在y,z平面内生成圆周路点位姿
    // double a=  start_pose.position.y;
    // double b = start_pose.position.z;
    // for(double th=0.0; th<6.28; th=th+0.01)
    // {
    //     start_pose.position.z = b + (radius - radius * cos(th));
    //     start_pose.position.y = a + radius * sin(th);
    //     waypoints.push_back(start_pose);
    // }
    // //在x,z平面内生成圆周路点位姿
    // double a=  start_pose.position.x;
    // double b = start_pose.position.z;
    // for(double th=0.0; th<6.28; th=th+0.01)
    // {
    //     start_pose.position.z = b + radius * sin(th);
    //     start_pose.position.x = a + (radius - radius * cos(th));
    //     waypoints.push_back(start_pose);
    // }

    // 设置末端的规划时间
    arm->setPlanningTime(10.0);

    moveit_msgs::RobotTrajectory trajectory;
    const double jump_threshold = 0.0;
    const double eef_step = 0.01;
    double fraction = 0.0;
    int maxtries = 100;   
    int attempts = 0;     

    while (fraction < 1.0 && attempts < maxtries)
    {
        fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
        attempts++;
    }

    if (fraction == 1)
    {
        ROS_INFO("Move_C Path computed successfully. Moving the arm.");

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

        arm.execute(plan);
        sleep(1);
        arm.setPlanningTime(5.0);
        return true;
    }
    else
    {
        ROS_INFO("Move_C Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);
        return false;
    }

}