【ROS-Noetic】双臂同时规划和运行随机姿态

bi3ri-dual_arm_demo

rqt_graph

Nodes/Topics(All)

运行环境

Ubuntu 20.04 + ROS Noetic

安装过程

mkdir -p ~/dual_arm_ws/src
cd ~/dual_arm_ws/src
git clone https://github.com/bi3ri/dual_arm_demo.git
git submodule update --init --recursive
catkin build 

echo “source ~/dual_arm_ws/devel/setup.bash” >> ~/.bashrc

问题 git submodule update --init --recursive

git submodule update --init --recursive是一个Git命令，用于初始化、更新并递归地检出一个仓库中的所有子模块。这个命令经常在克隆包含子模块的Git仓库之后使用，以确保仓库中所有子模块的代码也被正确地检出。下面是这个命令的组成部分的详细解释：

• git submodule：这是操作Git子模块的命令。Git子模块允许你将一个Git仓库作为另一个Git仓库的子目录。

• update：这个子命令用于更新子模块，以匹配在父仓库中记录的提交。

• --init：这个选项用于初始化子模块。如果子模块尚未初始化（即，子模块的目录存在但为空），这个选项将会克隆子模块仓库到子模块目录并检出正确的提交。

• --recursive：这个选项用于递归地更新每个子模块中的子模块。这意味着如果你的子模块内还包含其他子模块，这个命令也会初始化和更新这些嵌套的子模块。

在实际应用中，如果你克隆了一个包含子模块的Git仓库，通常需要执行这个命令来确保所有相关的代码都被检出。这样做可以确保你能够获取到仓库依赖的特定版本的代码，这对于项目的构建和运行是必需的。

例如，如果有一个项目依赖于特定版本的库或其他项目，并且这些依赖项以子模块的形式包含在项目中，那么使用git submodule update --init --recursive命令将确保所有的依赖项都处于正确的版本，从而使得项目能够正确构建和运行。

直接下载解压到本地目录~/dual_arm_ws/src/universal_robot

运行过程

#run in two shells
roslaunch dual_arm_demo app.launch 

rosrun dual_arm_demo moveit_demo

问题 Could not load controller ‘groot_controller’ because controller type ‘position_controllers/JointTrajectoryController’ does not exist.

[ INFO] [1709604560.663736648]: Loaded gazebo_ros_control.
[ERROR] [1709604560.784316301]: Could not load controller ‘groot_controller’ because controller type ‘position_controllers/JointTrajectoryController’ does not exist.
[ERROR] [1709604560.784580083]: Use ‘rosservice call controller_manager/list_controller_types’ to get the available types
Error when loading ‘groot_controller’
[ERROR] [1709604560.800029009]: Could not start controller with name ‘groot_controller’ because no controller with this name exists
Error when starting [‘groot_controller’] and stopping []
[ERROR] [1709604560.840430337]: Could not load controller ‘rocket_controller’ because controller type ‘position_controllers/JointTrajectoryController’ does not exist.
[ERROR] [1709604560.840694830]: Use ‘rosservice call controller_manager/list_controller_types’ to get the available types
Error when loading ‘rocket_controller’
[ERROR] [1709604560.864294751]: Could not start controller with name ‘rocket_controller’ because no controller with this name exists
Error when starting [‘rocket_controller’] and stopping []

没有安装相应的ros控制软件包

sudo apt-get update
sudo apt-get install ros-noetic-joint-trajectory-controller
sudo apt-get install ros-noetic-position-controllers

sudo apt-get install ros-noetic-joint-state-controller
sudo apt-get install ros-noetic-effort-controllers

问题 No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rocket_shoulder_pan_joint

