采集命令

rosrun baxter_moveit_config endpoint_recorder.py -f filename #此处filename为要保存的文件名，后面还可加上采集频率，但是采集频率过高会造成在轨迹复原的时候，机械臂抖动的特别厉害，建议20HZ就可以

代码

#!/usr/bin/env python
import argparse
import rospy
import baxter_interface
from baxter_interface import CHECK_VERSION
class JointRecorder(object):def __init__(self, filename, rate):"""Records joint data to a file at a specified rate."""self._filename = filenameself._raw_rate = rateself._rate = rospy.Rate(rate)self._start_time = rospy.get_time()self._done = Falseself._limb_left = baxter_interface.Limb("left")self._limb_right = baxter_interface.Limb("right")def _time_stamp(self):return rospy.get_time() - self._start_timedef stop(self):"""Stop recording."""self._done = Truedef done(self):"""Return whether or not recording is done."""if rospy.is_shutdown():self.stop()return self._donedef record(self):if self._filename:joints_left = self._limb_left.joint_names()joints_right = self._limb_right.joint_names()with open(self._filename, 'w') as f:f.write('time,')f.write(','.join([j for j in joints_left]) + ',')f.write(','.join([j for j in joints_right]) + '\n')while not self.done():current_pose = self._limb_left.endpoint_pose()endpoint_pose_position_left = [current_pose['position'].x,current_pose['position'].y,current_pose['position'].z]endpoint_pose_orientation_left = [current_pose['orientation'].x,current_pose['orientation'].y,current_pose['orientation'].z,current_pose['orientation'].w,]current_pose = self._limb_right.endpoint_pose()                      endpoint_pose_position_right = [current_pose['position'].x,current_pose['position'].y,current_pose['position'].z,]endpoint_pose_orientation_right = [current_pose['orientation'].x,current_pose['orientation'].y,current_pose['orientation'].z,current_pose['orientation'].w,]                     f.write("%f," % (self._time_stamp(),))f.write(','.join([str(x) for x in endpoint_pose_position_left]) + ',')f.write(','.join([str(x) for x in endpoint_pose_position_right]) + ',')f.write(','.join([str(x) for x in endpoint_pose_orientation_left]) + '\n')f.write(','.join([str(x) for x in endpoint_pose_orientation_right]) + '\n')self._rate.sleep()
def main():epilog = """
Related examples:joint_position_file_playback.py; joint_trajectory_file_playback.py."""arg_fmt = argparse.RawDescriptionHelpFormatterparser = argparse.ArgumentParser(formatter_class=arg_fmt,description=main.__doc__,epilog=epilog)required = parser.add_argument_group('required arguments')required.add_argument('-f', '--file', dest='filename', required=True,help='the file name to record to')parser.add_argument('-r', '--record-rate', type=int, default=20, metavar='RECORDRATE',help='rate at which to record (default: 100)')args = parser.parse_args(rospy.myargv()[1:])print("Initializing node... ")rospy.init_node("rsdk_joint_recorder")print("Getting robot state... ")rs = baxter_interface.RobotEnable(CHECK_VERSION)print("Enabling robot... ")rs.enable()Recorder = JointRecorder(args.filename, args.record_rate)rospy.on_shutdown(Recorder.stop)print("Recording. Press Ctrl-C to stop.")Recorder.record()print("\nDone.")
if __name__ == '__main__':main()

生成

ubuntu下生成的是txt文件，画图需要转化为csv。

图片:

轨迹还原

调用方式为：

调用命令：rosrun baxter_moveit_config endpoint_trajectory_playback.py -f filename joint_states:=/robot/joint_states #此处filename为存储的末端轨迹数据的文件名

代码

