Franka Ros Moveit 基于关节空间的目标位置运动

#include <cmath>
#include <iostream>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64MultiArray.h>

class FrankaPositionControl {
public:
  FrankaPositionControl(ros::NodeHandle& nh)
    : nh_(nh), move_group_("panda_arm") {
    joint_state_sub_ = nh_.subscribe("/franka/joint_states", 1, &FrankaPositionControl::jointStateCallback, this);
    joint_position_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/franka/joint_position_commands", 1);
  }

  void run() {
    // 设置目标关节角度
    std::vector<double> q_goal = {0.0, -M_PI_4, 0.0, -3 * M_PI_4, 0.0, M_PI_2, M_PI_4};

    // 创建运动规划请求
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    move_group_.setJointValueTarget(q_goal);

    // 进行运动规划
    moveit::planning_interface::MoveItErrorCode success = move_group_.plan(plan);
    if (success == moveit::planning_interface::MoveItErrorCode::SUCCESS) {
      // 执行运动计划
      move_group_.move();
      std::cout << "Trajectory completed." << std::endl;
    } else {
      std::cout << "Motion planning failed." << std::endl;
    }
  }

private:
  void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) {
    std_msgs::Float64MultiArray joint_position_cmd;
    joint_position_cmd.data = {msg->position[0], msg->position[1], msg->position[2],
                              msg->position[3], msg->position[4], msg->position[5],
                              msg->position[6]};
    joint_position_pub_.publish(joint_position_cmd);
  }

  ros::NodeHandle nh_;
  moveit::planning_interface::MoveGroupInterface move_group_;
  ros::Subscriber joint_state_sub_;
  ros::Publisher joint_position_pub_;
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "franka_position_control");
  ros::NodeHandle nh;
  FrankaPositionControl controller(nh);
  controller.run();
  ros::spin();
  return 0;
}

这个示例使用了MoveIt!来实现Franka机器人的关节空间位置控制。主要步骤如下:

1. 设置目标关节角度。
2. 创建一个`moveit::planning_interface::MoveGroupInterface`对象,并设置目标关节角度。
3. 调用`move_group_.plan()`进行运动规划。
4. 如果规划成功,则调用`move_group_.move()`执行运动计划。

与之前的示例相比,这个示例使用了MoveIt!库来完成运动规划和执行。MoveIt!提供了更加强大和灵活的运动规划功能,包括碰撞检测、前向运动学、逆运动学等。使用MoveIt!可以更方便地实现复杂的运动任务,并且具有更好的可重用性和可扩展性。

在实际使用时,可以根据具体需求调整目标关节角度,以及其他规划参数,如规划器类型、轨迹平滑等。同时也可以结合力矩控制、阻抗控制等方法,实现更复杂的运动控制。