文章目录
- 前言
- 一、官方样例实现
- 软件版本
- bug修复
- 运行测试
- 二、gp110机械臂控制
- 文件添加
- 文件修改
- 编译工程
- 运行测试
- 总结
前言
在之前文章的基础上相信大家已经学会了如何搭建一个机械臂模型，那么我们如何对其进行控制呢，上网检索了一下没找到能清晰指导实现整套流程的文章，所以自己摸索着写了一篇希望能帮到大家
这篇文章会分享记录如何实现用键盘控制之前我们建立的gp110机械臂的全套流程，想要学习搭建机械臂模型可参考–
一文学会MoveIt Setup Assistant搭建moveit2机械臂模型
一、官方样例实现
软件版本
官方样例的版本如下，这部分通过键盘做机械臂控制教程为–Realtime Arm Servoing
repository: moveit2_tutorials
branch: humble
commit hash: 1855ba9513584ea6f5a764ae618971fb0039cb8f
repository: moveit2
branch: humble
commit hash: 8d7a2140eab5d7ab344ab82f232e1e842fe21432
bug修复
官方教程有如下bug，已提交issus–realtime_servo can not control by keyboard input. fixed in local test #709
修复方法如下：
- 注释servo_cpp_interface_demo中不停发送控制指令部分
# moveit2_tutorials/doc/examples/realtime_servo/src/servo_cpp_interface_demo.cpp
# Line 188
//rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);
- 修改servo_keyboard_input中publisher的定义
# moveit2_tutorials/doc/examples/realtime_servo/src/servo_keyboard_input.cpp
# Line 70 71
const std::string TWIST_TOPIC = "/servo_demo_node/delta_twist_cmds";
const std::string JOINT_TOPIC = "/servo_demo_node/delta_joint_cmds";
运行测试
窗口1运行：
$ ros2 launch moveit2_tutorials servo_cpp_interface_demo.launch.py
窗口2运行：
$ ros2 run moveit2_tutorials servo_keyboard_input
Reading from keyboard
---------------------------
Use arrow keys and the '.' and ';' keys to Cartesian jog
Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame
Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging.
'Q' to quit.
二、gp110机械臂控制
文件添加
拷贝源代码文件到gp110_moveit_config/src，具体文件如下
moveit2_tutorials/doc/examples/realtime_servo/src/servo_cpp_interface_demo.cpp
moveit2_tutorials/doc/examples/realtime_servo/src/servo_keyboard_input.cpp
拷贝launch文件到gp110_moveit_config/launch，具体文件如下
moveit2_tutorials/doc/examples/realtime_servo/launch/servo_cpp_interface_demo.launch.py
拷贝rviz参数及servo参数文件到gp110_moveit_config/config，具体文件如下
moveit2_tutorials/doc/examples/realtime_servo/config/demo_rviz_config.rviz改名为moveit_empty.rviz
moveit2/moveit_ros/moveit_servo/config/panda_simulated_config.yaml改名为gp110_simulated_config.yaml
修改后文件如下所示：
gp110_moveit_config$ tree .
.
├── CMakeLists.txt
├── config
│ ├── common_colors.xacro
│ ├── common_materials.xacro
│ ├── gp110_macro.xacro
│ ├── gp110_simulated_config.yaml
│ ├── gp110.urdf.xacro
│ ├── initial_positions.yaml
│ ├── joint_limits.yaml
│ ├── kinematics.yaml
│ ├── moveit_controllers.yaml
│ ├── moveit_empty.rviz
│ ├── moveit.rviz
│ ├── pilz_cartesian_limits.yaml
│ ├── robot.ros2_control.xacro
│ ├── robot.srdf
│ ├── robot.urdf.xacro
│ └── ros2_controllers.yaml
├── launch
│ ├── demo.launch.py
│ ├── move_group.launch.py
│ ├── moveit_rviz.launch.py
│ ├── rsp.launch.py
│ ├── servo_cpp_interface_demo.launch.py
│ ├── setup_assistant.launch.py
│ ├── spawn_controllers.launch.py
│ ├── static_virtual_joint_tfs.launch.py
│ └── warehouse_db.launch.py
├── package.xml
└── src
├── servo_cpp_interface_demo.cpp
└── servo_keyboard_input.cpp
3 directories, 29 files
文件修改
# gp110_simulated_config.yaml
...
## MoveIt properties
# move_group_name: panda_arm # Often 'manipulator' or 'arm'
# planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world'
move_group_name: gp110_arm # Often 'manipulator' or 'arm'
planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world'
## Other frames
# ee_frame_name: panda_link8 # The name of the end effector link, used to return the EE pose
# robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector
ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose
robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector
...
# command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here
command_out_topic: /gp110_arm_controller/joint_trajectory # Publish outgoing commands here
# moveit_empty.rviz
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 628
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /moveit_servo/publish_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.20000000298023224
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 2.155569553375244
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.08608254045248032
Y: -0.20677587389945984
Z: 0.3424459993839264
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.4603978991508484
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.8753982782363892
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 857
Hide Left Dock: false
Hide Right Dock: true
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1586
X: 1179
Y: 393
# servo_cpp_interface_demo.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import ExecuteProcess
from moveit_configs_utils import MoveItConfigsBuilder
from launch_param_builder import ParameterBuilder
def generate_launch_description():
moveit_config = (
MoveItConfigsBuilder("moveit_resources_gp110")
.robot_description(file_path="config/robot.urdf.xacro")
.robot_description_semantic(file_path="config/robot.srdf")
.trajectory_execution(file_path="config/moveit_controllers.yaml")
.to_moveit_configs()
)
# Get parameters for the Servo node
servo_params = (
ParameterBuilder("moveit_resources_gp110_moveit_config")
.yaml(
parameter_namespace="moveit_servo",
file_path="config/gp110_simulated_config.yaml",
)
.to_dict()
)
# A node to publish world -> base_link transform
static_tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"],
)
# The servo cpp interface demo
# Creates the Servo node and publishes commands to it
servo_node = Node(
package="moveit_resources_gp110_moveit_config",
executable="gp110_servo_cpp_interface_demo",
output="screen",
parameters=[
servo_params,
moveit_config.robot_description,
moveit_config.robot_description_semantic,
],
)
# Publishes tf's for the robot
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[moveit_config.robot_description],
)
# RViz
rviz_base = os.path.join(
get_package_share_directory("moveit_resources_gp110_moveit_config"), "config"
)
rviz_full_config = os.path.join(rviz_base, "moveit_empty.rviz")
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_full_config],
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_gp110_moveit_config"),
"config",
"ros2_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[moveit_config.robot_description, ros2_controllers_path],
output="both",
)
# Load controllers
load_controllers = []
for controller in ["gp110_arm_controller", "joint_state_broadcaster"]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner {}".format(controller)],
shell=True,
output="screen",
)
]
return LaunchDescription(
[rviz_node, static_tf, servo_node, ros2_control_node, robot_state_publisher]
+ load_controllers
)
// servo_cpp_interface_demo.cpp
...
// Next we will create a collision object in the way of the arm. As the arm is servoed towards it, it will slow down
// and stop before colliding
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = "base_link";
collision_object.id = "box";
...
// 屏蔽下一行
// rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);
// servo_keyboard_input.cpp
...
// 修改如下
// Some constants used in the Servo Teleop demo
// const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
// const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
const std::string TWIST_TOPIC = "/servo_demo_node/delta_twist_cmds";
const std::string JOINT_TOPIC = "/servo_demo_node/delta_joint_cmds";
const size_t ROS_QUEUE_SIZE = 10;
const std::string EEF_FRAME_ID = "tool0";
const std::string BASE_FRAME_ID = "base_link";
...
case KEYCODE_1:
RCLCPP_DEBUG(nh_->get_logger(), "1");
// joint_msg->joint_names.push_back("panda_joint1");
joint_msg->joint_names.push_back("joint_1_s");
joint_msg->velocities.push_back(joint_vel_cmd_);
publish_joint = true;
break;
case KEYCODE_2:
RCLCPP_DEBUG(nh_->get_logger(), "2");
// joint_msg->joint_names.push_back("panda_joint2");
joint_msg->joint_names.push_back("joint_2_l");
joint_msg->velocities.push_back(joint_vel_cmd_);
publish_joint = true;
break;
case KEYCODE_3:
RCLCPP_DEBUG(nh_->get_logger(), "3");
// joint_msg->joint_names.push_back("panda_joint3");
joint_msg->joint_names.push_back("joint_3_u");
joint_msg->velocities.push_back(joint_vel_cmd_);
publish_joint = true;
break;
case KEYCODE_4:
RCLCPP_DEBUG(nh_->get_logger(), "4");
// joint_msg->joint_names.push_back("panda_joint4");
joint_msg->joint_names.push_back("joint_4_r");
joint_msg->velocities.push_back(joint_vel_cmd_);
publish_joint = true;
break;
case KEYCODE_5:
RCLCPP_DEBUG(nh_->get_logger(), "5");
// joint_msg->joint_names.push_back("panda_joint5");
joint_msg->joint_names.push_back("joint_5_b");
joint_msg->velocities.push_back(joint_vel_cmd_);
publish_joint = true;
break;
case KEYCODE_6:
RCLCPP_DEBUG(nh_->get_logger(), "6");
// joint_msg->joint_names.push_back("panda_joint6");
joint_msg->joint_names.push_back("joint_6_t");
joint_msg->velocities.push_back(joint_vel_cmd_);
publish_joint = true;
break;
// case KEYCODE_7:
// RCLCPP_DEBUG(nh_->get_logger(), "7");
// joint_msg->joint_names.push_back("panda_joint7");
// joint_msg->velocities.push_back(joint_vel_cmd_);
// publish_joint = true;
// break;
# CMakeLists.txt
cmake_minimum_required(VERSION 3.22)
project(moveit_resources_gp110_moveit_config)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(control_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(moveit_servo REQUIRED)
add_executable(gp110_servo_keyboard_input src/servo_keyboard_input.cpp)
target_include_directories(gp110_servo_keyboard_input PUBLIC include)
ament_target_dependencies(gp110_servo_keyboard_input std_msgs control_msgs rclcpp)
add_executable(gp110_servo_cpp_interface_demo src/servo_cpp_interface_demo.cpp)
target_include_directories(gp110_servo_cpp_interface_demo PUBLIC include)
ament_target_dependencies(gp110_servo_cpp_interface_demo moveit_servo rclcpp)
install(
TARGETS
gp110_servo_keyboard_input
gp110_servo_cpp_interface_demo
DESTINATION
lib/${PROJECT_NAME}
)
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
PATTERN "setup_assistant.launch" EXCLUDE)
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
ament_package()
<!-- package.xml 增加如下-->
<exec_depend>rclcpp</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>moveit_msgs</exec_depend>
<exec_depend>moveit_servo</exec_depend>
编译工程
$ colcon build --packages-up-to --cmake-args -DCMAKE_BUILD_TYPE=Release
$ . install/setup.bash
运行测试
窗口1运行：
$ ros2 launch moveit_resources_gp110_moveit_config servo_cpp_interface_demo.launch.py
窗口2运行：
$ ros2 run moveit_resources_gp110_moveit_config gp110_servo_keyboard_input
Reading from keyboard
---------------------------
Use arrow keys and the '.' and ';' keys to Cartesian jog
Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame
Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging.
'Q' to quit.
总结
以上就是今天要讲的内容，本文从头配置和修改实现了键盘控制机械臂，如果是joycon或其他输入控制与本文所分享思想相同，控制响应和publisher即可。
