目录
- 1.ROBOTIS OP3启动
- 2.如何执行默认demo
- 3.如何执行GUI程序
- 4.如何使用offset tuner
- 5.如何使用tuner client
- 6.如何创建动作
- 7.如何使用ball detector
- 8.如何用footstep planner升级步态
- 9.如何使用web端控制
- 10.读写示例
1.ROBOTIS OP3启动
1.1总览
本章说明如何启动ROBOTIS-OP3。op3_bringup演示启动op3_manager。
控制ROBOTIS-OP3,各种topics和services传输命令和反馈状态的op3_manager的几个模块
op3_manager的其他程序,如op3_demo和op3_gui_demo。
1.2测试环境
Linux Mint 18.1 Serena(基础:Ubuntu Xenial)
ROS动力学
1.3入门
1.3.1下载和构建
1.3.2停止运行默认演示的Linux服务
ROBOTIS-OP3自动运行默认演示。如果要运行op3_bringup演示,则必须停止默认的演示服务。
sudo service OP3-demo stop
[sudo] password for robotis: 111111
1.3.3BringUp
在终端窗口中键入以下命令。
roslaunch op3_bringup op3_bringup.launch
如果op3_bringup软件包不存在,请将源更新为最新版本
cd ~/catkin_ws/src/ROBOTIS-OP3-Demo
git pull
cd ~/catkin_ws
catkin_make
1.3.4执行结果
当op3_bringup运行时,机器人移动到初始姿态。
执行结果画面
如果您收到offset.yaml错误,请运行op3_offset_tuner(服务器和客户端)并保存偏移量。
ROBOTIS-OP3的执行结果(RVIZ中的初始姿势)
准备移动:现在用户可以使用诸如op3_gui_demo之类的程序来控制ROBOTIS-OP3 。
如果用户关闭程序,请在终端窗口中按ctrl+c即可
op3_bringup.launch如下
<?xml version="1.0" ?>
<launch>
<!-- OP3 Manager -->
<include file="$(find op3_manager)/launch/op3_manager.launch" />
<!-- UVC camera -->
<node pkg="usb_cam" type="usb_cam_node" name="usb_cam_node" output="screen">
<param name="video_device" type="string" value="/dev/video0" />
<param name="image_width" type="int" value="1280" />
<param name="image_height" type="int" value="720" />
<param name="framerate " type="int" value="30" />
<param name="camera_frame_id" type="string" value="cam_link" />
<param name="camera_name" type="string" value="camera" />
</node>
</launch>
op3_manager:控制ROBOTIS-OP3的框架
- 机器人文件: op3_manager/config/OP3.robot
- 联合初始化文件: op3_manager/config/dxl_init_OP3.yaml
- 偏移文件: op3_manager/config/offset.yaml
usb_cam_node:ROBOTIS-OP3的USB摄像机软件包
1.3.5可视化
在终端窗口中键入以下命令以进行可视化。
roslaunch op3_bringup op3_bringup_visualization.launch
rviz屏幕
1.3.6TF树
如果要查看TF树,请按照以下说明进行操作。
- 运行rqt
rqt
- 选择 Plugins -> Visualization -> TF Tree
op3_bringup_visualization.launch 如下
<?xml version="1.0" ?>
<launch>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find op3_description)/urdf/robotis_op3.urdf.xacro'"/>
<!-- Send fake joint values and monitoring present joint angle -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
<rosparam param="/source_list">[/robotis/present_joint_states]</rosparam>
</node>
<!-- Combine joint values -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
<remap from="/joint_states" to="/robotis/present_joint_states" />
</node>
<!-- Show in Rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find op3_bringup)/rviz/op3_bringup.rviz"/>
</launch>
参数
- robot_description:rviz中用于TF和可视化的机器人模型
joint_state_publisher:ROBOTIS-OP3联合值的可视化
robot_state_publisher:为机器人模型制作TF消息
rviz:可视化工具
1.3.7 Description
本部分说明了中的op3_manager内使用的配置文件op3_bringup.launch。
Robot file(.robot)
要操作的机器人的信息。
定义了控制频率,通信接口,波特率,可用设备及其属性。
机器人文件的默认路径: op3_manager/config/OP3.robot
内容如下
[ control info ]
control_cycle = 8 # milliseconds
[ port info ]
# PORT NAME | BAUDRATE | DEFAULT JOINT
/dev/U2D2 | 2000000 | r_sho_pitch
[ device info ]
# TYPE | PORT NAME | ID | MODEL | PROTOCOL | DEV NAME | BULK READ ITEMS
dynamixel | /dev/U2D2 | 1 | XM-430 | 2.0 | r_sho_pitch | present_position
dynamixel | /dev/U2D2 | 2 | XM-430 | 2.0 | l_sho_pitch | present_position
dynamixel | /dev/U2D2 | 3 | XM-430 | 2.0 | r_sho_roll | present_position
dynamixel | /dev/U2D2 | 4 | XM-430 | 2.0 | l_sho_roll | present_position
dynamixel | /dev/U2D2 | 5 | XM-430 | 2.0 | r_el | present_position
dynamixel | /dev/U2D2 | 6 | XM-430 | 2.0 | l_el | present_position
dynamixel | /dev/U2D2 | 7 | XM-430 | 2.0 | r_hip_yaw | present_position
dynamixel | /dev/U2D2 | 8 | XM-430 | 2.0 | l_hip_yaw | present_position
dynamixel | /dev/U2D2 | 9 | XM-430 | 2.0 | r_hip_roll | present_position
dynamixel | /dev/U2D2 | 10 | XM-430 | 2.0 | l_hip_roll | present_position
dynamixel | /dev/U2D2 | 11 | XM-430 | 2.0 | r_hip_pitch | present_position
dynamixel | /dev/U2D2 | 12 | XM-430 | 2.0 | l_hip_pitch | present_position
dynamixel | /dev/U2D2 | 13 | XM-430 | 2.0 | r_knee | present_position
dynamixel | /dev/U2D2 | 14 | XM-430 | 2.0 | l_knee | present_position
dynamixel | /dev/U2D2 | 15 | XM-430 | 2.0 | r_ank_pitch | present_position
dynamixel | /dev/U2D2 | 16 | XM-430 | 2.0 | l_ank_pitch | present_position
dynamixel | /dev/U2D2 | 17 | XM-430 | 2.0 | r_ank_roll | present_position
dynamixel | /dev/U2D2 | 18 | XM-430 | 2.0 | l_ank_roll | present_position
dynamixel | /dev/U2D2 | 19 | XM-430 | 2.0 | head_pan | present_position
dynamixel | /dev/U2D2 | 20 | XM-430 | 2.0 | head_tilt | present_position
sensor | /dev/U2D2 | 200 | OPEN-CR | 2.0 | open-cr | button, present_voltage, gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, roll, pitch, yaw
联合初始化文件Joint Initialize File (.yaml)
为DYNAMIXEL或传感器的属性设置初始化值。
初始化文件的默认路径: op3_manager/config/dxl_init_OP3.yaml
内容如下
r_sho_pitch : # XM-430
return_delay_time : 1 # item name : value
min_position_limit : 0
max_position_limit : 4,095
r_sho_pitch : # XM-430
return_delay_time : 1 # item name : value
min_position_limit : 0
max_position_limit : 4,095
...
偏移文件Offset file(.yaml)
如果机器人由于装配公差和其他原因而机械变形,则调整偏移量可以帮助纠正错误。
偏移文件包含每个关节(弧度)的偏移角度,以校正“偏移调谐器”的变形和初始姿势关节角度。
偏移文件的默认路径: op3_manager/config/offset.yaml
内容如下:
offset:
head_pan: 0
head_tilt: 0
l_ank_pitch: 0.0174532925199433
l_ank_roll: 0
l_el: 0
l_hip_pitch: 0.01221730476396031
l_hip_roll: -0.01570796326794897
l_hip_yaw: 0.004363323129985824
l_knee: 0.006981317007977318
l_sho_pitch: 0
l_sho_roll: 0
r_ank_pitch: 0.008726646259971646
r_ank_roll: 0
r_el: 0
r_hip_pitch: 0.01658062789394613
r_hip_roll: 0.0148352986419518
r_hip_yaw: 0.008726646259971646
r_knee: 0.008726646259971646
r_sho_pitch: 0
r_sho_roll: 0
init_pose_for_offset_tuner:
head_pan: 0
head_tilt: 0
l_ank_pitch: 0
l_ank_roll: 0
l_el: 0
l_hip_pitch: 0
l_hip_roll: 0
l_hip_yaw: 0
l_knee: 0
l_sho_pitch: 0
l_sho_roll: 0
r_ank_pitch: 0
r_ank_roll: 0
r_el: 0
r_hip_pitch: 0
r_hip_roll: 0
r_hip_yaw: 0
r_knee: 0
r_sho_pitch: 0
r_sho_roll: 0
2.如何执行默认demo
2.1总览
本章介绍如何展示OP3的基本demo。有三个可用的demo。踢足球,视觉和各种动作的顺序表演。
2.2入门
2.2.1下载和构建
2.2.2 运行
- 自动启动
ROBOTIS-OP3在启动时开始演示。 - 手动启动
连接到ROBOTIS-OP3并打开终端窗口。获取root权限并执行启动文件。
在终端中输入以下命令。
(密码:111111)
sudo bash
[sudo] password for robotis: 111111
roslaunch op3_demo demo.launch
演示启动文件将执行op3_demo和op3_manager
2.2.3执行结果
ROBOTIS-OP3的DYNAMIXEL将被供电并采取初始姿势。
2.3描述
2.3.1按钮功能
从左侧开始数分别是模式按钮,开始按钮,用户按钮、重置按钮。
模式按钮
- 短按:在就绪模式下,模式按钮切换到下一个演示(足球>视觉>动作)
- 长按:演示运行时,按住模式按钮切换到就绪模式。
开始按钮
- 短按:从就绪模式播放选定的演示。如果演示正在运行,则“开始”按钮将暂停或恢复演示。
用户按钮
复位按钮
- 重置按钮将切断所有DYNAMIXEL的电源。
2.3.2足球示范
-
怎么玩
从演示就绪模式中按一次模式按钮即可切换到自主足球模式,然后按开始按钮进行足球演示。
(ROBOTIS-OP3将宣布“自动足球模式”,并且后面的红色LED指示灯将点亮。)
演示开始时,ROBOTIS-OP3将宣布“开始足球演示”并站起来寻找球。
如果检测到所需的球,请靠近球并踢它。 -
设置步行走参数保存在运动进口参数op3_walking_module中的op3_manager。可以使用中的步进调谐器配置默认参数op3_gui_demo。
-
返回演示就绪模式
按住模式按钮3秒钟,ROBOTIS-OP3会采取初始姿势并返回演示就绪模式。
2.3.3视觉演示
- 如何运行
从演示就绪模式中按两次模式按钮以切换到视觉处理模式,然后按开始按钮播放视觉演示。
(ROBOTIS-OP3将宣布“视觉处理模式”,并且后面的绿色LED指示灯将亮起。)
演示开始时,ROBOTIS-OP3将宣布“开始视觉处理演示”,并站起来寻找脸部。
如果检测到面部,则胸部和背部的RGB-LED变为白色,OP3的头部将跟随检测到的面部。 - 返回演示就绪模式
按住模式按钮3秒钟,ROBOTIS-OP3会采取初始姿势并返回演示就绪模式。
2.3.4动作示范
- 运行方法
在演示就绪模式下,按三次模式按钮可切换到交互动作模式,然后按开始按钮进行动作演示。
(ROBOTIS-OP3将宣布“互动运动模式”,并且后面的蓝色LED将会点亮。)
演示开始时,ROBOTIS-OP3将开始播放预定义的动作序列以及音频。
action_script.yaml包含运动和音频包。
action_script.yaml 文件描述
文件路径 : /op3_demo/script/action_script.yaml
内容如下:
# combination action page number and mp3 file path
action_and_sound:
4 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Thank you.mp3"
41: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Introduction.mp3"
24: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Wow.mp3"
23: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Yes go.mp3"
15: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Sit down.mp3"
1: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Stand up.mp3"
54: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Clap please.mp3"
27: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3"
38: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Bye bye.mp3"
# play list
default: [4, 41, 24, 23, 15, 1, 54, 27, 38]
action_and_sound:要播放的动作文件的页码信息和mp3文件路径的组合信息
action number : mp3 file path
默认值:在互动运动模式下播放动作和声音列表
- 返回演示就绪模式
按住模式按钮3秒钟,ROBOTIS-OP3会采取初始姿势并返回演示就绪模式。
3.如何执行GUI程序
3.1运行程序
有三个选项可运行GUI程序。
- 将输入设备和显示设备直接连接到ROBOTIS-OP3,然后在机器人上运行GUI程序。
- 使用远程PC上的VNC获得对OP3 SBC(英特尔NUC)的控制并远程启动GUI程序。
- 在与ROBOTIS-OP3相同的ROS网络中的远程PC上运行GUI程序。 打开终端窗口,然后输入以下命令。
op3_manager应该在执行GUI演示程序之前运行。
roslaunch op3_gui_demo op3_demo.launch
执行结果
3.2如何摆出ROBOTIS-OP3的初始姿势
单击由红色虚线矩形包围的按钮,将可以base_module控制ROBOTIS-OP3的每个关节并采取初始姿势
3.3如何设置模块
请按照以下步骤配置控制ROBOTIS-OP3对应关节的模块。
单击模块按钮进行配置。
- none
- head_control_module
- action_module
- walking_module
从模块按钮下方的关节状态表中确认相应的关节已正确设置。
Get Mode按钮将报告为每个关节分配了哪个模块。
3.4如何使用步行调谐器
3.4.1总览
本章介绍如何配置行走参数以及如何使用ROBOTIS-OP3对其进行测试。
基本演示使用保存的行走参数。
3.4.2描述
设定模块
walking_module在ROBOTIS-OP3的下半部激活以进行步行测试。
确认用于行走的关节设置为walking_module,然后移至Walking选项卡。
(激活步行模块后,ROBOTIS-OP3将采取步行的初始姿势。)
开始/停止步行
start 按钮:开始步行
stop按钮:停止行走。停止时,与步行相关的参数将被重置。
套用参数
Refresh按钮:获取当前应用于的所有参数walking_module。
Save按钮:将当前应用的所有参数另存walking_module为默认参数,并将其用于其他程序,例如op3_demo。
Apply按钮:将来自GUI的修改后的参数应用于walking_module。
3.5如何运行动作
3.5.1总览
本章介绍如何执行预定义的动作。
所述action_module控制ROBOTIS-OP3的各关节。
3.5.2描述
怎么运行
- 设置模块:按下action_module按钮。
- 选择Motion gui演示程序的选项卡。
- 点击动作按钮播放
创建和编辑动作 action_module
3.6如何控制头部关节
3.6.1总览
本章说明如何控制ROBOTIS-OP3的头部关节。
通过控制头部关节,操作员可以获得不同的摄像机视角。
3.6.2头部关节控制
设置模块:单击head_control_module按钮
选择Head Control gui演示程序的选项卡。
更改特定关节的值。
- 使用滑杆控制头部关节。
- 在文本框中输入所需的值以控制头部关节。
- 将头部关节置于中心位置
3.7如何控制升级后的步行(在线步行)
3.7.1总览
本页说明如何控制升级的步行(在线步行)。
3.7.2描述
如何
准备:设置模块并移至选项卡
- 设置模块:点击online_walking_module按钮
- 选择Online Walking gui演示程序的选项卡
控制ROBOTIS-OP3的行走 - 转到初始姿势:单击Go to Initial Pose按钮
- Balance On
- 设置步行参数
- 发送步行命令到 op3_manager
- 步行参数
- DSP ratio DSP比率:双支持相位比率
- LIPM Height LIPM高度:线性倒立摆高度
- Foot Height Max 脚高度最大值:脚高度的最大值
- ZMP Offset x ZMP偏移量x:x方向的ZMP偏移量
- ZMP Offset y ZMP偏移y:y方向的ZMP偏移(值越大,zmp越远。)
- Body Offset 身体偏移:所需身体偏移
- Foot Distance 脚距:左脚和右脚之间的所需脚距。
使用足迹计划器进行在线行走
4.如何使用offset tuner
5.如何使用tuner client
5.1总览
本章介绍如何调整ROBOTIS-OP3的运动学偏置和位置增益。过去,我们可以使用op3_offset_server和op3_offset_client来调整偏移量。现在,我们可以使用新的op3_tuning_module和op3_tuner_client设置偏移和增益。我们制作了一个op3_tuning_module并将其与op3_manager一起使用,因此您无需运行服务器即可仅用于调整偏移量。
之前
之后
5.2运行调谐器程序
5.2.1如何启动程序
- 分别启动op3_manager和op3_tuner_client编程
调谐器由op3_manager和客户端程序组成,因此同一ROS网络中的其他PC可以调谐偏移和增益。
执行第一个op3_manager。
(其他程序,例如op3_action_editor and op3_walking_tuner should be terminated to run the op3_manager`)。
roslaunch op3_manager op3_manager.launch
启动后op3_manager,从同一台PC或同一ROS网络中的任何PC执行客户端GUI程序。
roslaunch op3_tuner_client op3_tuner_client.launch
- 一次启动op3_manager并op3_tuner_client编程
在终端窗口中输入以下命令。
(其他程序,例如op3_action_editor和op3_walking_tuner应该终止以运行偏移量调谐器。)
roslaunch op3_tuner_client op3_tuner.launch
5.3配置文件
5.3.1 op3_manager 配置文件
config/OP3.robot:保存ROBOTIS-OP3的描述
config/dxl_init_OP3.yaml:包含增益的DYNAMIXEL配置被保存并用于联合初始化
5.3.2 op3_tuning_module 配置文件
data/offset.yaml:保存偏移数据
data/tune_pose.yaml:保存偏移调整姿势信息和增益调整姿势信息
- init_pose
- move_time
- target_pose
- joint_name : angle(degree)
- ...
- tune_pose_01
- move_time : [time, time, ...]
- target_pose : [pose_name, pose_name, ...]
- tune_pose_02
- move_time : [time, time, ...]
- target_pose : [pose_name, pose_name, ...]
- tune_pose_03
- move_time : [time, time, ...]
- target_pose : [pose_name, pose_name, ...]
- tune_pose_04
- move_time : [time, time, ...]
- target_pose : [pose_name, pose_name, ...]
- pose_data
- pose_name
- joint_name : angle(degree)
- ...
5.3.3 op3_tuner_client 配置文件
config/joint_data.yaml:GUI菜单配置文件
5.4如何使用调谐器客户端GUI程序
5.4.1如何调整偏移
- 单击Initial Pose
- 选择的标签 Kinematics Group
- 单击Refresh按钮以获取关节的当前状态
- 调整关节的偏移
- 单击Save Offset按钮保存到文件。
5.4.2如何调整增益
- 单击Initial Pose
- 选择的标签 Kinematics Group
- 单击Refresh按钮以获取关节的当前状态
- 更改姿势并调整关节的增益以观看关节图
(如果要调整其他关节,请删除主题并添加要调整的主题) - 单击Save Gain按钮保存到文件。
0 : haed_pan
1 : haed_tilt
2 : l_ank_pitch
3 : l_ank_roll
4 : l_el
5 : l_hip_pitch
6 : l_hip_roll
7 : l_hip_yaw
8 : l_knee
9 : l_sho_pitch
10 : l_sho_roll
11 : r_ank_pitch
12 : r_ank_roll
13 : r_el
14 : r_hip_pitch
15 : r_hip_roll
16 : r_hip_yaw
17 : r_knee
18 : r_sho_pitch
19 : r_sho_roll
如果您想签入,请输入以下内容
rostopic echo /robotis/goal_joint_states -n 1
6.如何创建动作
6.1总览
ROBOTIS-OP3动作编辑器节点。
本章说明如何创建和编辑ROBOTIS-OP3 的op3_action_module中使用的操作文件。
6.1.1动作文件
动作文件包含ROBOTIS-OP3的姿势和时间数据。描述了DYNAMIXEL的当前位置,该位置已从实际DYNAMIXEL分辨率转换为4,095分辨率。操作文件被写入二进制文件,因此用户可以使用op3_action_editor读取其内容。ROBOTIS当前提供带有源代码的默认操作文件。它位于“ op3_action_module / data”目录中。
动作文件包含256页。每个页面最多可以存储7个阶段(或步骤)的操作数据。默认操作文件不会使用所有页面,用户可以通过将其写在空白页面上来添加自己的操作。
6.2开始
6.2.1下载和构建
6.2.2运行
执行启动文件。
op3_action_editor是对ROBOTIS-OP3的直接控制,会影响其他程序,因此其他控制程序,如op3_manager,op3_offset_tuner和op3_walking_tuner不应该运行。
在执行op3_action_editor启动文件之前,应终止其他程序。
roslaunch op3_action_editor op3_action_editor.launch
6.2.3用户界面
Page number(页码):页码是列出的页码。如果用户要创建新的动作姿势,则可以使用任何空白页。
Page title页面标题:ROBOTIS建议用户在空白页面上创建新操作时使用页面标题。
Current position当前位置:当前位置描述了DYNAMIXEL的位置,该位置已从实际DYNAMIXEL分辨率转换为4,095分辨率。该数据由op3_action_editor中的STP7表示。有时,在op3_action_editor中,该位置可能会读为-。这意味着尚未读取DYNAMIXEL的位置(或扭矩已关闭)。
如果用户关闭DYNAMIXEL,则必须重新打开当前位置才能读取当前位置。
用户可以关闭特定DYNAMIXEL的扭矩。当直接从DYNAMIXEL获取新的机器人姿势的位置值而不是计算这些值时,这非常方便。为此,请关闭所需DYNAMIXEL的扭矩,然后摆出姿势并用手抓住机器人关节,直到重新打开扭矩。机器人将保持当前姿势,并且用户可以读取相应DYNAMIXEL的位置值。
Steps or stages步骤或阶段:每个页面最多可以存储7个步骤,从STP0到STP6。但是,某些动作可能需要超过7个阶段才能完全执行。只需使用多个页面并将其与下一步链接即可解决此问题。
Next下一个:下一个指示是否继续在其他页面上执行操作。要继续操作,只需列出要继续执行操作的页码。数字0表示操作不会继续到另一页(默认值)。链接页面不必具有数字顺序。
Play Count播放计数:播放计数是页面动作被播放的次数。
Exit退出:在某些情况下,必须停止操作。在这些情况下,机器人可能处于不稳定的位置。退出很像“下一步”,因此“退出”应该链接到ROBOTIS-OP3可以回到稳定姿势的页面。如果“ Exit”为0,则表示没有链接的退出页面(默认值)。
Tip:调用一个动作需要多个页面时,ROBOTIS强烈建议用户从起始页面调用该动作。例如,鼓掌在第7页开始,在第8页结束。这意味着您在调用鼓掌时应呼叫第7页。调用第8页可能会导致机器人的意外行为。
STP7:“ STP7”列是DYNAMIXEL的当前位置,该位置已从其原始分辨率转换为4,095分辨率。“-”表示扭矩已释放。
PauseTime:“ PauseTime”是步骤STP [x]的运动回放的暂停持续时间。
Time(x 8msec)时间(x 8毫秒):“时间”是ROBOTIS-OP3完成步骤STP [x]的时间段。每个时间单位占用8毫秒的时间。
强烈建议用户在测试用户自己的新创建或编辑的动作时,为了保持ROBOTIS-OP3的稳定性,位置,速度/时间和暂停值应有较小的增量变化。
6.2.4默认操作文件的内容
下表显示了默认操作文件的内容。
页码 | 页面标题 | 页面简要说明 | 页数 |
---|---|---|---|
1 | walki_init | 初始站立姿势 | 1 |
2 | hello | 问候 | 1 |
3 | thank_you | 谢谢 | 1 |
4 | yes | 是 | 1 |
5 | no | 没有 | 1 |
6 | fighting | 战斗 | 1 |
7 | clap | 鼓掌 | 2 |
9 | S_H_RE | 准备握手 | 1 |
10 | S_H | 握手 | 1 |
11 | S_H_END | 移动到初始姿势fram准备好握手的姿势 | 1 |
12 | scanning | 环顾四周 | 1 |
13 | ceremony | 庆祝 | 1 |
6.2.5动作基本命令编辑器
输入“ help”后,将出现如下所示的推荐列表
exit:退出程序。
re:刷新屏幕。
b:移至上一页。
n:移至下一页。
page [index]:移至[索引]页面。例如,键入第5页会从屏幕上的第5页输出数据。
list:输出页面列表。
new:通过清除所有执行器位置数据来初始化当前页面。
copy [index]:将数据从页面[索引]复制到当前页面。例如,如果您在第5页上并且想要复制第9页,则键入copy 9。
set [value]:设置所选执行器的位置值。例如,如果您希望ID19(头部平移)的值为512,则使用键盘的方向键将光标置于ID19上并键入设置512。
save:保存您所做的任何更改。保存的运动文件(可在“ op3_action_module /数据”中找到motion_4096.bin)
play:播放当前页面的动作。
name:更改当前页面的名称。您可以在屏幕的右上方查看页面名称。例如,第2页的标题为hello;更改名称类型名称,然后按“ ENTER”键。“名称:”将出现在屏幕底部。输入页面所需的名称,例如好,然后再次按“ ENTER”键。
i:将数据从STP7插入STP0。将数据从STP [x]移动到STP [x + 1](如果有)。
i [index]:将数据从STP7插入STP [索引]。将数据从STP [index]移动到STP [index + 1](如果有)。
m [index] [index2]:将数据从[index2]移至[index]。
d [index]:从STP [索引]中删除数据。将数据从STP [index]移到STP [index-1]。
on/off:从所有DYNAMIXEL打开/关闭扭矩。
on / off [index1] [index2] [index3]…:从ID [index1] ID [index2] ID [index3]打开/关闭扭矩。例如,断开20会从ID20释放扭矩。请注意,ID20的STP7将显示为[-]。输入20会再次从ID20打开扭矩,屏幕会输出ID20的当前位置数据。
6.2.6使用op3_action_editor编辑动作的示例
- 运行op3_action_editor
- 通过键入“list”找到“ walking_init page”所在的页面
- 退出列表并通过键入“ page [x]”(例如,第15页)转到任何空白页。
- 并将页面1复制到页面[x]
- 通过键入“play”转到“ walking_init”姿势
- 通过键入“ off 2 4 8”来关闭ID 2、4和8的扭矩
- 在获得所需的姿势后,通过简单的输入再次打开扭矩。然后输入“ i 1”将姿势插入步骤1
- 如下所示,编辑“Pause Time”,STP1的“Time”和“Page Step”。
- 输入“play”并检查ROBOTIS-OP3的动作
7.如何使用ball detector
7.1总览
本章介绍如何通过校准ball_detector_node来找球。ball_detector_node通过指定的颜色和霍夫圈来检测球。
7.2开始
7.2.1运行程序
roslaunch ball_detector ball_detector_from_usb_cam.launch
或者
roslaunch op3_demo demo.launch
7.2.2如何更改参数
Dynamic Reconfigure(动态重新配置) Image View(和图像视图)
运行 rqt
rqt
打开Dynamic Reconfigure
选择Plugins -> Configuration -> Dynamic Reconfigure
打开Image View
选择Plugins-> Visualization -> Image View
- 参数
将鼠标光标放在每个参数时每个参数的描述将弹出。
gaussian_blur_size:高斯模糊核的大小(奇数值)
gaussian_blur_sigma:高斯模糊的标准偏差
canny_edge_th:边缘检测器的阈值
hough_accum_resolution:霍夫累加器的分辨率,以图像分辨率的反比表示
min_circle_dist:圆之间的最小距离
hough_accum_th:确定中心检测的累加器阈值
min_radius:允许的最小圆半径,以像素为单位。(如果未知,请输入0进行检测。)
max_radius:允许的最大圆半径,以像素为单位。(如果未知,请输入0进行检测。)
filter_h_min:H滤波器的最小阈值
filter_h_max:H滤波器的最大阈值
filter_s_min:S滤波器的最小阈值
filter_s_max:S滤波器的最大阈值
filter_v_min:V滤波器的最小阈值
filter_v_max:V滤波器的最大阈值
use_second_filter:使用第二个过滤器
filter2_h_min:H滤波器的最小阈值
filter2_h_max:H滤波器的最大阈值
filter2_s_min:S滤波器的最小阈值
filter2_s_max:S滤波器的最大阈值
filter2_v_min:V滤波器的最小阈值
filter2_v_max:V滤波器的最大阈值
ellipse_size:椭圆大小
debug_image:显示过滤的图像以进行调试
Description
ball_detector_node的工作方式
ball_detector_node首先过滤HSV值,然后使用以下方法检测球HoughCircles
- 如何校准
- 首先在S和V的整个范围内设置H(色相)值(色彩值)。然后在S(饱和度)和V(值)的范围内设置以消除噪点。
Hue通过在圆柱形表示的颜色系统内的度数来测量。因此,值360可以表示为0,最小值可以比最大值大。(例如:最小值-350 /最大值-10
[红色区域]) - 如果选中,use_second_filter您将获得一张带有两种HSV变化的图像。
- 如果选中,则debug_image可以检查经过HSV过滤的二进制图像。
- 设置HSV范围后,您可以校准阈值以更好地检测边缘。
8.如何用footstep planner升级步态
8.1总览
本章介绍升级的步行和足迹计划器。
8.2开始
8.2.1安装
humanoid_navigation的脚步规划师
后续链接和安装包
op3_navigation
- 下载源
cd ~/catkin_ws/src
git clone https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Tools.git
- 构建源
cd ~/catkin_ws
catkin_make
8.2.2运行程序
- op3_manager
roslaunch op3_manager op3_manager.launch
- op3_gui_demo和足迹计划程序
roslaunch op3_gui_demo op3_demo_walking.launch
- rviz用于使用足迹计划程序进行升级的步行。如果用户要在ROBOTIS-OP3中启动rviz,则将监视器连接到ROBOTIS-OP3。
roslaunch op3_description op3_walking.launch
8.2.3与足迹计划者一起散步
准备:设置模块并移至选项卡
- 设置模块:点击online_walking_module按钮
- 选择Online Walking gui演示程序的选项卡。
控制ROBOTIS-OP3的行走 - 转到初始姿势:单击Go to Initial Pose按钮
- Balance On
- 设置步行参数
- 在rviz 3d屏幕中设置交互式标记,然后将标记移动到脚的目标姿势。
- 规划路径并将步行消息发送到ROBOTIS-OP3
9.如何使用web端控制
9.1总览
本章介绍如何通过网页设置ROBOTIS-OP3的参数。
9.2入门
9.2.1安装
web_video_server
sudo apt install ros-kinetic-web-video-server
rosbridge_server
sudo apt install ros-kinetic-rosbridge-server
更新和构建源
- ROBOTIS-OP3-Demo
cd ~/catkin_ws/src/ROBOTIS-OP3-Demo
git pull
- ROBOTIS-OP3-Tools
cd ~/catkin_ws/src/ROBOTIS-OP3-Tools
git pull
- 构建源
cd ~/catkin_ws
catkin_make
设置Web服务器
- 安装Web服务器(APACHE2)
sudo apt install apache2
- 从网络浏览器检查默认页面
- http://10.42.0.1
将文件从ROBOTIS-OP3-Tools文件夹复制到Web服务器文件夹:github
cd ~/catkin_ws/src/ROBOTIS-OP3-Tools/op3_web_setting
sudo cp -r ./html /var/www
9.2.2如何连接ROBOTIS-OP3 WiFi
使用以下信息连接到ROBOTIS-OP3 WiFi
WiFi ssid:ROBOTIS-OP3-share
密码:11111111
出于安全原因,建议更改WiFi密码。(自己用的话也可以不改)
9.2.3如何运行网页设置工具
如何手动为Web服务器运行软件包
roslaunch op3_web_setting_tool web_setting_server.launch
9.2.4 如何设定参数
-
菜单
Demo(演示):控制ROBOTIS-OP3的默认演示。
Soccer(足球):设置ball_detector。
Vision(视觉):查看检测到的脸部状态。
Action(动作):测试ROBOTIS-OP3动作。
Camera(摄像机):更改ROBOTIS-OP3摄像机的参数。 -
球检测器参数
修改后的参数值将自动保存。
gaussian blur size(高斯模糊大小):高斯模糊核的大小(奇数值)
gaussian blur sigma(高斯模糊sigma):高斯模糊的标准偏差
canny edge threshold(边缘边缘阈值):边缘检测器的阈值
hough accum resolutio(霍夫累加器分辨率):霍夫累加器的分辨率,以图像分辨率的反比表示
min circle distance(最小圆距离):圆之间的最小距离
hough accum threshold(霍夫累积阈值):决定中心检测的累积阈值
min radius(最小半径):允许的最小圆半径,以像素为单位。(如果未知,则默认设置为0。)
max radius(最大半径):允许的最大圆半径,以像素为单位。(如果未知,则默认设置为0。)
ellipse size(椭圆尺寸):椭圆尺寸
Hue min(色相最小值):H滤波器的最小阈值
Hue max(色调最大值):H滤波器的最大阈值
Saturation min(饱和度最小值):S滤波器的最小阈值
Saturation max(饱和度最大值):S滤波器的最大阈值
Value min(最小值):V滤波器的最小阈值
Value max(最大值):V滤波器的最大阈值
Debug image(调试图像):显示过滤后的图像进行调试
Normal image(普通图像):显示普通图像
Reset parameters(重置参数):重置所有参数 -
相机设定参数
修改后的参数值将自动保存。
10.读写示例
10.1下载和编译
待更新
10.2如何运行demo
停止默认demo
sudo service OP3-demo stop
运行读写演示
下面是op3_read_write.launch
roslaunch op3_read_write_demo op3_read_write.launch
<?xml version="1.0" ?>
<launch>
<param name="gazebo" value="false" type="bool"/>
<param name="gazebo_robot_name" value="robotis_op3"/>
<param name="offset_file_path" value="$(find op3_manager)/config/offset.yaml"/>
<param name="robot_file_path" value="$(find op3_manager)/config/OP3.robot"/>
<param name="init_file_path" value="$(find op3_manager)/config/dxl_init_OP3.yaml"/>
<param name="device_name" value="/dev/ttyUSB0"/>
<param name="/robotis/direct_control/default_moving_time" value="0.04"/>
<param name="/robotis/direct_control/default_moving_angle" value="90"/>
<!-- OP3 Manager -->
<node pkg="op3_manager" type="op3_manager" name="op3_manager" output="screen">
<param name="angle_unit" value="30" />
</node>
<!-- OP3 Localization -->
<node pkg="op3_localization" type="op3_localization" name="op3_localization" output="screen"/>
<!-- OP3 Read-Write demo -->
<node pkg="op3_read_write_demo" type="read_write" name="op3_read_write" output="screen"/>
</launch>
参数:
gazebo:gazebo模拟的参数
gazebo_robot_name:gazebo模拟的机器人名称
offset_file_path:包含op3_manager使用的每个关节的偏移值的文件路径。
robot_file_path:包含机器人信息的文件路径
init_file_path:包含关节初始参数的文件路径
device_name:与机器人通信的设备名称
/robotis/direct_control/default_moving_time:direct_control_module的最小移动时间
/robotis/direct_control/default_moving_angle:每1秒移动一次的角度,在direct_control_module中使用
节点:
op3_manager:此节点控制ROBOTIS-OP3硬件
op3_localization:用于op3_online_walking的简单本地化节点
op3_read_write_demo:在本章介绍的读写demo
10.3如何操作
说明:按钮
从左至右:模式按钮,开始按钮,用户按钮,重置按钮
模式按钮:使用robotis_controller启动read_write演示
开始按钮:使用direct_control_module启动read_write演示
用户按钮:所有关节上的扭矩使能
重置按钮:所有关节上的扭矩失能
10.4源码
read_write.cpp
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>
#include "robotis_controller_msgs/SetModule.h"
#include "robotis_controller_msgs/SyncWriteItem.h"
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
void jointstatesCallback(const sensor_msgs::JointState::ConstPtr& msg);
void readyToDemo();
void setModule(const std::string& module_name);
void goInitPose();
void setLED(int led);
bool checkManagerRunning(std::string& manager_name);
void torqueOnAll();
void torqueOff(const std::string& body_side);
enum ControlModule
{
None = 0,
DirectControlModule = 1,
Framework = 2,
};
const int SPIN_RATE = 30;
const bool DEBUG_PRINT = false;
ros::Publisher init_pose_pub;
ros::Publisher sync_write_pub;
ros::Publisher dxl_torque_pub;
ros::Publisher write_joint_pub;
ros::Publisher write_joint_pub2;
ros::Subscriber buttuon_sub;
ros::Subscriber read_joint_sub;
ros::ServiceClient set_joint_module_client;
int control_module = None;
bool demo_ready = false;
//node main
int main(int argc, char **argv)
{
//init ros
ros::init(argc, argv, "read_write");
ros::NodeHandle nh(ros::this_node::getName());
init_pose_pub = nh.advertise<std_msgs::String>("/robotis/base/ini_pose", 0);
sync_write_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
dxl_torque_pub = nh.advertise<std_msgs::String>("/robotis/dxl_torque", 0);
write_joint_pub = nh.advertise<sensor_msgs::JointState>("/robotis/set_joint_states", 0);
write_joint_pub2 = nh.advertise<sensor_msgs::JointState>("/robotis/direct_control/set_joint_states", 0);
read_joint_sub = nh.subscribe("/robotis/present_joint_states", 1, jointstatesCallback);
buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback);
// service
set_joint_module_client = nh.serviceClient<robotis_controller_msgs::SetModule>("/robotis/set_present_ctrl_modules");
ros::start();
//set node loop rate
ros::Rate loop_rate(SPIN_RATE);
// wait for starting of op3_manager
//检查/op3_manager节点是否在运行,如果没有运行的话,等待运行
std::string manager_name = "/op3_manager";
while (ros::ok())
{
ros::Duration(1.0).sleep();
if (checkManagerRunning(manager_name) == true)
{
break;
ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect");
}
ROS_WARN("Waiting for op3 manager");
}
//如果/op3_manager正在运行,执行初始姿势并处理主题(topic)和服务(servcice)。
readyToDemo();
//node loop
while (ros::ok())
{
// process
//execute pending callbacks
ros::spinOnce();
//relax to fit output rate
loop_rate.sleep();
}
//exit program
return 0;
}
//按下ROBOTIS-OP3背面的按钮时,将通过以下功能进行处理。
//mode和start按钮将开始演示。如果按下user按钮,将打开所有关节的扭矩。
//在开始演示之前,该readyToDemo()功能用于SyncWriteItem关闭右臂扭矩。
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
{
// starting demo using robotis_controller
if (msg->data == "mode")
{
control_module = Framework;
ROS_INFO("Button : mode | Framework");
readyToDemo();
}
// starting demo using direct_control_module
else if (msg->data == "start")
{
control_module = DirectControlModule;
ROS_INFO("Button : start | Direct control module");
readyToDemo();
}
// torque on all joints of ROBOTIS-OP3
else if (msg->data == "user")
{
torqueOnAll();
control_module = None;
}
}
//如果op3_manager接收到关节的当前值,则根据演示模式通过以下功能进行处理。
//使用通过镜像右手臂关节的值创建的左手臂关节的值创建JointState消息,
//并根据每个demo将其作为主题传递给op3_manager
void jointstatesCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
if(control_module == None)
return;
sensor_msgs::JointState write_msg;
write_msg.header = msg->header;
for(int ix = 0; ix < msg->name.size(); ix++)
{
std::string joint_name = msg->name[ix];
double joint_position = msg->position[ix];
// mirror and copy joint angles from right to left
if(joint_name == "r_sho_pitch")
{
write_msg.name.push_back("r_sho_pitch");
write_msg.position.push_back(joint_position);
write_msg.name.push_back("l_sho_pitch");
write_msg.position.push_back(-joint_position);
}
if(joint_name == "r_sho_roll")
{
write_msg.name.push_back("r_sho_roll");
write_msg.position.push_back(joint_position);
write_msg.name.push_back("l_sho_roll");
write_msg.position.push_back(-joint_position);
}
if(joint_name == "r_el")
{
write_msg.name.push_back("r_el");
write_msg.position.push_back(joint_position);
write_msg.name.push_back("l_el");
write_msg.position.push_back(-joint_position);
}
}
// publish a message to set the joint angles
if(control_module == Framework)
write_joint_pub.publish(write_msg);
else if(control_module == DirectControlModule)
write_joint_pub2.publish(write_msg);
}
void readyToDemo()
{
ROS_INFO("Start Read-Write Demo");
// turn off LED
setLED(0x04);
torqueOnAll();
ROS_INFO("Torque on All joints");
// send message for going init posture
goInitPose();
ROS_INFO("Go Init pose");
// wait while ROBOTIS-OP3 goes to the init posture.
ros::Duration(4.0).sleep();
// turn on R/G/B LED [0x01 | 0x02 | 0x04]
setLED(control_module);
// change the module for demo
if(control_module == Framework)
{
setModule("none");
ROS_INFO("Change module to none");
}
else if(control_module == DirectControlModule)
{
setModule("direct_control_module");
ROS_INFO("Change module to direct_control_module");
}
else
return;
// torque off : right arm
torqueOff("right");
ROS_INFO("Torque off");
}
void goInitPose()
{
std_msgs::String init_msg;
init_msg.data = "ini_pose";
init_pose_pub.publish(init_msg);
}
void setLED(int led)
{
robotis_controller_msgs::SyncWriteItem syncwrite_msg;
syncwrite_msg.item_name = "LED";
syncwrite_msg.joint_name.push_back("open-cr");
syncwrite_msg.value.push_back(led);
sync_write_pub.publish(syncwrite_msg);
}
bool checkManagerRunning(std::string& manager_name)
{
std::vector<std::string> node_list;
ros::master::getNodes(node_list);
for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); node_list_idx++)
{
if (node_list[node_list_idx] == manager_name)
return true;
}
ROS_ERROR("Can't find op3_manager");
return false;
}
void setModule(const std::string& module_name)
{
robotis_controller_msgs::SetModule set_module_srv;
set_module_srv.request.module_name = module_name;
if (set_joint_module_client.call(set_module_srv) == false)
{
ROS_ERROR("Failed to set module");
return;
}
return ;
}
void torqueOnAll()
{
std_msgs::String check_msg;
check_msg.data = "check";
dxl_torque_pub.publish(check_msg);
}
void torqueOff(const std::string& body_side)
{
robotis_controller_msgs::SyncWriteItem syncwrite_msg;
int torque_value = 0;
syncwrite_msg.item_name = "torque_enable";
if(body_side == "right")
{
syncwrite_msg.joint_name.push_back("r_sho_pitch");
syncwrite_msg.value.push_back(torque_value);
syncwrite_msg.joint_name.push_back("r_sho_roll");
syncwrite_msg.value.push_back(torque_value);
syncwrite_msg.joint_name.push_back("r_el");
syncwrite_msg.value.push_back(torque_value);
}
else if(body_side == "left")
{
syncwrite_msg.joint_name.push_back("l_sho_pitch");
syncwrite_msg.value.push_back(torque_value);
syncwrite_msg.joint_name.push_back("l_sho_roll");
syncwrite_msg.value.push_back(torque_value);
syncwrite_msg.joint_name.push_back("l_el");
syncwrite_msg.value.push_back(torque_value);
}
else
return;
sync_write_pub.publish(syncwrite_msg);
}