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

本文主要介绍如何仿真一个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()

运动学计算

机械臂的参数: 大臂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()

完整的项目地址：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] # 猜测是一个角速度

至于如何设置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>	

而当我在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>

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

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