使用gazebo对scara机械臂进行仿真_gazebo机械臂仿真

本文主要介绍如何仿真一个scara机械臂，以及在网上看了一些项目以后，有了一些感想，不想看的可以直接跳到机械臂部分。

感想(自己的理解，不一定对。)

ros控制gazebo中机器人的方式:
为了控制gazebo中的机器人运动, ros node需要调用gazebo的一些服务service，具体的方法在一些实现的细节中有写

但是人家项目里面ros_controller是怎么回事? 我的理解是：调用gazebo的服务其实非常不好用，因为你的需求可能是想让机械臂运动到某个点，但是你输入的只能是力，因此需要自己根据机械臂的位置计算出力的大小。

因此诞生了ros_controller，它通过gazebo包的形式，指定具体的关节，开发者只需要提供目标位置，可以自动帮你计算出力来。调用的方法是：把gazebo的service映射到了topic上，开发者只需要订阅topic，或者发布topic就可以间接使用gazebo的服务。

对于复杂的joint，你无法自己计算出力，必须通过ros_controller来。但是为什么本项目没有使用ros_controller呢? 因为本项目的机械臂非常简单，完全可以自己手算出来力的大小。

当然对于不使用ros_controller也有好处：

• 开发简单，不用配置gazebo包，以及yam文件等等
• 不用通过topic对机器人控制，直接通过变量传参，更加便捷
• 对机器人理解更加深刻，控制起来得心应手

Scara机械臂的开发

其实是借鉴了这个大佬的项目：https://github.com/yangliu28/two_scara_collaboration

首先是封装了一下该机器人的一些接口：

#! /usr/bin/env python
# -*- coding: utf-8 -*
import rospy
import numpy as np
from joint import Joint

class ScaraInterface:

    def __init__(self, controller_name, robot_name, robot_pose, r1_pose, r2_pose):
        """
        创建scara控制器

        Input:  controller_name - 控制器节点名称
                robot_name - 控制器对应的机械臂名称
                robot_pose - scara机械臂所在位置
                r1_pose    - rotation1的初始位置
                r2_pose    - rotation2的初始位置    
        """
        rospy.init_node(controller_name)
        
        # 关节控制的参数
        self.kps = [80, 1]
        self.kvs = [16, 0.2]
        self.GRIPPER_UP_EFFORT = 0.001
        self.GRIPPER_DOWN_EFFORT = 0.0
        self.GRIPPER_GRASP_EFFORT = 0.002
        self.GRIPPER_RELEASE_EFFORT = 0.0001
        
        self.robot_name = robot_name # 机器人名字
        self.robot_pose = robot_pose # 机器人所在的位置
        
        # 初始化joint信息
        self.joints={
            # rotation joint
            "rotation1": Joint("{}::rotation1".format(self.robot_name), init_pose=r1_pose),
            "rotation2": Joint("{}::rotation2".format(self.robot_name), init_pose=r2_pose),
            # joint control gripper up
            "gripper": Joint("{}::gripper_joint".format(self.robot_name), init_effort=self.GRIPPER_UP_EFFORT, duration=0.25),
            # joint control gripper fingers
            "finger1": Joint("{}::finger1_joint".format(self.robot_name), init_effort=self.GRIPPER_RELEASE_EFFORT, duration=0.25),
            "finger2": Joint("{}::finger2_joint".format(self.robot_name), init_effort=-self.GRIPPER_RELEASE_EFFORT, duration=0.25),
            "finger3": Joint("{}::finger3_joint".format(self.robot_name), init_effort=-self.GRIPPER_RELEASE_EFFORT, duration=0.25),
            "finger4": Joint("{}::finger4_joint".format(self.robot_name), init_effort=self.GRIPPER_RELEASE_EFFORT, duration=0.25),
        }

    
    def move_to(self, pose):
        """
        移动gripper到指定的pose

        Input  pose - 移动的目标
        """
        # 计算相对坐标, 不清楚两个为什么是反着减
        x = self.robot_pose.position.x - pose.position.x
        y = self.robot_pose.position.y - pose.position.y
        dist_square = x*x + y*y # 目标到机器人中心的距离平方
        # 余弦定理计算出两个joint的转动角度, scara和中心连接的手臂长度为1, 另一个手臂长度为0.8
        angles = [
            np.arctan(np.divide(y,x)) - np.arccos((0.36 + dist_square)/(2*np.sqrt(dist_square))),
            np.pi - np.arccos((1.64 - dist_square)/1.6)
        ]
        # add robust to this inverse kinematics
        if np.isnan(angles).any():
            angles = [np.arctan(y/x), 0]
               
         # 发布joint需要旋转的角度
        for i,name in enumerate(["rotation1", "rotation2"]):
            pose_err = angles[i] - self.joints[name].cur_pose
            effort = self.kps[i] * pose_err - self.kvs[i] * self.joints[name].cur_rate
            self.joints[name].set_effort(effort)
            self.joints[name].publish()

    def move_down(self):
        """
        向下移动gripper
        """
        self.joints["gripper"].set_effort(self.GRIPPER_DOWN_EFFORT)
        pass
    
    def move_up(self):
        """
        向上移动gripper
        """
        self.joints["gripper"].set_effort(self.GRIPPER_UP_EFFORT)

    def grasp(self):
        self.joints["finger1"].set_effort(-self.GRIPPER_GRASP_EFFORT)
        self.joints["finger2"].set_effort(self.GRIPPER_GRASP_EFFORT)
        self.joints["finger3"].set_effort(self.GRIPPER_GRASP_EFFORT)
        self.joints["finger4"].set_effort(-self.GRIPPER_GRASP_EFFORT)

    def release(self):
        self.joints["finger1"].set_effort(self.GRIPPER_RELEASE_EFFORT)
        self.joints["finger2"].set_effort(-self.GRIPPER_RELEASE_EFFORT)
        self.joints["finger3"].set_effort(-self.GRIPPER_RELEASE_EFFORT)
        self.joints["finger4"].set_effort(self.GRIPPER_RELEASE_EFFORT)

    def update(self):
        """
        更新rotation_joint的位置, 发布gripper_joint和finger_joint的力
        """
        # 更新joint目前的位置
        for joint_name in ["rotation1", "rotation2"]:
            self.joints[joint_name].update_state()
        
        # 持续发布joint effort
        for joint_name in ["gripper", "finger1", "finger2", "finger3", "finger4"]:
            self.joints[joint_name].publish()
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
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108

