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 right limb
for name in joint_names:
if self.limb == name[:-3]:
self.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
for name in joint_names:
if self.limb == name[:-3]:
cmd.append(pos[name])
cur.append(self.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:]):
#clean each line of file
cmd, values = self._clean_line(values, joint_names)
#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.arm.joint_angle(jnt) for jnt in self.goal.trajectory.joint_names]
self._add_point(cur_cmd, self.limb, 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 + values[0])
#add a point for this set of commands with recorded time
cur_cmd = [cmd[jnt] for jnt in self.goal.trajectory.joint_names]
self._add_point(cur_cmd, self.limb, values[0] + start_offset)
if self.gripper:
cur_cmd = [cmd[self.gripper_name]]
self._add_point(cur_cmd, self.gripper_name, values[0] + start_offset)
python类abs()的实例源码
joint_trajectory_file_playback.py 文件源码
项目:intera_sdk
作者: RethinkRobotics
项目源码
文件源码
阅读 27
收藏 0
点赞 0
评论 0