#!/usr/bin/env python
import rospy,sys
import argparse
import moveit_commander
import numpy as np
from geometry_msgs.msg import PoseStamped,Pose
from moveit_commander import MoveGroupCommander
import time
from copy import deepcopy
from os import path
class MoveItDemo:def __init__ (self):moveit_commander.roscpp_initialize(sys.argv)self.left_arm  = moveit_commander.MoveGroupCommander( 'left_arm')self.right_arm = moveit_commander.MoveGroupCommander('right_arm')self.right_eef = self.right_arm.get_end_effector_link()self.left_eef = self.left_arm.get_end_effector_link()self.right_arm.allow_replanning(True)self.left_arm.allow_replanning(True)self.right_arm.set_goal_position_tolerance(0.5)self.right_arm.set_goal_orientation_tolerance(0.5)self.left_arm.set_goal_position_tolerance(0.1)self.left_arm.set_goal_orientation_tolerance(0.1)self.left_arm.set_planning_time(5)self.right_arm.set_planning_time(5)def execute(self,filename):with open(filename, 'r') as f:lines = f.readlines()waypoints1 = []waypoints2 = []waypoints_l_start = []waypoints_r_start = []i = 1for idx, values in enumerate(lines[1:]):print "Line:\n", ii = i + 1if i == len(lines):breakvalues = values.rstrip().split(',')values = [float(x) for x in values]print valuesif idx == 0:print idxstart_pose1 = self.left_arm.get_current_pose(self.left_eef).posewpose1 = deepcopy(start_pose1)wpose1.position.x = values[1]wpose1.position.y= values[2]wpose1.position.z = values[3]wpose1.orientation.x = values[7]wpose1.orientation.y = values[8]wpose1.orientation.z = values[9]wpose1.orientation.w = values[10]start_pose2 = self.right_arm.get_current_pose(self.right_eef).posewpose2 = deepcopy(start_pose2)wpose2.position.x = values[4]wpose2.position.y = values[5]wpose2.position.z = values[6]wpose2.orientation.x = values[11]wpose2.orientation.y = values[12]wpose2.orientation.z = values[13]wpose2.orientation.w = values[14]waypoints_l_start.append(deepcopy(start_pose1))waypoints_l_start.append(deepcopy(wpose1))waypoints_r_start.append(deepcopy(start_pose2))waypoints_r_start.append(deepcopy(wpose2))wpose1.position.x = values[1]wpose1.position.y = values[2]wpose1.position.z = values[3]wpose1.orientation.x = values[7]wpose1.orientation.y = values[8]wpose1.orientation.z = values[9]wpose1.orientation.w = values[10]waypoints1.append(deepcopy(wpose1))wpose2.position.x = values[4]wpose2.position.y = values[5]wpose2.position.z = values[6]wpose2.orientation.x = values[11]wpose2.orientation.y = values[12]wpose2.orientation.z = values[13]wpose2.orientation.w = values[14]waypoints2.append(deepcopy(wpose2))print "To start pose\n"(cartesian_plan_l,fraction) = self.left_arm.compute_cartesian_path(waypoints_l_start,0.01, 0.0, True)self.left_arm.execute(cartesian_plan_l)(cartesian_plan_r,fraction) = self.right_arm.compute_cartesian_path(waypoints_r_start,0.01, 0.0, True)self.right_arm.execute(cartesian_plan_r)rospy.sleep(3)print "start playback\n"(cartesian_plan_l,fraction) = self.left_arm.compute_cartesian_path(waypoints1,0.01, 0.0, True)self.left_arm.execute(cartesian_plan_l)(cartesian_plan_r,fraction) = self.right_arm.compute_cartesian_path(waypoints2,0.01, 0.0, True)self.right_arm.execute(cartesian_plan_r)rospy.sleep(3)print("Done.")moveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)
def main():print("Initializing node... ")rospy.init_node('moveit_demo')rospy.sleep(1)epilog = """
Related examples:joint_recorder.py; joint_position_file_playback.py."""arg_fmt = argparse.RawDescriptionHelpFormatterparser = argparse.ArgumentParser(formatter_class=arg_fmt,description=main.__doc__,epilog=epilog)parser.add_argument('-f', '--file', metavar='PATH', required=True,help='path to input file')args = parser.parse_args(rospy.myargv()[1:])moveit = MoveItDemo()moveit.execute(path.expanduser(args.file))
if __name__ == "__main__":main()