运动学计算

机械臂的参数: 大臂1m, 小臂0.8m
其中move_to(pose)表示了把机械臂的一端移动到pose这个位置，这里是直接计算了大臂到x轴的夹角，小臂到大臂的夹角。然后比较当前角度和目标角度的差，作为力的大小的kp倍。
另外力还和速度有关，当机械臂一端运动到了目标以后，需要保证速度为0，因此力其实和速度成反比，和位置偏差成正比。具体的角度计算:

如何使用

• 先继承ScaraInterface，
• 在每个主循环都调用self.update()，更新joint的位置
• 在需要机械臂运动的时候，调用move_to，
• 在需要机械臂上升的时候调用move_up，在下降的时候调用move_down
• 在需要抓起的时候调用grasp，在需要松开的时候调用release

机械臂工作图

抓起货物

搬运货物

一个例子: 在start_pose抓起货物，在end_pose放置货物

#! /usr/bin/env python
# -*- coding: utf-8 -*
import rospy
import argparse
from enum import IntEnum
from scara_gazebo.msg import Poses
from geometry_msgs.msg import Pose, Point
from scara_interface import ScaraInterface

class state(IntEnum):
    waiting = 0
    move_forth = 1
    down_forth = 2
    grasp = 3
    up_forth = 4
    move_back = 5
    down_back = 6
    release = 7
    up_back = 8

class ScaraController(ScaraInterface):

    def __init__(self, controller_name, robot_name, robot_pose, \
                    start_pose, end_pose, r1_pose, r2_pose):
        """
        创建scara控制器

        Input:  controller_name - 控制器节点名称
                robot_name - 控制器对应的机械臂名称
                robot_pose - scara机械臂所在位置
                start_pose - 开始搬运的位置
                end_pose   - 结束搬运的位置
                r1_pose    - rotation1的初始位置
                r2_pose    - rotation2的初始位置    
        """
        ScaraInterface.__init__(self, controller_name, robot_name, robot_pose, \
                                    r1_pose, r2_pose)
              
        self.start_pose = start_pose # 开始搬运的位置
        self.end_pose = end_pose # 结束搬运的位置
        self.target_loop_num = 0 # 目标循环次数
        self.cur_loop_num = 0 # 计算循环次数 
        self.cur_state = state.waiting # 当前的状态
        self.cur_action = self.wait # 当前状态执行的函数
        self.time_unit = 0.01 # 每次循环的时间单位
        self.waiting = False # 是否在等待物体到达
        
        # 状态表, next_state, next_action, duration
        self.func_tbl = [
            (state.move_forth, self.move_forth, 1.50),
            (state.down_forth, self.move_down , 0.25),
            (state.grasp     , self.grasp     , 0.05),
            (state.up_forth  , self.move_up   , 0.25),
            (state.move_back , self.move_back , 1.50),
            (state.down_back , self.move_down , 0.25),
            (state.release   , self.release   , 0.05),
            (state.up_back   , self.move_up   , 0.25),
            (state.waiting   , self.wait      , 0.00),
        ]
        
        # 订阅位置信息，注册回调
        rospy.Subscriber('/rfid_tags', Poses, self.callback)

    def wait(self):
        pass

    def move_forth(self):
        self.move_to(self.end_pose)

    def move_back(self):
        self.move_to(self.start_pose)
        
    def callback(self, poses):
        for pose in poses:
            if pose.position == self.target_pose:
                self.waiting = False
                break

    def start(self):
        """
        开始主循环
        """
        while not rospy.is_shutdown():
            if self.cur_state == state.waiting and self.waiting: # 没有物体到达target_pose，等待
                pass
            else:
                if self.cur_state == state.waiting and not self.waiting:
                    self.waiting = True # 物体到达start_pose, 第一次进入循环，重新设置waiting信号

                if self.cur_loop_num == self.target_loop_num: # 到达该状态的循环次数，则更新状态
                    self.cur_state, self.cur_action, time_cost = self.func_tbl[self.cur_state]
                    self.target_loop_num = time_cost//self.time_unit
                    self.cur_loop_num = 0
                else:
                    self.cur_action()
                    self.cur_loop_num += 1
            
            self.update()

            rospy.loginfo(self.cur_state)
            rospy.sleep(self.time_unit)

