Baxter机器人在ros下，机械臂采集末端姿态并且轨迹还原
一、采集数据：
rosrun 采集程序，例如：joint_recoder.py
rosrun baxter_examples joint_recorder.py -f example_file #example_file 是你要保存的文件
#!/usr/bin/env python
# Copyright (c) 2013-2015, Rethink Robotics
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# 3. Neither the name of the Rethink Robotics nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
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 = filename
self._raw_rate = rate
self._rate = rospy.Rate(rate)
self._start_time = rospy.get_time()
self._done = False
self._limb_left = baxter_interface.Limb("left")
self._limb_right = baxter_interface.Limb("right")
def _time_stamp(self):
return rospy.get_time() - self._start_time
def stop(self):
"""
Stop recording.
"""
self._done = True
def done(self):
"""
Return whether or not recording is done.
"""
if rospy.is_shutdown():
self.stop()
return self._done
def record(self):
"""
Records the current joint positions to a csv file if outputFilename was
provided at construction this function will record the latest set of
joint angles in a csv format.
This function does not test to see if a file exists and will overwrite
existing files.
"""
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]) + ',')
while not self.done():
current_pose = self._limb_left.endpoint_pose()
endpoint_pose_left = [current_pose['position'].x,
current_pose['position'].y,
current_pose['position'].z]
current_pose = self._limb_right.endpoint_pose()
endpoint_pose_right = [current_pose['position'].x,
current_pose['position'].y,
current_pose['position'].z,]
f.write("%f," % (self._time_stamp(),))
f.write(','.join([str(x) for x in endpoint_pose_left]) + ',')
f.write(','.join([str(x) for x in endpoint_pose_right]) + '\n')
self._rate.sleep()
def main():
"""RSDK Joint Recorder Example
Record timestamped joint and gripper positions to a file for
later play back.
Run this example while moving the robot's arms and grippers
to record a time series of joint and gripper positions to a
new csv file with the provided *filename*. This example can
be run in parallel with any other example or standalone
(moving the arms in zero-g mode while pressing the cuff
buttons to open/close grippers).
You can later play the movements back using one of the
*_file_playback examples.
"""
epilog = """
Related examples:
joint_position_file_playback.py; joint_trajectory_file_playback.py.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = 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=100, 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()二、数据还原：
首先rosrun baxter_interface joint_trajectory_action_server.py –mode velocity
然后在新的终端里面rosrun baxter_examples joint_trajectory_file_playback.py -f example_file #example_file 是你已经保存的文件
#!/usr/bin/env python
"""
Baxter RSDK Joint Trajectory Example: file playback
"""
import argparse
import operator
import sys
import threading
import struct
from bisect import bisect
from copy import copy
from os import path
import rospy
import actionlib
from control_msgs.msg import (
FollowJointTrajectoryAction,
FollowJointTrajectoryGoal,
)
from trajectory_msgs.msg import (
JointTrajectoryPoint,
)
from std_msgs.msg import Header
import baxter_interface
from baxter_interface import CHECK_VERSION
from baxter_core_msgs.srv import (
SolvePositionIK,
SolvePositionIKRequest,
)
from geometry_msgs.msg import (
PoseStamped,
Pose,
Point,
Quaternion,
)
class Trajectory(object):
def __init__(self):
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#verify joint trajectory action servers are available
l_server_up = self._left_client.wait_for_server(rospy.Duration(10.0))
r_server_up = self._right_client.wait_for_server(rospy.Duration(10.0))
if not l_server_up or not r_server_up:
msg = ("Action server not available."
" Verify action server availability.")
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
# Timing offset to prevent gripper playback before trajectory has started
self._slow_move_offset = 0.0
self._trajectory_start_offset = rospy.Duration(0.0)
self._trajectory_actual_offset = rospy.Duration(0.0)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
def _clean_line(self, line, joint_names):
#print "1 line\n",line
"""
Cleans a single line of recorded joint positions
@param line: the line described in a list to process
@param joint_names: joint name keys
@return command: returns dictionary {joint: value} of valid commands
@return line: returns list of current line values stripped of commas
"""
# print "joint_names\n",joint_names
def try_float(x):
try:
return float(x)
except ValueError:
return None
#convert the line of strings to a float or None
line = [try_float(x) for x in line[1:-1].rstrip().split(',')]
#zip the values with the joint names
# print "line\n",line
combined = zip(joint_names[1:], line[1:])
# print "combined\n",combined
#take out any tuples that have a none value
cleaned = [x for x in combined if x[1] is not None]
# print "cleaned\n",cleaned
#convert it to a dictionary with only valid commands
command = dict(cleaned)
return (command, line,)
def _add_point(self, positions, side, time):
"""
Appends trajectory with new point
@param positions: joint positions
@param side: limb to command point
@param time: time from start for point in seconds
"""
#creates a point in trajectory with time_from_start and positions
point = JointTrajectoryPoint()
point.positions = copy(positions)
point.time_from_start = rospy.Duration(time)
if side == 'left':
self._l_goal.trajectory.points.append(point)
elif side == 'right':
self._r_goal.trajectory.points.append(point)
def ik_request(self,pose,limb,verbose=True):
self._verbose = verbose
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
#print ikreq
try:
resp = self._iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return False
# Check if result valid, and type of seed ultimately used to get solution
# convert rospy's string representation of uint8[]'s to int's
resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type)
limb_joints = {}
if (resp_seeds[0] != resp.RESULT_INVALID):
seed_str = {
ikreq.SEED_USER: 'User Provided Seed',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp_seeds[0], 'None')
if self._verbose:
print("IK Solution SUCCESS - Valid Joint Solution Found from Seed Type: {0}".format(
(seed_str)))
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
if self._verbose:
print("IK Joint Solution:\n{0}".format(limb_joints))
print("------------------")
else:
rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
return False
return limb_joints
def parse_file(self, filename):
"""
Parses input file into FollowJointTrajectoryGoal format
@param filename: input filename
"""
#open recorded file
with open(filename, 'r') as f:
lines = f.readlines()
#read joint names specified in file
joint_names = lines[0].rstrip().split(',')
#parse joint names for the left and right limbs
i=1
for name in joint_names:
if 'left' == name[:-3]:
self._l_goal.trajectory.joint_names.append(name)
elif 'right' == name[:-3]:
self._r_goal.trajectory.joint_names.append(name)
def find_start_offset(pos):
#create empty lists
cur = []
cmd = []
dflt_vel = []
vel_param = self._param_ns + "%s_default_velocity"
#for all joints find our current and first commanded position
#reading default velocities from the parameter server if specified
# print 'pos\n',pos
for name in joint_names:
if 'left' == name[:-3]:
print name
cmd.append(pos[name])
cur.append(self._l_arm.joint_angle(name))
prm = rospy.get_param(vel_param % name, 0.25)
dflt_vel.append(prm)
elif 'right' == name[:-3]:
print name
cmd.append(pos[name])
cur.append(self._r_arm.joint_angle(name))
prm = rospy.get_param(vel_param % name, 0.25)
dflt_vel.append(prm)
diffs = map(operator.sub, cmd, cur)
diffs = map(operator.abs, diffs)
#determine the largest time offset necessary across all joints
offset = max(map(operator.div, diffs, dflt_vel))
return offset
for idx, values in enumerate(lines[1:]):
print "TIME:\n", i
i = i+1
#print values
if i == len(lines):
break
#clean each line of file
values = values.rstrip().split(',')
# print "idx:",idx
#print "values:\n",values
ik_pose1 = Pose()
ik_pose1.position.x = float(values[1])
ik_pose1.position.y = float(values[2])
ik_pose1.position.z = float(values[3])
ik_pose1.orientation.x = x=-0.0249590815779
ik_pose1.orientation.y = y=0.999649402929
ik_pose1.orientation.z = z=0.00737916180073
ik_pose1.orientation.w = w=0.00486450832011
ik_pose2 = Pose()
ik_pose2.position.x = float(values[4])
ik_pose2.position.y = float(values[5])
ik_pose2.position.z = float(values[6])
ik_pose2.orientation.x = x=-0.0249590815779
ik_pose2.orientation.y = y=0.999649402929
ik_pose2.orientation.z = z=0.00737916180073
ik_pose2.orientation.w = w=0.00486450832011
#print "ik_pose1\n",ik_pose1
#print "ik_pose2\n",ik_pose2
values1 = self.ik_request(pose=ik_pose1,limb='left')
values2 = self.ik_request(pose=ik_pose2,limb='right')
values1 = [values1[jnt] for jnt in self._l_goal.trajectory.joint_names]
values2 = [values2[jnt] for jnt in self._r_goal.trajectory.joint_names]
# print "***********\n",values1
# print "***********\n",values2
values = str([float(values[0])]+values1+values2)
# print "***********\n",type(values),values
# print self._r_goal.trajectory.joint_names
cmd, values = self._clean_line(values, joint_names)
# print "cmd\n",cmd
#find allowable time offset for move to start position
if idx == 0:
# Set the initial position to be the current pose.
# This ensures we move slowly to the starting point of the
# trajectory from the current pose - The user may have moved
# arm since recording
cur_cmd = [self._l_arm.joint_angle(jnt) for jnt in self._l_goal.trajectory.joint_names]
self._add_point(cur_cmd, 'left', 0.0)
cur_cmd = [self._r_arm.joint_angle(jnt) for jnt in self._r_goal.trajectory.joint_names]
self._add_point(cur_cmd, 'right', 0.0)
start_offset = find_start_offset(cmd)
# Gripper playback won't start until the starting movement's
# duration has passed, and the actual trajectory playback begins
self._slow_move_offset = start_offset
self._trajectory_start_offset = rospy.Duration(start_offset + float(values[0]))
#add a point for this set of commands with recorded time
cur_cmd = [cmd[jnt] for jnt in self._l_goal.trajectory.joint_names]
self._add_point(cur_cmd, 'left', float(values[0]) + start_offset)
cur_cmd = [cmd[jnt] for jnt in self._r_goal.trajectory.joint_names]
self._add_point(cur_cmd, 'right', float(values[0]) + start_offset)
def _feedback(self, data):
# Test to see if the actual playback time has exceeded
# the move-to-start-pose timing offset
if (not self._get_trajectory_flag() and
data.actual.time_from_start >= self._trajectory_start_offset):
self._set_trajectory_flag(value=True)
self._trajectory_actual_offset = data.actual.time_from_start
def _set_trajectory_flag(self, value=False):
with self._lock:
# Assign a value to the flag
self._arm_trajectory_started = value
def _get_trajectory_flag(self):
temp_flag = False
with self._lock:
# Copy to external variable
temp_flag = self._arm_trajectory_started
return temp_flag
def start(self):
"""
Sends FollowJointTrajectoryAction request
"""
self._left_client.send_goal(self._l_goal, feedback_cb=self._feedback)
self._right_client.send_goal(self._r_goal, feedback_cb=self._feedback)
# Syncronize playback by waiting for the trajectories to start
while not rospy.is_shutdown() and not self._get_trajectory_flag():
rospy.sleep(0.05)
def stop(self):
"""
Preempts trajectory execution by sending cancel goals
"""
if (self._left_client.gh is not None and
self._left_client.get_state() == actionlib.GoalStatus.ACTIVE):
self._left_client.cancel_goal()
if (self._right_client.gh is not None and
self._right_client.get_state() == actionlib.GoalStatus.ACTIVE):
self._right_client.cancel_goal()
#delay to allow for terminating handshake
rospy.sleep(0.1)
def wait(self):
"""
Waits for and verifies trajectory execution result
"""
#create a timeout for our trajectory execution
#total time trajectory expected for trajectory execution plus a buffer
last_time = self._r_goal.trajectory.points[-1].time_from_start.to_sec()
time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5
timeout = rospy.Duration(self._slow_move_offset +
last_time +
time_buffer)
l_finish = self._left_client.wait_for_result(timeout)
r_finish = self._right_client.wait_for_result(timeout)
l_result = (self._left_client.get_result().error_code == 0)
r_result = (self._right_client.get_result().error_code == 0)
#verify result
if all([l_finish, r_finish, l_result, r_result]):
return True
else:
msg = ("Trajectory action failed or did not finish before "
"timeout/interrupt.")
rospy.logwarn(msg)
return False
def main():
"""RSDK Joint Trajectory Example: File Playback
Plays back joint positions honoring timestamps recorded
via the joint_recorder example.
Run the joint_recorder.py example first to create a recording
file for use with this example. Then make sure to start the
joint_trajectory_action_server before running this example.
This example will use the joint trajectory action server
with velocity control to follow the positions and times of
the recorded motion, accurately replicating movement speed
necessary to hit each trajectory point on time.
"""
epilog = """
Related examples:
joint_recorder.py; joint_position_file_playback.py.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = 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'
)
parser.add_argument(
'-l', '--loops', type=int, default=1,
help='number of playback loops. 0=infinite.'
)
# remove ROS args and filename (sys.arv[0]) for argparse
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node("rsdk_joint_trajectory_file_playback")
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
print("Running. Ctrl-c to quit")
traj = Trajectory()
traj.parse_file(path.expanduser(args.file))
#for safe interrupt handling
rospy.on_shutdown(traj.stop)
result = True
loop_cnt = 1
loopstr = str(args.loops)
if args.loops == 0:
args.loops = float('inf')
loopstr = "forever"
while (result == True and loop_cnt <= args.loops
and not rospy.is_shutdown()):
print("Playback loop %d of %s" % (loop_cnt, loopstr,))
traj.start()
result = traj.wait()
loop_cnt = loop_cnt + 1
print("Exiting - File Playback Complete")
if __name__ == "__main__":
main()