[ INFO] [1709604560.488654758]: Loading gazebo_ros_control plugin
[ INFO] [1709604560.490601643]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1709604560.493038258]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1709604560.635958183]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rocket_shoulder_pan_joint
[ERROR] [1709604560.637498902]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rocket_shoulder_lift_joint
[ERROR] [1709604560.637899467]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rocket_elbow_joint
[ERROR] [1709604560.638312231]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rocket_wrist_1_joint
[ERROR] [1709604560.639747672]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rocket_wrist_2_joint
[ERROR] [1709604560.640322101]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rocket_wrist_3_joint
[ERROR] [1709604560.641844206]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/groot_shoulder_pan_joint
[ERROR] [1709604560.643445501]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/groot_shoulder_lift_joint
[ERROR] [1709604560.654915671]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/groot_elbow_joint
[ERROR] [1709604560.659061206]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/groot_wrist_1_joint
[ERROR] [1709604560.660483962]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/groot_wrist_2_joint
[ERROR] [1709604560.661047190]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/groot_wrist_3_joint

本问题可以忽略，PID调节较为困难，如果要修改~/dual_arm_ws/src/dual_arm_demo/controller/robot_controllers.yaml文件,注意命名空间的差异，可以将gazebo_ros_control放到最后作为单独的控制器(与rocket_controller、groot_controller并列)

记录

moveit_demo.cpp

MoveIt!库来进行机器人运动规划和执行。MoveIt!库提供了碰撞检测的功能，可以确保在规划和执行过程中不会发生碰撞。

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

创建PlanningSceneInterface实例，用于添加和管理规划场景。规划场景包括机器人、物体和环境，它是MoveIt!进行碰撞检测的基础。

#include <ros/ros.h>
#include <memory>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
#include <moveit/robot_state/conversions.h>

#include <geometry_msgs/PointStamped.h>

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

#include <stdlib.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <random>

