本文主要介绍如何仿真一个scara机械臂,以及在网上看了一些项目以后,有了一些感想,不想看的可以直接跳到机械臂部分。
目录
- 感想(自己的理解,不一定对。)
- Scara机械臂的开发
- 运动学计算
- 如何使用
- 机械臂工作图
- 一个例子: 在start_pose抓起货物,在end_pose放置货物
- 一些实现的细节
- 1 通过ros控制gazebo中的机器人
- 1.1 使用python调用gazebo的服务
- 1.2 joint命名问题
感想(自己的理解,不一定对。)
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的名称在加载机器人以后是固定的。