def testSwitchPlaces(show=True): #trajoptpy.SetInteractive(True) rospy.init_node('testSwitchPlaces',anonymous=True) rp = RavenPlanner(raven_constants.Arm.Both, thread=True) #rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=raven_constants.Frames.Link0) #leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=raven_constants.Frames.Link0) leftCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Left]),raven_constants.Frames.Link0) rightCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Right]),raven_constants.Frames.Link0) rp.getTrajectoryPoseToPose(raven_constants.Arm.Left, leftCurrPose, rightCurrPose, n_steps=50) rp.getTrajectoryPoseToPose(raven_constants.Arm.Right, rightCurrPose, leftCurrPose, n_steps=50) #rp.getPoseTrajectory(raven_constants.Arm.Left, rightCurrPose, n_steps=50) #rp.getPoseTrajectory(raven_constants.Arm.Right, leftCurrPose, n_steps=50) #rp.getPoseTrajectory(raven_constants.Arm.Left, leftCurrPose+[0.01,0,0], n_steps=50) #rp.getPoseTrajectory(raven_constants.Arm.Right, rightCurrPose-[0.01,0,0], n_steps=50) print 'waiting' rp.waitForTrajReady() print 'woooooooo' if rospy.is_shutdown(): return #IPython.embed() if not show: return rp.env.SetViewer('qtcoin') leftPoseTraj = rp.poseTraj[raven_constants.Arm.Left] rightPoseTraj = rp.poseTraj[raven_constants.Arm.Right] for left, right in zip(leftPoseTraj,rightPoseTraj): if rospy.is_shutdown(): break rp.updateOpenraveJoints('L', rp.getJointsFromPose('L', left, rp.getCurrentGrasp('L')), grasp=rp.getCurrentGrasp('L')) rp.updateOpenraveJoints('R', rp.getJointsFromPose('R', right, rp.getCurrentGrasp('R')), grasp=rp.getCurrentGrasp('R')) rospy.loginfo('Press enter to go to next step') raw_input() return leftTraj = rp.jointTraj[raven_constants.Arm.Left] rightTraj = rp.jointTraj[raven_constants.Arm.Right] for left, right in zip(leftTraj,rightTraj): if rospy.is_shutdown(): break rp.updateOpenraveJoints('L', left) rp.updateOpenraveJoints('R', right) rospy.loginfo('Press enter to go to next step') raw_input()
def setStartAndEndPose(self, armName, startPose, endPose, **kwargs): startGrasp = kwargs.get('startGrasp',kwargs.get('grasp',0)) endGrasp = kwargs.get('endGrasp',kwargs.get('grasp',0)) startJoints = self.getJointsFromPose(armName, startPose, grasp=startGrasp) endJoints = self.getJointsFromPose(armName, endPose, grasp=endGrasp) self.trajStartGrasp[armName] = startGrasp self.trajEndGrasp[armName] = endGrasp startPose = raven_util.convertToFrame(tfx.pose(startPose), raven_constants.Frames.Link0) self.trajStartPose[armName] = startPose endPose = raven_util.convertToFrame(tfx.pose(endPose), raven_constants.Frames.Link0) self.trajEndPose[armName] = endPose self.trajStartJoints[armName] = startJoints self.trajEndJoints[armName] = endJoints
def testFromAbove(show=True): trajoptpy.SetInteractive(True) rospy.init_node('testFromAbove',anonymous=True) rp = RavenPlanner([raven_constants.Arm.Left, raven_constants.Arm.Right], thread=True) leftCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Left]),raven_constants.Frames.Link0) rightCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Right]),raven_constants.Frames.Link0) rp.getTrajectoryFromPose(raven_constants.Arm.Left, leftCurrPose-[0,0.05,0], n_steps=50, approachDir=np.array([0,0,1]), block=False) rp.getTrajectoryFromPose(raven_constants.Arm.Right, rightCurrPose-[0,0.05,0], n_steps=50, approachDir=np.array([0,0,1])) #IPython.embed() print 'waiting' rp.waitForTrajReady() print 'woooooooo' if rospy.is_shutdown(): return #IPython.embed() if not show: return rp.env.SetViewer('qtcoin') leftPoseTraj = rp.poseTraj[raven_constants.Arm.Left] rightPoseTraj = rp.poseTraj[raven_constants.Arm.Right] leftTraj = rp.jointTraj[raven_constants.Arm.Left] rightTraj = rp.jointTraj[raven_constants.Arm.Right] for right in rightTraj: if rospy.is_shutdown(): break rp.updateOpenraveJoints('L', left) rp.updateOpenraveJoints('R', right) rospy.loginfo('Press enter to go to next step') raw_input() IPython.embed()
def getGripperPose(self,frame=raven_constants.Frames.Link0): """ Returns gripper pose w.r.t. frame geometry_msgs.msg.Pose """ pose = tfx.pose([0,0,0],frame=self.toolFrame) return tfx.convertToFrame(pose, self.commandFrame, pose.frame, wait=10) gripperPose = self.ravenController.currentPose if gripperPose != None: gripperPose = raven_util.convertToFrame(tfx.pose(gripperPose), frame) return gripperPose
def goToGripperPose(self, endPose, startPose=None, block=True, duration=None, speed=None, ignoreOrientation=False): """ Move to endPose 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 startPose = raven_util.convertToFrame(tfx.pose(startPose), self.commandFrame) endPose = raven_util.convertToFrame(tfx.pose(endPose), self.commandFrame) if ignoreOrientation: endPose.orientation = startPose.orientation self.ravenController.goToPose(endPose, start=startPose, duration=duration, speed=speed) if block: return self.blockUntilPaused() return True
def getJointsFromPose(self, armName, pose, grasp, quiet=False): """ Calls IK server and returns a dictionary of {jointType : jointPos} jointType is from raven_2_msgs.msg.Constants jointPos is position in radians Needs to return finger1 and finger2 """ pose = raven_util.convertToFrame(tfx.pose(pose), self.refFrame) joints = kin.invArmKin(armName, pose, grasp) if joints is None: rospy.loginfo('IK failed!') if quiet: return None else: raise RuntimeError("IK failed!") return joints
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