参考

[1]: 链接: link.

【Baxter机器人末端轨迹坐标采集和轨迹还原】相关推荐

ros moveit进行机器人末端轨迹移动
ros moveit进行机器人末端轨迹移动参考链接: ROS moveit 机械臂笛卡尔空间运动 ROS moveit 机械臂逆运动机械臂末端直线运动: #!/usr/bin/env python ...
matlab如何看机器人末端坐标,关于RobotStudio中机器人末端位置的MATLAB仿真验证
关于RobotStudio中机器人末端位置的MATLAB仿真验证最近事情贼多,感觉有点乏力. 主要是最近在着手写一篇关于机器人轨迹规划的文章.随之而来的,当然是一堆的仿真,以及实验平台的搭建,还要想 ...
遨博协作机器人ROS开发 - 机械臂复杂轨迹规划
目录一.简介二.环境版本三.学习目标四.知识储备五.任务实施六.任务拓展七.课堂小结八.课后练习一.简介大家好,欢迎关注遨博学院带来的系列技术分享文章(协作机器人ROS开发),今天 ...
Matlab机械臂综合仿真平台，包含运动学、动力学和控制。 MATLAB机器人仿真正逆运动学simulink轨迹规划机械臂动力学控制等
Matlab机械臂综合仿真平台,包含运动学.动力学和控制. MATLAB机器人仿真正逆运动学simulink轨迹规划机械臂动力学控制等 gui控制仿真平台PUMA机器人 robotics toolb ...
地理围栏 | EXCEL表格中分析轨迹坐标是否在设定的围栏范围内
1 前言地理围栏(Geo-fencing)是LBS的一种新应用,就是用一个虚拟的栅栏围出一个虚拟地理边界.当手机进入.离开某个特定地理区域,或在该区域内活动时,手机可以接收自动通知和警告.有了地理围 ...
【四足机器人】学习笔记足端轨迹规划和步态规划
[四足机器人]学习笔记足端轨迹规划和步态规划一.足端轨迹规划(摆线) 二.步态规划 1.Walk步态 2.Trot步态近期,博主在古月居学习关于四足机器人的相关部分知识,从阳炼老师的四足机器人控 ...
LiveGBS国标GB/T28181国标平台功能-电子地图移动位置订阅mobileposition地图定位GPS轨迹坐标位置获取redis获取位置
LiveGBS国标GB/T28181国标平台功能-电子地图移动位置订阅mobileposition地图定位GPS轨迹坐标位置获取redis获取位置 1.位置订阅 1.1.国标设备编辑 1.2.选择设备 ...
python 轨迹预测_CVPR 2019轨迹预测竞赛冠军方法总结
背景 CVPR 2019 是机器视觉方向最重要的学术会议,本届大会共吸引了来自全世界各地共计 5160 篇论文,共接收 1294 篇论文,投稿数量和接受数量都创下了历史新高,其中与自动驾驶相关的论文. ...
matlab 根轨迹虚轴交点,根轨迹与虚轴的交点.ppt
根轨迹与虚轴的交点.ppt 第八.九周实验安排第8周时间待定第9周时间待定设一单位反馈控制系统的开环传递函数为: ,绘制该系统的根轨迹. 4．分离点: 得: 5．与虚轴交点: School ...

【Baxter机器人末端轨迹坐标采集和轨迹还原】

Baxter机器人末端轨迹坐标采集和轨迹还原

采集命令

代码

生成

轨迹还原

代码

参考

【Baxter机器人末端轨迹坐标采集和轨迹还原】相关推荐

最新文章

热门文章