def testMoveStraightLine(arm=raven_constants.Arm.Right): rospy.init_node('raven_commander', anonymous=True) ravenArm = RavenArm(arm, False) rospy.sleep(1) ravenArm.start() rospy.loginfo('Press enter to start') raw_input() startPose = ravenArm.ravenController.currentPose delta = tfx.pose([0.01, 0.00, 0.00]) endPose = raven_util.endPose(startPose, delta) print "Start Pose:" print startPose print "End Pose:" print endPose # ravenArm.ravenController.goToPose(endPose) rospy.loginfo('Press enter to stop') raw_input() ravenArm.ravenController.stop() rospy.loginfo('Press enter to exit') raw_input()
def test_gripperMove(): rospy.init_node('raven_controller',anonymous=True) leftArm = RavenController(raven_constants.Arm.Left, defaultPoseSpeed=0.05) rospy.sleep(2) rospy.loginfo('Press enter to start') raw_input() leftArm.start() startPose = leftArm.currentPose delta = tfx.pose([0.01, 0.01, 0.01]) endPose = raven_util.endPose(startPose, delta) # print "--Start Pose" # print startPose # print endPose # print "--End Pose" # tfx.pose([-0.02, -0.08, -0.17], tfx.tb_angles(-90,90,0), frame=startPose.frame)#raven_util.endPose(startPose, tfx.pose([-0.02, -0.08, -0.17]), startPose.frame) leftArm.goToPose(endPose) rospy.loginfo('Press enter to stop') raw_input() leftArm.stop() rospy.loginfo('Press enter to exit') raw_input()
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 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 testMoveStraightLine(arm=raven_constants.Arm.Right): rospy.init_node('raven_commander',anonymous=True) ravenArm = RavenArm(arm, False) rospy.sleep(1) ravenArm.start() rospy.loginfo('Press enter to start') raw_input() startPose = ravenArm.ravenController.currentPose delta = tfx.pose([0.01, 0.00, 0.00]) endPose = raven_util.endPose(startPose, delta) print "Start Pose:" print startPose print "End Pose:" print endPose # ravenArm.ravenController.goToPose(endPose) rospy.loginfo('Press enter to stop') raw_input() ravenArm.ravenController.stop() rospy.loginfo('Press enter to exit') raw_input()
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 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