int main(int argc, char** argv)
{
  // 初始化ROS节点
  ros::init(argc, argv, "moveit_demo");

  // 创建异步ROS节点处理器
  ros::AsyncSpinner spinner(8);
  spinner.start();

  // 定义关节值和MoveGroup接口实例
  std::vector<double> joint_values;
  moveit::planning_interface::MoveGroupInterface rng_group_interface("rocket_and_groot");
  moveit::planning_interface::MoveGroupInterface rocket_group_interface("rocket");
  moveit::planning_interface::MoveGroupInterface groot_group_interface("groot");

  // 设置运动规划的相关参数
  rng_group_interface.setMaxVelocityScalingFactor(1.0);
  rng_group_interface.setMaxAccelerationScalingFactor(1.0);
  rng_group_interface.setPlanningTime(15.0);
  rng_group_interface.setNumPlanningAttempts(20.0);
  
  // 获取机器人模型和当前机器人状态
  moveit::core::RobotModelConstPtr kinematic_model = rocket_group_interface.getRobotModel();
  moveit::core::RobotStatePtr kinematic_state = rng_group_interface.getCurrentState();
  const moveit::core::JointModelGroup* rocket_joint_model_group = kinematic_model->getJointModelGroup("rocket");
  const moveit::core::JointModelGroup* groot_joint_model_group = kinematic_model->getJointModelGroup("groot");

  // 获取关节名称列表
  const std::vector<std::string>& rocket_joint_names = rocket_joint_model_group->getVariableNames();
  const std::vector<std::string>& groot_joint_names = groot_joint_model_group->getVariableNames();
  std::vector<double> rocket_joint_values;
  std::vector<double> groot_joint_values;

  // 初始化随机数生成器
  std::random_device rd; 
  std::mt19937 gen(rd()); 
  std::uniform_int_distribution<> distr(-10, 10);
  std::uniform_int_distribution<> rad_distr(-30, 30);

  // 定义机器人姿态
  geometry_msgs::Pose rocket_pose;
  geometry_msgs::Pose groot_pose;
 
  // 初始化机器人的四元数表示
  Eigen::Quaternionf rocket_q = Eigen::Quaternionf(0.0044319521005895665 , -0.0018064082028716572, 0.714190127940822, -0.6999353940485185);
  Eigen::Quaternionf groot_q = Eigen::Quaternionf(0.7171097271676862 , -0.6959453209354478, -0.029260144371181365, -0.02361341612136324);

  // 循环尝试100次不同的姿态和位置
  for(int i = 0; i < 100; i++){

    // 生成随机的位置偏移量
    float random_x = ((float)distr(gen)) * 0.01;
    float random_y = ((float)distr(gen)) * 0.01;
    float random_z = ((float)distr(gen)) * 0.01;

    // 更新rocket和groot的位置
    rocket_pose.position.x = -0.015463195119993365 + random_x;
    rocket_pose.position.y = 0.02029402510664674 + random_y;
    rocket_pose.position.z = 1.658157440477098 + random_z;
    groot_pose.position.x = -0.01565011581780207 + random_x;
    groot_pose.position.y = -0.019683543216663102 + random_y;
    groot_pose.position.z = 1.657396455658871 + random_z;

    // 生成随机的旋转角度
    float x_rotation = rad_distr(gen) * 0.01;
    float y_rotation = rad_distr(gen) * 0.01;
    float z_rotation = rad_distr(gen) * 0.01;

    // 对rocket和groot的四元数进行旋转
    rocket_q = Eigen::AngleAxisf(x_rotation, Eigen::Vector3f::UnitX()) *
                Eigen::AngleAxisf(y_rotation, Eigen::Vector3f::UnitY()) *
                Eigen::AngleAxisf(z_rotation, Eigen::Vector3f::UnitZ()) *
                rocket_q;

    groot_q = Eigen::AngleAxisf(x_rotation, Eigen::Vector3f::UnitX()) *
                Eigen::AngleAxisf(y_rotation, Eigen::Vector3f::UnitY()) *
                Eigen::AngleAxisf(z_rotation, Eigen::Vector3f::UnitZ()) *
                groot_q;

    // 更新rocket和groot的姿态
    rocket_pose.orientation.w = rocket_q.w();
    rocket_pose.orientation.x = rocket_q.x();
    rocket_pose.orientation.y = rocket_q.y();
    rocket_pose.orientation.z = rocket_q.z();
    groot_pose.orientation.w = groot_q.w();
    groot_pose.orientation.x = groot_q.x();
    groot_pose.orientation.y = groot_q.y();
    groot_pose.orientation.z = groot_q.z();

    // 设置逆向运动学解算的超时时间
    double timeout = 0.1;
    bool rocket_found_ik = kinematic_state->setFromIK(rocket_joint_model_group, rocket_pose, timeout);
    bool groot_found_ik = kinematic_state->setFromIK(groot_joint_model_group, groot_pose, timeout);

    // 判断是否成功找到逆向运动学解
    if (rocket_found_ik && groot_found_ik)
    {
      // 复制关节角度信息
      kinematic_state->copyJointGroupPositions(rocket_joint_model_group, rocket_joint_values);
      kinematic_state->copyJointGroupPositions(groot_joint_model_group, groot_joint_values);

      // 输出关节信息
      for (std::size_t i = 0; i < rocket_joint_names.size(); ++i)
      {
        ROS_INFO("Joint %s: %f", rocket_joint_names[i].c_str(), rocket_joint_values[i]);
        ROS_INFO("Joint %s: %f", groot_joint_names[i].c_str(), groot_joint_values[i]);
      }
    }
    else
    {
      // 输出未找到逆向运动学解的信息
      ROS_INFO("Did not find IK solution");
    }

    // 设置MoveGroup目标关节值
    rng_group_interface.setJointValueTarget(rocket_joint_names, rocket_joint_values);
    rng_group_interface.setJointValueTarget(groot_joint_names, groot_joint_values);

    // 进行运动规划
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (rng_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    // 判断运动规划是否成功
    if(!success){
      ROS_INFO("Plan did not succeed");
    }

    // 执行规划的运动
    rng_group_interface.execute(my_plan);
  }

  // 关闭ROS节点
  ros::shutdown();
}