ROS robotic arm From Scratch

warning

Some of the images cannot be displayed properly because the original blogging platform has closed external links
here is the original links
https://www.jianshu.com/p/53bd12fd7155

https://www.jianshu.com/p/30b922dae307

https://www.jianshu.com/p/040635e4d8c8

Demonstrate

Arduino control servo motor with ros

设计图

第一版设计,总感觉有点不对劲,先试试看。
第二版设计
最终结果大概是想设计成上面这样。
第一个问题是如何使用ROS控制舵机转动一定角度。
首先是arduino,安装好rosserial后确保一切正常,下面这个是详细且成功率高的安装方法。
https://www.intorobotics.com/installing-and-setting-up-arduino-with-ros-kinetic-raspberry-pi-3/
但是这种方法在Ubuntu18.04下有bug
另一种方法是从arduino官网上下载,解压出来后运行install.sh
之后再从Library Manager中安装rosserial库。

安装好之后可以先试试example里的控制舵机的程序(用第二种方法没有这段代码)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif

#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16.h>

ros::NodeHandle nh;

Servo servo;

void servo_cb( const std_msgs::UInt16& cmd_msg){
servo.write(cmd_msg.data); //set servo angle, should be from 0-180
digitalWrite(13, HIGH-digitalRead(13)); //toggle led
}


ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);

void setup(){
pinMode(13, OUTPUT);

nh.initNode();
nh.subscribe(sub);

servo.attach(9); //attach it to pin 9
}

void loop(){
nh.spinOnce();
delay(1);
}

上述代码的主要内容是订阅“servo”这个话题获取一个int值,然后用它操作舵机旋转的角度。

然后使用rosserial,安装好rosserial之后,在终端运行

rosrun rosserial_python serial_node.py
Screenshot from 2018-10-20 10-46-03.png
之后在终端输入

rostopic list

检查是否有servo话题
Screenshot from 2018-10-20 11-00-04.png
然后在servo话题下手动发布消息进行测试,把“servo”打出来后不停按tab就行了,然后改一下data的值。

rostopic pub /servo std_msgs/UInt16 “data: 0”

如果成功的话舵机就会旋转到设定的那个角度。

URDF

这一篇内容的成果大概长这样,因为比较简单,手写比较方便。
显然,这是一个便宜的sg90舵机
视频地址: https://www.bilibili.com/video/av34229434/

关于urdf的介绍目前看到的最好的一篇介绍应该是ROSCon的这篇
https://youtu.be/g9WHxOpAUns
ROSWIKI上有完整的urdf教程,你可以根据教程自己写一些简单的机器人,实际上相当的麻烦而且容易出错,我以前从未想过用这种方法写一个机器人的模型出来,还以为和做游戏一样在Blender或者3dsmax这样的建模软件里搞定一切然后直接导出到rviz里就能摆弄了。实际上urdf也是可以的,但目前好像只有solidworks和matlab有这种插件,其中solidworks的插件叫做sw2urdf,我使用了之后发现还是相当方便的,至少比手写方便。

catkin_create_pkg <你要创建的package的名字> roscpp tf geometry_msgs urdf rviz xacro

创建好包之后我们来写urdf,目前好像没找到带urdf代码提示插件的文本编辑器,如果你使用带ros_qct_plugin的qtcreator的话里面可以新建urdf文件,但并没有提示功能。
下面具体来写这个简单的sg90舵机的urdf文件的过程
首先打开http://wiki.ros.org/urdf/XML
urdf是一种XML格式的文件,参考上面的链接找到你需要的东西,主要是各种link和joint。
上面这个舵机的模型由两个link和一个joint组成,下面蓝色的是舵机本体,上面红色的是买舵机的时候送的配件,非常便宜,随处可见。
不能在urdf里直接写出它们模型,而是选择mesh型的link,从别的地方引用一个stl格式的模型。这里是用Blender简单的做了一个sg90的外形。
做的时候看了一下比例,Blender里默认的box大小是2m2m2m,在urdf里直接写一个box型的link,size设置成“2 2 2”,两者的大小是一样的。如果懒得做模型的话直接搞两个box替代就好了。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
<?xml version="1.0"?>
<robot name="my_01">
<link name="base_link">
<visual>
<geometry>
<mesh filename="package://my_robot/meshes/servo_9g.stl" size="1 1 1" />
</geometry>
<material name="blue">
<color rgba="0 31 255 1"/>
</material>
<origin rpy="0 0 0" xyz="0 0 0" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://my_robot/meshes/servo_9g.stl" size="1 1 1" />
</geometry>
</collision>
</link>