if __name__ == "__main__":
    parser = argparse.ArgumentParser(description='argparse for scara_controller')
    parser.add_argument('-cn', type=str, default="scara_controller", help="name of the controller node")
    parser.add_argument('-rn', type=str, default="scara_robot1", help="name of the robot")
    parser.add_argument('-rpx', type=float, default="0.0", help="robot_pose.position.x")
    parser.add_argument('-rpy', type=float, default="0.0", help="robot_pose.position.y")
    parser.add_argument('-rpz', type=float, default="0.0", help="robot_pose.position.z")
    parser.add_argument('-spx', type=float, default="1.5", help="start_pose.position.x")
    parser.add_argument('-spy', type=float, default="0.0", help="start_pose.position.y")
    parser.add_argument('-spz', type=float, default="0.0", help="start_pose.position.z")
    parser.add_argument('-epx', type=float, default="0.0", help="end_pose.position.x")
    parser.add_argument('-epy', type=float, default="1.5", help="robot_pose.position.y")
    parser.add_argument('-epz', type=float, default="0.0", help="robot_pose.position.z")
    parser.add_argument('-r1p', type=float, default="-0.78", help="rotation1_joint init angle")
    parser.add_argument('-r2p', type=float, default="2.1", help="rotation2_joint init angle")
    
    myargv = rospy.myargv()
    args = parser.parse_args(myargv[1:])
    
    scara_controller = \
        ScaraController(
            controller_name=args.cn, 
            robot_name=args.rn, 
            robot_pose=Pose(position=Point(args.rpx, args.rpy, args.rpz)),
            start_pose=Pose(position=Point(args.spx, args.spy, args.spz)),
            end_pose=Pose(position=Point(args.epx, args.epy, args.epz)),
            r1_pose=args.r1p,
            r2_pose=args.r2p
        )
    scara_controller.start()

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
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133

完整的项目地址：https://github.com/HGGshiwo/scara_gazebo.git

接下来我打算学习一下如何使用ros_control，毕竟机器人太复杂的时候，或者说用别人的模型的时候，往往会用到ros_conrol。

一些实现的细节

1 通过ros控制gazebo中的机器人

为了控制gazebo中的机器人运动, ros node需要调用gazebo的一些服务service。具体的服务包括：

• 设置施加在joint的力
• 查询joint的移动速度以及位置
• 其他等等

完整的gazebo支持的服务可以在gazebo_ros查看

• 网址是 https://github.com/ros-simulation/gazebo_ros_pkgs， 在里面的gazebo_msgs里有完整的gazebo支持的消息。
• 文档说明是: http://docs.ros.org/en/jade/api/gazebo_msgs/html/index-msg.html

1.1 使用python调用gazebo的服务

一个完整的获取gazebo中查询关节速度和位置的服务的例子:

from gazebo_msgs.srv import GetJointProperties, GetJointPropertiesRequest
get_property_proxy = rospy.ServiceProxy( # 获取joint状态的代理
            name="/gazebo/get_joint_properties", 
            service_class=GetJointProperties
        )
rospy.wait_for_service("/gazebo/get_joint_properties")
property_msg = GetJointPropertiesRequest() # 配置一个joint信息的msg
property_msg.joint_name = joint_name 
response = get_property_proxy.call(property_msg) # 生成一个获取joint状态的请求
cur_pose = response.position[0] # 猜测是一个角度
cur_rate = response.rate[0] # 猜测是一个角速度
1
2
3
4
5
6
7
8
9
10
11

至于如何设置joint的力，和这个非常类似。

1.2 joint命名问题

如何确定joint_name呢? 经过检测，如果使用spawn_model脚本添加的模型，gazebo中的joint以及link的名字格式是：model_name::link_name，其中model_name是spawn_model脚本-model 之后填写的参数。而和在urdf里面填写的robot_name以及spawn_model的node name没有关系。

比如我的机器人name是sara_robot，urdf文件是:

<?xml version="1.0"?>
<robot name="scara_robot">
	<link name="base_link">
	....
	</link>
	<joint name="rotation1">
	 ....
	</joint>
</robot>	
1
2
3
4
5
6
7
8
9

而当我在launch中添加以后：

<launch>
  <node name="scara_robot1" pkg="gazebo_ros" type="spawn_model" args="-file $(find scara_controller)/urdf/scara_robot.urdf -urdf -model scara_robot2" />
</launch>
1
2
3

那么实际的机器人jointt中的rotate1名称为：scara_robot2::rotate1，下图为证：

而使用命名空间是不管用的，因为命名空间只是分割了消息，而joint的名称在加载机器人以后是固定的。