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