<joint name="base_link_to_link_01" type="revolute">
<parent link="base_link"/>
<child link="link_01" />
<origin xyz="0.5 0 1.4" />
<axis xyz="0 0 -1" />
<limit effort="2.5" velocity="0.1" lower="-1.57" upper="1.57" />
<!-- <dynamics damping="0" friction="0" /> -->
</joint>

<link name="link_01">
<visual>
<geometry>
<mesh filename="package://my_robot/meshes/servo_9g_bar.stl" size="1 1 1" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 0" />
</visual>
<collision>
<geometry>
<mesh filename="package://my_robot/meshes/servo_9g_bar.stl" size="1 1 1" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 0" />
</collision>
</link>
</robot>

其中joint是比较关键的部分,joint有很多种,比较常见的有

  • continuous 可以一直转下去
  • revolute 舵机一般用这种,可以从一个角度转到另一个角度,limit里设置一下就好了
  • prismatic 沿着一个导轨平移的东西就用它
  • fixed 什么也不干,只是把两个link就这么接在一起

这里值得注意的是joint的

1
<limit effort="2.5" velocity="0.1" lower="-1.57" upper="1.57" />

我一度认为effort和velocity就是下图的torque和speed
sg90_datasheet.pdf
然而好像并不是,effort是百分数,velocity是每秒转动的弧度。
所以目前填什么其实都无所谓,现阶段arduino只接收position就行了。

这两个link实际上是从外部导入的stl格式的3d模型,但很多3d模型很复杂,所以有英伟达显卡的话最好能安装一下驱动程序,另外要准备一份低模,也就是面数比较低的模型,俗称low-poly,作为机器人的碰撞体collision,外形和尺寸要和你的用于显示的高模差不多,origin当然也要设置成一样的。

另外需要注意的是joint的位置和旋转的轴,这点在上面那个ROSCon的视频里有比较直观的讲解。写的时候脑子未必能转的过来,需要时不时打开rviz查看模型是否正确,所以这个时候就需要写一个launch文件来启动rviz。这里顺便一说atom有一个ros代码提示的package,可以用来写launch文件,大概长这样:
atom-ros

1
2
3
4
5
6
7
8
<launch>
<arg name="model" />
<param name="robot_description" textfile="$(find my_robot)/urdf/my01.urdf" />
<param name="use_gui" value="true"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" />
</launch>

这里值得注意的是joint_state_publisher 和 robot_state_publisher
前者用于发布joint_state信息,大概长这样:
Header header
string[] name
float64[] position
float64[] velocity
float64[] effort
http://docs.ros.org/melodic/api/sensor_msgs/html/msg/JointState.html
现阶段我们在arduino中订阅joint_state这个话题就能获得上面这些信息,比如我们可以将位于”0”的舵机旋转到msg.position[0]的位置。

当初在YouTube上看到了这个视频
https://youtu.be/6OjYXSEfVJ4
问了作者之后发现原来只要订阅joint_state就行了。现在我们稍微修改一下ros_lib里控制舵机的示例代码来测试一下这个想法。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif

#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/JointState.h>
ros::NodeHandle nh;
//float pos=0;
Servo servo;
void servo_cb(const sensor_msgs::JointState& msg)
{
float pos=msg.position[0];
servo.write(90-pos*57.3248);
}
ros::Subscriber<sensor_msgs::JointState> sub("joint_states",servo_cb);


void setup(){
//Serial.begin(57600);
nh.initNode();
nh.subscribe(sub);
servo.attach(9); //attach it to pin 9
}

void loop(){
nh.spinOnce();

delay(1);
}

于此同时,在launch文件里启动joint_state_publisher之外还要设置参数

1
<param name="use_gui" value="true"/>

这样运行的时候就会有GUI你可以用上面的slider来控制之前在joint->limit里设置的角度,这里是从-1.57到1.57,原因是它是一个180度的舵机。
然后我们使用刚刚的launch文件进入rviz
DISPLAY
首先把Fixed Frame设置成我们的base_link,然后添加RobotModel,其中Robot Description就是我们刚刚写的urdf。
滑动刚刚那个slider的话就可以让上面这个红色的配件沿着事先设定好的轴旋转,这个在urdf的joint中设定。
值得一提的是每个link还有joint都有

1
<origin rpy="0 0 0" xyz="0 0 0" />

child link的origin实际上是相对于它的parent link的,如果发生了问题,比如上面的配件转的莫名奇妙的,那可能是joint的origin和axis没有设置好,这点在从solidworks导出的时候也有可能出现(老年痴呆),所以掌握了手写urdf的技巧的话就不用重新导出好几遍了。

