def goToGripperPoseDelta(self, deltaPose, startPose=None, block=True, duration=None, speed=None, ignoreOrientation=False): """ Move by deltaPose If startPose is not specified, default to current pose according to tf (w.r.t. Link0) """ if startPose == None: startPose = self.getGripperPose() if startPose == None: return endPose = raven_util.endPose(startPose, deltaPose, self.commandFrame) self.goToGripperPose(endPose, startPose=startPose, block=block, duration=duration, speed=speed, ignoreOrientation=ignoreOrientation)
def executeDeltaPoseTrajectory(self, deltaPoses, startPose=None, block=True, speed=None, ignoreOrientation=False): """ Each deltaPose in deltaPoses is with respect to the startPose Assuming deltaPoses and startPose in same frame (0_link) """ if startPose is None: startPose = self.getGripperPose() if startPose is None: return endPoses = [raven_util.endPose(startPose, deltaPose, self.commandFrame) for deltaPose in deltaPoses] #IPython.embed() print print 'START', startPose print 'END', endPoses[-1] print '''IPython.embed() ''' return self.executePoseTrajectory(endPoses, block=block, speed=speed, ignoreOrientation=ignoreOrientation)
def executeStampedDeltaPoseTrajectory(self, stampedDeltaPoses, startPose=None, block=True): """ stampedDeltaPoses is a list of tfx poses with time stamps each stampedDeltaPose is endPose - startPose, where each endPose is different but the startPose is the same This function is intended to be used where each endPose is from the vision system and the one startPose is the gripper position according to vision """ durations = [] prevTime = rospy.Time.now() for stampedDeltaPose in stampedDeltaPoses: durations.append(stampedDeltaPose.stamp - prevTime) prevTime = stampedDeltaPose.stamp if startPose == None: startPose = self.getGripperPose() if startPose == None: return startPose = raven_util.convertToFrame(tfx.pose(startPose), self.commandFrame) endPoses = [] for deltaPose in stampedDeltaPoses: endPoses.append(raven_util.endPose(startPose, deltaPose, self.commandFrame)) prevPose = None for duration, endPose in zip(duration, endPoses): self.goToGripperPose(endPose, startPose=prevPose, block=False, duration=duration) prevPose = endPose if block: return self.blockUntilPaused() return True
def getFullTrajectoryFromPose(self, armName, endPose, startPose=None, endGrasp = None, n_steps=50, block=True, approachDir=None, speed=None, correctTrajectory=False): success = False numTries = 0 while not success and numTries < 10: self.waitForState() joints1 = self.getCurrentJoints(armName) if startPose is None: startPose = self.getCurrentPose(armName) startGrasp = self.getCurrentGrasp(armName) if endGrasp is None: endGrasp = startGrasp self.setStartAndEndPose(armName, startPose, endPose, startGrasp=startGrasp, endGrasp=endGrasp) self.trajSteps[armName] = n_steps with self.lock: self.trajRequest[armName] = True if self.trajEndJoints[armName] is None: print armName, startPose, startGrasp, endPose, endGrasp raise Exception() self.approachDir[armName] = approachDir self.start_pose_pubs[armName].publish(startPose.msg.PoseStamped()) self.end_pose_pubs[armName].publish(endPose.msg.PoseStamped()) if block: print 'waiting for arm {} traj'.format(armName) while self.trajRequest[armName] and not rospy.is_shutdown(): rospy.sleep(0.05) print 'received arm {} traj'.format(armName) endPoses = [raven_util.endPose(startPose, deltaPose) for deltaPose in self.deltaPoseTraj[armName]] success = (endPoses[-1].position.distance(endPose.position) <= 0.005) numTries = numTries+1 if endPoses[-1].position.distance(endPose.position) > 0.005: print 'FAIL' if correctTrajectory: # run a second time to correct (needed the desired approach direction) if self.errorModel is not None: #startPoseCorrected, startPoseOffset = self.errorModel.predictSinglePose(armName, startPose, startPose) dt = 1 secondLastPose = endPoses[-2] lastPose = endPoses[-1] if speed is not None: dt = lastPose.position.distance(secondLastPose.position) / speed endPoseCorrected, endPoseOffset = self.errorModel.predictSinglePose(armName, lastPose, secondLastPose, dt) # << this is wrong, need true delta print 'End Before', self.poseTraj[armName][n_steps-1] correctedTraj = self.getFullTrajectoryFromPose(armName, endPoseCorrected, startPose=startPose, endGrasp=endGrasp, n_steps=n_steps, block=block, approachDir=approachDir, correctTrajectory=False) print print armName for p in endPoses: print 'Pose', p print 'START', startPose print 'END', self.poseTraj[armName][n_steps-1] print 'ORIG', endPose print 'dt', dt print # if armName == 'R': # IPython.embed() return correctedTraj return (self.poseTraj[armName][0], self.deltaPoseTraj[armName])