Shadow五指灵巧手使用教程
版本:2023年8月版
一、产品介绍
1.产品概述
Shadow Hand Plus是世界上最先进的5指仿生机器手,它被世界各地的研究人员使用。它提供了前所未有的准确性和灵巧性,使研究人员能够以比以往更高的精度和控制力操纵工具和物体。有了这只灵巧的手,研究人员可以探索机器人和人工智能的新途径,并完善现有技术。
2.产品特点
1)小拇指额外增加的自由度允许小指与拇指相对,并使手掌更像人类的行为。
2)手腕上有2个额外的自由度,可提供灵活性,优化手的准确定位。
3)每个手指都有一个独立的左右运动,可实现令人印象深刻的手部操作和其他具有高级灵巧性的动作。
4)肌腱驱动,提供姿势稳定性、减轻冲击和弯曲以促进灵巧运动。
5)多种触觉感应选项,根据您的灵巧性需求,提供一系列不同的手指触觉尖端。
3.箱内物品
4.物品清单
名称 | 数量 | 名称 | 数量 |
灵巧手 | 1 | “SERVER”USB转网口 | 1 |
灵巧手电源、电源线 | 1 | “RIGHT/LEFT HAND”USB转网口 | 1 |
NUC 迷你电脑 | 1 | 工具箱(内六角扳手) | 1 |
迷你电脑电源、电源线 | 1 | 行李箱密码锁 | 2 |
安装板(用于将手连接到优傲臂) | 1 | “Shadow Backup”备份优盘 | 1 |
安装板螺钉(用于安装板) | 4 | 说明书 | 1 |
手部安装板螺钉 | 8 | 笔记本-服务器 | 1 |
3m 网线 | 2 | 笔记本电源、电源线 | 1 |
“NUC-CONTROL”USB转网口 | 1 |
5.机械结构
共有24个关节,20个自由度(电机):
拇指:5个关节,5个自由度
食指、中指、无名指:4个关节,3个自由度
小指:5个关节,4个自由度
腕部:2个关节,2个自由度
重量4.3kg,负载5kg
电源:48V @2.5A
5.机械结构(手指*4)
5.机械结构(拇指)
5.机械结构(腕部)
6.关节角度范围
关节 | 角度 | 弧度 | ||
最小 | 最大 | 最小 | 最大 | |
FF1,MF1,RF1,LF1 | 0 | 90 | 0 | 1.571 |
FF2,MF2,RF2,LF2 | 0 | 90 | 0 | 1.571 |
FF3,MF3,RF3,LF3 | -15 | 90 | -0.262 | 1.571 |
FF4,MF4,RF4,LF4 | -20 | 20 | -0.349 | 0.349 |
LF5 | 0 | 45 | 0 | 0.785 |
TH1 | -15 | 90 | -0.262 | 1.571 |
TH2 | -40 | 40 | -0.698 | 0.698 |
TH3 | -12 | 12 | -0.209 | 0.209 |
TH4 | 0 | 70 | 0 | 1.222 |
TH5 | -60 | 60 | -1.047 | 1.047 |
WR1 | -40 | 28 | -0.698 | 0.489 |
WR2 | -28 | 10 | -0.489 | 0.174 |
7.电机特性(大电机*4+小电机*16)
上图为小电机3D模型
大电机属性 | 参数 | 单位 |
电机功率 | 6 | w |
齿轮比 | 128:1 | |
肌腱最大持续性安全负载 | 65 | N |
肌腱最大负载 | 190 | N |
最大持续电流 | 350 | mA |
小电机属性 | 参数 | 单位 |
电机功率 | 3 | w |
齿轮比 | 131:1 | |
肌腱最大持续性安全负载 | 65 | N |
肌腱最大负载 | 110 | N |
最大持续电流 | 217 | mA |
8.指尖PST(触觉传感器)
原理:通过测量指尖气泡内的空气压力,当指尖 压在物体上时,气泡中的压力增加。
该传感器集成了自动漂移和温度补偿算法(本质上是一个极低截止频率的高通滤波器)。
9.NUC(迷你电脑)
NUC主要用来运行控制循环层程序,这样做的原因是在同一台笔记本电脑上运行其他繁重的程序时会导致运算能力不足的问题,所以将控制循环层的程序迁移到单独的NUC计算机上。
特别是在遥操作等复杂系统中,如果只运行灵巧手,这可能没问题,但是如果运行手臂+手+任何其他比较吃算力的程序,比如视觉程序等,这将会导致算力超支。
10.Laptop-server(笔记本电脑)
这台电脑主要和NUC中的docker镜像进行数据交换,里面搭载了Ubuntu系统和适配灵巧手的ROS环境。在这台电脑上可以运行ROS相关的工具,比如rqt图形界面、rviz可视化、Moveit等等。
11.电气概述
20个电机按奇偶设置ID(0-19),10个一组,共分成两组,通过两条CAN总线接入手掌背部的嵌入式微控制器。
12.外部预留接口
辅助SPI:此处可以连接外部SPI器件,例如ADC、DAC或I/O扩展。手掌能够自动检测连接的设备类型,并通知主机。目前仅支持三种设备:
MCP3208 - 8通道、12位ADC
MCP3204 - 4通道、12位ADC
MCP3202 - 2通道、12位ADC
13.指示灯
图中的指示灯显示的颜色即代表当前的指示状态。
14.控制流图
15.数据流
指令数据:
- EDC指令
- 电机请求数据
- 奇/偶电机
- 查询电机类型
- 电机通信数据
- 触觉传感器请求数据
状态数据:
- 关节传感器/惯性传感器数据
- 电机类型数据
- 每个电机的位置数据
- 电机数据
- 触觉传感器类型数据
- 触觉传感器数据
传感器 | 频率(Hz) | Bits | 备注 |
位置 | 1000 | 12 | 关节位置,分辨率0.2° |
PST | 500 | 11 | 压力触觉传感器,位于指尖 |
扭矩 | 5000 | 12 | 分辨率30mN,内部控制使用 |
温度 | 100 | 12 | 电机温度 |
电流 | 100 | 12 | 电机电流 |
电压 | 100 | 12 | 电机电压 |
16.控制频率
主机端控制频率(1KHz):
“P”,“I”&“D”:控制增益参数
“Max_force”:输出限制
“Position_Deadband”:被认定为0的错误波段
电机端控制频率(5KHz):
“F”:扭矩需求前馈到输出
“P”,“I”&“D”:控制增益参数
“Max_PWM”:电机PWM限制
“Deadband”:被认定为0的错误波段
“Sign”:控制肌腱的曲张
二级电机控制频率(5KHz):
扭矩限制:控制扭矩需求限制
扭矩限制增益:比例项参数
17.控制类型
PWM
控制输入:PWM
输入刷新率:1kHz
实现于:电机端
控制输出:PWM
示教模式
控制输入:扭矩(力)
输入刷新率:1kHz
控制循环:5kHz
实现于:电机端
控制输出:PWM
传感器反馈:电机、应变片
位置模式
控制输入:位置
输入刷新率:1kHz
控制循环:1kHz
实现方式:主机端
控制输出:PWM
传感器反馈:关节位置传感器
轨迹模式
控制输入:位置+时间
位置控制,上面增加了一个算法,将位置目标分割成一个点的集合,创建一个控制关节速度的齿条。
二、使用说明
1.注意事项
1)精密仪器,请轻拿轻放。
2)指尖处的PST传感器比较脆弱,请勿用蛮力挤压,可能会导致指尖破损。
3)请勿在非示教模式和0力模式时用手掰动关节,这可能会对电机或肌腱等造成损伤。
4)请勿在NUC运行状态下强制掉电,可以通过关机按钮关闭NUC后掉电。
5)请勿将手放置在机器人的任何活动关节处,避免造成夹伤。
6)请避免灵巧手遭受液体冲刷或浸泡,如果发生请及时切断总电源开关。
7)请勿拆开灵巧手外壳,这将会影响机器人的保修策略。
8)请尽可能避免或减少修改笔记本服务器或NUC迷你主机中的任何文件,这可能会造成系统或程序无法正常运行。
2.连接设备
1)将对应上述设备的电源和电源线分别正确连接到设备。
2)注意区分不同的USB转网口,不可混用。
3)灵巧手的电源线插头带有红点,和电源端的插头红点对齐即可正常插入。
3.快速启动灵巧手
(启动“Launch Shadow Ritht Hand”后,将会自动运行右侧的4个启动文件,也可以在“Shadow Advanced Launchers”文件夹中手动逐一启动它们。)
1、给灵巧手上电、笔记本电脑开机、NUC开机。
2、双击打开笔记本桌面的图标“Shadow Launchers”,将会进入灵巧手的启动文件存放路径。
3、双击启动路径中的“Launch Shadow Ritht Hand”,等待所有程序启动完成。这个过程会对灵巧手进行初始化,所有手指会轻微抖动片刻。
4、在启动的Rviz图形化界面中可以对灵巧手进行轨迹规划和执行。
5、在“Shadow Demos”文件夹下存放了演示例程,双击图标启动即可触发相应动作,其中“Demo Right Hand.desktop”运行后可以通过轻触不同指尖的PST来触发相应的动作,动作结束后灵巧手会停止运动。
三、基于ROS开发
1.Rviz可视化
2.rqt工具
Dexterous Hand Data Visualizer
在docker终端窗口中输入“rqt”并回车,可以打开rqt界面。该界面包含许多通过以太网口与灵巧手进行交互的插件。大多数都可以从“ Plugins >> Shadow Robot”菜单中打开。
1)监视器
Plugins >> Robot Tools >> Diagnostic Viewer
包含机器人的所有部件的树状图,所有的部件都要打上绿色的勾。
可以通过双击一个电机来详细检查,将会弹出右图Motor Monitor对话框。这个窗口可以用来检查电机的状态,或调试任何问题。
常用字段含义信息:
2)控制模式
Plugins >>Shadow Robot >>Change Robot Control Mode
轨迹控制、位置控制、示教模式、PWM模式
3)关节控制条
Plugins >>Shadow Robot >>Joint Slider
有20个滑条,可以单独控制每个关节的位置。
使用前必须确认灵巧手在位置控制或教学模式。如果控件被更改,请重新加载插件以确保滑块对应于此时灵巧手的关节位置。
4)电机调节器(扭矩控制)
Plugins >>Shadow Robot >>Advanced >>Hand Tuning
这个插件分两个部分,扭矩控制和位置控制。主要用来厂家内部调节电机的PID、扭矩、位置相关参数。默认参数出厂已经调校至最优,无需进行额外的调整。
4)电机调节器(位置控制)
Plugins >>Shadow Robot >>Advanced >>Hand Tuning
这个插件分两个部分,扭矩控制和位置控制。主要用来厂家内部调节电机的PID、扭矩、位置相关参数。默认参数出厂已经调校至最优,无需进行额外的调整。
5)灵巧手标定
Plugin >>Shadow Robot >>Advanced >>Hand Calibration
这个插件是厂家用来校准位置传感器数据。校准必须在NUC机器上运行,可以使用桌面图标“Shadow NUC RQT”启动它。默认参数出厂已经调校至最优,无需进行额外的调整。
7)重启电机
Plugin >>Shadow Robot >>Advanced >>Motor Resetter
如果出于某种原因,您需要重新运行电机上的固件,您可以使用此插件进行软件重启。
勾选您希望重置的电机,然后单击“Reset Motors”。当电机自动调零应变片时,可以看到相应的关节抖动。
8)数据图形化
Plugins >> Shadow Robot >> Dexterous Hand Data Visualizer
图形化的主要数据有:
关节状态(位置、扭矩、速度)
控制回路(设定点、输入、输出、报错)
电机状态(左、右应变片,PWM,电流,电压,功率,温度,位置,扭矩,算法要求的扭矩,编码器位置)
Palm附加组件(加速度计,陀螺仪,模拟信号输入)
触觉传感器数据(压力AC 0,压力AC 1,压力DC,温度AC,温度DC)
触觉传感器数据可视化
在数据可视化器上显示的图形越多,速度就越慢,而且可能无法读取。为了能够看到特定数据类型的完整视图,请切换正确的单选按钮并选中您希望更清楚地看到的图形。
上图展示了PST和配备了Biotacs作为触觉传感器情况下的两种指尖数据,Biotacs还有一个按钮可以切换电极之间触觉点的视觉表现模式。
当PST检测到按压力之后相应的状态显示颜色会由浅(蓝)变深(红)。
“T”项是一个调试值,用于查看温度传感器是否工作。该值用于调整压力读数,它不能用于计算皮肤表面的温度,它不是一个可以转换为ºC的值。
“P”项是一个相对压力值,在未校准的情况下,其范围约为300至1000。它可以用作近似值,以了解施加在指尖上的压力有多大,并且是单调值。
3.ROS话题
Rostopic:命令行工具,用于显示ROS topic的调试信息,包括发布者、订阅者、发布速率和ROS消息。例如在docker终端中输入指令:
$ rostopic echo /rh/debug_etherCAT_data
$ rostopic echo /joint_states
$ rostopic echo /sh_rh_ffj0_position_controller/state
$ rostopic echo /rh/palm_extras
Rqt_plot:提供一个GUI插件,使用不同的绘图后端在2D绘图中使数值可视化。
Rosservice:包含Rosservice命令行工具,用于列出和查询服务并动态调用它们。比如重置电机或者更改电机模式。
1)rostopic echo /rh/debug_etherCAT_data
这个话题是由驱动程序发布的,以1000Hz的频率更新来自灵巧手的数据,因为它是通过EtherCAT接收的,对调试很有用。
2)rostopic echo /joint_states
这个话题是只读的,以125 Hz的频率更新,其中包含了手中的所有关节的名称、位置、速度和力的大小。
3)rostopic echo /rh/sh_rh_ffj0_position_controller/state
这个话题以87Hz的频率发布,它包含control_msgs/JointControllerState类型的消息,其中包含用于每个关节位置控制器的参数。
4)rostopic echo /rh/palm_extras
这个话题以84Hz的频率更新,数据来自插入手掌的传感器。
四、API概览
1.Python接口(推荐)
通过SrHandCommander接口可以实现灵巧手当前基础状态信息的获取、手部动作的规划和执行。sr_interface/sr_robot_commander
eg1:在docker终端中,输入以下命令,执行demo.py脚本(按键或挤压PST执行相关动作):
$ rosrun sr_demos demo.py -s right -ht hand_e
eg2:在docker终端中,输入以下命令,执行open.py脚本(灵巧手张开):
$ rosrun sr_demos open.py -s right -ht hand_e
资料详见:
https://github.com/shadow-robot/sr_interface
2.C++接口
参考sr_interface/sr_example/src/link_joints.cpp
* This example demonstrates how two joints can have their positions 'linked' by
* having a child joint subscribing to the parent joint's controller state topic.
* The messages are then published to the child joint's controller.
*
* The hand should be launched but the trajectory controllers should
* not be running as they overwrite position commands. To check if they are, a call
* to rosservice can be made with the following command:
* >rosservice call /controller_manager/list_controllers
* To stop the trajectory controllers, open the gui (type rqt) and select position control in
* plugins > Shadow Robot > Change controllers
* NOTE: If the joint sliders plugin is open during this change of controllers, it will
* need to be reloaded.
*
* A position can then be published to the parent joint or the joint could be moved by
* using the joint sliders in the gui, plugins > Shadow Robot > joint slider
*
* If you move the joint slider for FFJ3, then MFJ3 will move as well.
*/
#include <ros/ros.h>
#include <string>
// messages
#include <std_msgs/Float64.h>
#include <control_msgs/JointControllerState.h>
/// the name of the parent joint
const char* parent_name = "rh_ffj3";
/// the name of the child joint to link to the parent
const char* child_name = "rh_mfj3";
/// Controller that controls joint position
const char* controller_type = "_position_controller";
// a ros subscriber (will be instantiated later on)
ros::Subscriber sub;
// a ros publisher (will be instantiated later on)
ros::Publisher pub;
/**
* The callback function is called each time a message is received on the
* controller
*
* @param msg message of type sr_hand::joints_data
*/
void callback(const control_msgs::JointControllerStateConstPtr &msg)
{
// publish the message
std_msgs::Float64 command;
command.data = msg->set_point;
pub.publish(command);
return;
}
/**
* The main: initialise a ros node, a subscriber and a publisher
*
* @param argc
* @param argv
*
* @return 0 on success
*/
int main(int argc, char **argv)
{
// init the ros node
ros::init(argc, argv, "link_joints_example");
ros::NodeHandle node;
/**
* init the subscriber and subscribe to the
* parent joint controller topic using the callback function
* callback()
*/
sub = node.subscribe(std::string("sh_") + parent_name + controller_type + "/state", 2, callback);
/**
* init the publisher on the child joint controller command topic
* publishing messages of the type std_msgs::Float64.
*/
pub = node.advertise<std_msgs::Float64>(std::string("sh_") + child_name + controller_type + "/command", 2);
// subscribe until interrupted.
while (ros::ok())
{
ros::spin();
}
return 0;
}
五、注意事项
1)精密仪器,请轻拿轻放。
2)指尖处的PST传感器比较脆弱,请勿用蛮力挤压,可能会导致指尖破损。
3)请勿在非示教模式和0力模式时用手掰动关节,这可能会对电机或肌腱等造成损伤。
4)请勿在NUC运行状态下强制掉电,可以通过关机按钮关闭NUC后掉电。
5)请勿将手放置在机器人的任何活动关节处,避免造成夹伤。
6)请避免灵巧手遭受液体冲刷或浸泡,如果发生请及时切断总电源开关。
7)请勿拆开灵巧手外壳,这将会影响机器人的保修策略。
8)请尽可能避免或减少修改笔记本电脑或NUC迷你主机中的任何文件,这可能会造成系统或程序无法正常运行。
喜欢上述内容的话敬请关注点赞加收藏转发吧,当前文档在社区资源中可以下载,您的支持就是我们最大的动力!非常感谢。