def getFullTrajFromBag(self,
whicharm,
basename,
seg_num,
desired_hz=10.0,
use_cart=True,
white_thresh=0.001,
is_sim=False,
control_frame = -1,
goal_frame = -1):
"""
Given a directory name and segment number, loads the relevant files from that directory to get a trajectory.
Assumes the file only contains a single skill and does not need to be segmented.
Args:
whicharm (int): Which arm is this trajectory for? 0 for right, 1 for left.
basename (string): The path to examine for demonstration directories
seg_num (int): This specifies which set of files to load, corresponding to a segment for the left or right arm.
Will get trajectory from basename/part<seg_num>.bag
desired_hz (double): The rate at which to sample from the bagfile
use_cart (bool): True if we want cartesian data, false for joint data
white_thresh (double): The threshold of motion below which we count a timestep as being "motionless" and remove that data
is_sim (bool): Did this data come from simulation or a real robot?
control_frame (int): Object id that should be controlled by the skill. No entry means the control point is the end effector by default.
goal_frame (int): Object ids that the skill goal is relative to. No entry means that the goal is in the torso frame.
Returns:
(Demonstration) A demonstration from the appropriate files.
"""
#Construct file names
demofile = basename + '/part' + str(seg_num) + '.bag'
picklefile = basename + '/Pickle' + str(seg_num) + '.txt'
matfile = basename + '/Mat' + str(seg_num) + '.txt'
markerfile = basename + '/Marker' + str(seg_num) + '.txt'
#Load data from bagfiles and process
#Sample the bagfiles at regular intervals, convert to an easier format to use, and write data to files
b2m = bag2mat.Bag2Mat(whicharm, is_sim)
b2m.convertToMat(demofile, picklefile, matfile, markerfile, desired_hz, use_cart, white_thresh)
#Get the traj of the gripper
arm_control_data = skillParse.singleSkillParse(picklefile)
#Get the traj of the goal marker, if approprite
if(goal_frame >= 0):
marker_goal_data = TrajUtils.createMarkerTrajFromFile(markerfile, goal_frame, -1)
#If a control frame other than the gripper is specified, get that marker's trajectory and add on gripper info
if (control_frame >= 0):
#Estimate the rigid transform between marker and wrist and infer the rest of the traj from arm pose
#This is much more accurate than following a marker traj from perception
traj_data = self.inferMarkerTrajFromRigidTrans(arm_control_data, markerfile, control_frame)
#Otherwise, get the whole skill demo from the arm traj
else:
traj_data = arm_control_data
#Smooth the gripper data
TrajUtils.gripperSmoother(traj_data, 5)
return traj_data
print "Usage: python singleReplay <whicharm> <skill_id> <control_frame> <goal_frame> <plan_only>"
print "0 for right arm, 1 for left arm"
print "-1 for control frame: wrist_roll_joint ; -1 for goal_frame: torso_lift_link ; otherwise use ID of marker\n"
sys.exit(0)
#Construct file names
basename = 'data/bagfiles/2014_8_4_22_37_32/'
demofile = basename + 'demo' + str(skill_id) + '.bag'
picklefile = basename + 'Pickle' + str(skill_id) + '.txt'
matfile = basename + 'Mat' + str(skill_id) + '.txt'
markerfile = basename + 'Marker' + str(skill_id) + '.txt'
#################### Load data and process trajectories ####################
#Get the traj of the gripper
arm_control_data = skillParse.singleSkillParse(picklefile)
#Get the traj of the goal marker, if approprite
if (goal_frame >= 0):
marker_goal_data = traj_utils.createMarkerTrajFromFile(
markerfile, goal_frame, -1)
#If a control frame other than the gripper is specified, get that marker's trajectory and add on gripper info
if (control_frame >= 0):
#Estimate the rigid transform between marker and wrist and infer the rest from arm pose
traj_data = traj_utils.inferMarkerTrajFromRigidTrans(
arm_control_data, markerfile, control_frame)
#Otherwise, get the whole skill demo from the arm traj
else:
traj_data = arm_control_data
sys.exit(0)
#Construct file names
basename = 'data/bagfiles/2014_8_4_22_37_32/'
demofile = basename + 'demo' + str(skill_id) + '.bag'
picklefile = basename + 'Pickle' + str(skill_id) + '.txt'
matfile = basename + 'Mat' + str(skill_id) + '.txt'
markerfile = basename + 'Marker' + str(skill_id) + '.txt'
#################### Load data and process trajectories ####################
#Get the traj of the gripper
arm_control_data = skillParse.singleSkillParse(picklefile)
#Get the traj of the goal marker, if approprite
if(goal_frame >= 0):
marker_goal_data = traj_utils.createMarkerTrajFromFile(markerfile, goal_frame, -1)
#If a control frame other than the gripper is specified, get that marker's trajectory and add on gripper info
if (control_frame >= 0):
#Estimate the rigid transform between marker and wrist and infer the rest from arm pose
traj_data = traj_utils.inferMarkerTrajFromRigidTrans(arm_control_data, markerfile, control_frame)
#Otherwise, get the whole skill demo from the arm traj
else:
traj_data = arm_control_data
def getFullTrajFromBag(self,
whicharm,
basename,
seg_num,
desired_hz=10.0,
use_cart=True,
white_thresh=0.001,
is_sim=False,
control_frame=-1,
goal_frame=-1):
"""
Given a directory name and segment number, loads the relevant files from that directory to get a trajectory.
Assumes the file only contains a single skill and does not need to be segmented.
Args:
whicharm (int): Which arm is this trajectory for? 0 for right, 1 for left.
basename (string): The path to examine for demonstration directories
seg_num (int): This specifies which set of files to load, corresponding to a segment for the left or right arm.
Will get trajectory from basename/part<seg_num>.bag
desired_hz (double): The rate at which to sample from the bagfile
use_cart (bool): True if we want cartesian data, false for joint data
white_thresh (double): The threshold of motion below which we count a timestep as being "motionless" and remove that data
is_sim (bool): Did this data come from simulation or a real robot?
control_frame (int): Object id that should be controlled by the skill. No entry means the control point is the end effector by default.
goal_frame (int): Object ids that the skill goal is relative to. No entry means that the goal is in the torso frame.
Returns:
(Demonstration) A demonstration from the appropriate files.
"""
#Construct file names
demofile = basename + '/part' + str(seg_num) + '.bag'
picklefile = basename + '/Pickle' + str(seg_num) + '.txt'
matfile = basename + '/Mat' + str(seg_num) + '.txt'
markerfile = basename + '/Marker' + str(seg_num) + '.txt'
#Load data from bagfiles and process
#Sample the bagfiles at regular intervals, convert to an easier format to use, and write data to files
b2m = bag2mat.Bag2Mat(whicharm, is_sim)
b2m.convertToMat(demofile, picklefile, matfile, markerfile, desired_hz,
use_cart, white_thresh)
#Get the traj of the gripper
arm_control_data = skillParse.singleSkillParse(picklefile)
#Get the traj of the goal marker, if approprite
if (goal_frame >= 0):
marker_goal_data = TrajUtils.createMarkerTrajFromFile(
markerfile, goal_frame, -1)
#If a control frame other than the gripper is specified, get that marker's trajectory and add on gripper info
if (control_frame >= 0):
#Estimate the rigid transform between marker and wrist and infer the rest of the traj from arm pose
#This is much more accurate than following a marker traj from perception
traj_data = self.inferMarkerTrajFromRigidTrans(
arm_control_data, markerfile, control_frame)
#Otherwise, get the whole skill demo from the arm traj
else:
traj_data = arm_control_data
#Smooth the gripper data
TrajUtils.gripperSmoother(traj_data, 5)
return traj_data