现在,无论是rviz里的3d模型还是现实中的sg90舵机都订阅了joint_states,它们会同步旋转。
https://www.bilibili.com/video/av34229434/

下一篇可能是讲如何使用sw2urdf,直接从solidworks里导出urdf文件(我反正是不想手写,xacro也不想写)
solidworks

SW2URDF

上回说到手写urdf是一件有些令人感到悲伤的事情,所以我想到了用solidworks直接导出urdf的插件——SW2URDF。
在此之前我做机械建模的aibo一直用UG,我强迫他学了solidworks。然后装了这个插件。

从aibo手上拿到的文件是这样的:

sw2urdf的安装官网上已经给出http://wiki.ros.org/sw_urdf_exporter,这里不赘述,下载好之后双击运行一步步点下去就行了,界面要切换成英语,不然没法使用

这里我们主要是要导出装配体。
首先第一步是添加Axis 和 Coordinate System,也就是轴线和坐标系。
这个主要是为joint添加的,相当于joint下的axis和origin这两个属性。
可以在参考图形中选择它们。

  • 第一步,添加axis
    选择Axis
    axis
    image.png
    选好之后点击绿色的√,就会出现蓝色的Axis,嫌短的话可以手动拉长,但其实没有影响。
    image.png
    然后选中这个原点,点击之后变为蓝色,确保这个原点在Axis上
    image.png
    这之后添加坐标系,如果没有这个原点的话要先添加点
    image.png
    image.png
    确认了xyz轴符合要求之后就可以点√了,如果要调整的话可以选这个模型上的其他面来调整。现在我们有了坐标系和轴,也就确定了这个joint的位置和旋转。按照这个方法把其他的joint也都做了,就可以进行下一步了。
    image.png
    image.png
    image.png
    image.png
    image.png
    image.png
    全部都配置好之后点击这个
    image.png
    image.png

image.png
全好了之后点Finish,就会生成一个包
image.png
然后把它拷到你的工作空间中,运行这个display.launch
Screenshot from 2018-10-21 12-37-05.png
Screenshot from 2018-10-21 12-36-32.png
按照上面的样子设置好DISPLAY就行了,然后把launch文件里的usegui改成true,就能拖动slider发布jointstate消息了。

Rosserial PCA9685

上一篇开始,我们有了一个从SolidWorks中导出的package,其中已经有了display.launch供我们查看模型,同时,我们把use_gui设置为True之后,就可以用slider发布joint_states上的message了。

  • 一开始的方案是:
    由arduino订阅joint_state话题
    获取position[0] , position[1] , position[2]…….position[5],是一个float64的数组
    获取的position在-1.57~1.57之间,使用map将其变为0~180
    servo.write(position)
    Compact Message Definition

但是现在我们有总共6个舵机用于机械臂,还有一个舵机用在末端执行器上,不希望全部接在一个arduino uno上,所以使用便宜的PCA9685舵机驱动板。
连接方法
然后再给它提供一个5~6V的电源就行了。

之后下载# Adafruit-PWM-Servo-Driver-Library
如果是用之前的篇章提供的方法安装的arduino-ide的话,解压后需要将名字中的【-】符号替换成【_】,再放入sketchbook/libraries 中,不然可能会出现问题。

安装好之后可以运行其中的示例代码进行测试,另外还需要写一个函数将pulse映射成angle。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 555// this is the 'maximum' pulse length count (out of 4096)

uint8_t servonum = 0;

void setup() {
Serial.begin(9600);
Serial.println("8 channel Servo test!");

pwm.begin();

pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates

delay(10);
}


void loop() {

pwm.setPWM(servonum, 0, servo_write(90));
delay(1);
servonum++;
if(servonum>=7)
servonum=0;
}

int servo_write(int angle)
{
int pulse=map(angle,0,180,SERVOMIN,SERVOMAX);
Serial.print("Angle: ");
Serial.println(angle);
Serial.print("pulse: ");
Serial.println(pulse);

return pulse;
}

这里值得注意的是

1
2
#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 555// this is the 'maximum' pulse length count (out of 4096)

和预设的不同,需要你手动调整一下,具体方法是插上一个舵机,用它赠送的配件做一个指针,利用下面这行代码让舵机转到最小值或最大值的极限,比如140,130,或600,620,直到它转不动为止,然后将这个值重新设置为上面的SERVOMIN和SERVOMAX,但是我们购买的180度的MG996R实际上比180要稍微大一点,这里我们就把SERVOMIN和SERVOMAX还是设置成让它正好从0到180的pulse。
pwm.setPWM(servonum, 0,<你认为的最小值或最大值>);

to be continue…