def currentPose(self): currTfPose = Util.convertToFrame(self.ravenArm.getGripperPose(), self.camFrame) currDetectedPose = Util.convertToFrame(self.imageDetector.getGripperPose(self.armName), self.camFrame) currPoseInCamera = tfx.pose(currTfPose.position, currDetectedPose.orientation) return currPoseInCamera
def testSwitchPlaces(show=True): #trajoptpy.SetInteractive(True) rospy.init_node('testSwitchPlaces',anonymous=True) rp = RavenPlanner([MyConstants.Arm.Left,MyConstants.Arm.Right], thread=True) #rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=MyConstants.Frames.Link0) #leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=MyConstants.Frames.Link0) leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0) rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0) #rp.getTrajectoryPoseToPose(MyConstants.Arm.Left, leftCurrPose, rightCurrPose, n_steps=50) #rp.getTrajectoryPoseToPose(MyConstants.Arm.Right, rightCurrPose, leftCurrPose, n_steps=50) rospy.loginfo('Press enter to get poses') raw_input() rp.getPoseTrajectory(MyConstants.Arm.Left, leftCurrPose+[0.01,0,0], n_steps=50) rp.getPoseTrajectory(MyConstants.Arm.Right, rightCurrPose-[0.01,0,0], n_steps=50) #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[MyConstants.Arm.Left] rightPoseTraj = rp.poseTraj[MyConstants.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[MyConstants.Arm.Left] rightTraj = rp.jointTraj[MyConstants.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 testSwitchPlaces(armNames=[MyConstants.Arm.Left,MyConstants.Arm.Right],fixedPose=False): rospy.init_node('testSwitchPlaces',anonymous=True) rp = RavenPlanner(armNames) rospy.sleep(2) leftStartJoints = None rightStartJoints = None if fixedPose: rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=MyConstants.Frames.Link0) leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=MyConstants.Frames.Link0) leftStartJoints = rp.getJointsFromPose(MyConstants.Arm.Left, leftCurrPose, grasp=0) rightStartJoints = rp.getJointsFromPose(MyConstants.Arm.Right, rightCurrPose, grasp=0) else: leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0) rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0) #trajoptpy.SetInteractive(True) if fixedPose: rp.getTrajectoryFromPose(MyConstants.Arm.Left, rightCurrPose, endGrasp=0, startJoints=leftStartJoints, debug=True) rp.getTrajectoryFromPose(MyConstants.Arm.Right, leftCurrPose, endGrasp=0, startJoints=rightStartJoints, debug=True) else: rp.getTrajectoryFromPose(MyConstants.Arm.Left, rightCurrPose, leftStartJoints, debug=True) rp.getTrajectoryFromPose(MyConstants.Arm.Right, leftCurrPose, rightStartJoints, debug=True) """ for index in range(len(armNames)): armName = armNames[index] otherArmName = armNames[(index+1)%len(armNames)] desPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[otherArmName]),MyConstants.Frames.Link0) rp.getTrajectoryFromPose(armName, desPose, debug=True) """ rp.getTrajectoryFromPoseThread(once=True) # while rp.trajRequest[armNames[0]] and rp.trajRequest[armNames[1]] and not rospy.is_shutdown(): # rospy.sleep(.05) leftTraj = rp.jointTraj[MyConstants.Arm.Left] rightTraj = rp.jointTraj[MyConstants.Arm.Right] rp.env.SetViewer('qtcoin') for left, right in zip(leftTraj,rightTraj): rospy.loginfo('Press enter to go to next step') raw_input() rp.updateOpenraveJoints('L', left) rp.updateOpenraveJoints('R', right)
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 = Util.convertToFrame(tfx.pose(startPose), MyConstants.Frames.Link0) self.trajStartPose[armName] = startPose endPose = Util.convertToFrame(tfx.pose(endPose), MyConstants.Frames.Link0) self.trajEndPose[armName] = endPose self.trajStartJoints[armName] = startJoints self.trajEndJoints[armName] = endJoints
def getGripperPoseEstimate(self, armName): if not self.hasFoundGripper(armName): return False if armName == Constants.Arm.Left: lastGripperPose = self.leftGripperPose lastTfPose = self.leftGripperPoseTf currTfPose = Util.convertToFrame(tfx.pose([0,0,0],frame=Constants.Frames.LeftTool), Constants.Frames.Link0) else: lastGripperPose = self.rightGripperPose lastTfPose = self.rightGripperPoseTf currTfPose = Util.convertToFrame(tfx.pose([0,0,0],frame=Constants.Frames.RightTool), Constants.Frames.Link0) deltaTf = Util.deltaPose(currTfPose, lastTfPose)
def getTrajectoryFromPose(self, armName, endPose, endGrasp=None, startJoints=None, reqType=Request.Type.Joints, n_steps=50, debug=False): endPose = Util.convertToFrame(tfx.pose(endPose), MyConstants.Frames.Link0) self.trajEndPose[armName] = endPose self.trajEndJoints[armName] = self.getJointsFromPose(armName, endPose, grasp=endGrasp) if self.trajEndJoints[armName] == None: return None if startJoints == None: self.trajStartJoints[armName] = self.currentJoints[armName] print 'cj', self.currentJoints[armName] else: self.trajStartJoints[armName] = startJoints self.trajSteps[armName] = n_steps self.trajReqType = reqType self.trajRequest[armName] = True if debug: return print 'waiting for traj' while self.trajRequest[armName] and not rospy.is_shutdown(): rospy.sleep(.05) return self.poseTraj[armName]
def tapeCallbackRight(self, msg): self.newRightGripperPose = True self.tapeMsg = msg self.rightGripperPose = msg self.gripperPoseIsEstimate = False self.rightGripperPoseTf = Util.convertToFrame(tfx.pose([0,0,0],frame=Constants.Frames.RightTool), Constants.Frames.Link0)
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 = Util.convertToFrame(tfx.pose(pose), self.refFrame) try: rospy.loginfo('Waiting for IK server...') rospy.wait_for_service('inv_kin_server',timeout=5) inv_kin_service = rospy.ServiceProxy('inv_kin_server', InvKinSrv) rospy.loginfo('Calling the IK server') resp = inv_kin_service(self.invKinArm[armName], grasp, pose.msg.Pose()) rospy.loginfo('IK success!') except (rospy.ServiceException, rospy.ROSException) as e: rospy.loginfo("IK server failure: %s"%e) if quiet: return else: raise e return dict((joint.type, joint.position) for joint in resp.joints)
def testCheckPickup(armName=Constants.Arm.Right): rospy.init_node('testCheckPickup',anonymous=True) imageDetector = ImageDetector() rospy.sleep(4) rospy.loginfo('Press enter to get {0} gripper point'.format(armName)) raw_input() gripperPose = imageDetector.getGripperPose(armName) convGripperPose = tfx.pose(Util.convertToFrame(gripperPose, Constants.Frames.Link0)) lastGripperPoint = convGripperPose.position.msg.PointStamped() lastGripperPoint.point.z += 0 rospy.loginfo('Press enter to thresh for red at last gripper point') raw_input() serviceName = '{0}_{1}'.format(Constants.Services.isFoamGrasped, armName) try: rospy.wait_for_service(serviceName, timeout=5) foamGraspedService = rospy.ServiceProxy(serviceName, ThreshRed) isFoamGrasped = foamGraspedService(lastGripperPoint).output if isFoamGrasped == 1: rospy.loginfo('Found red piece!') else: rospy.loginfo('Did not find red piece!') except e: rospy.loginfo('Thresh service failed: {0}'.format(e)) rospy.spin()
def testFromAbove(show=True): trajoptpy.SetInteractive(True) rospy.init_node('testFromAbove',anonymous=True) rp = RavenPlanner([MyConstants.Arm.Left, MyConstants.Arm.Right], thread=True) leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0) rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0) rp.getTrajectoryFromPose(MyConstants.Arm.Left, leftCurrPose-[0,0.05,0], n_steps=50, approachDir=np.array([0,0,1]), block=False) rp.getTrajectoryFromPose(MyConstants.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[MyConstants.Arm.Left] rightPoseTraj = rp.poseTraj[MyConstants.Arm.Right] leftTraj = rp.jointTraj[MyConstants.Arm.Left] rightTraj = rp.jointTraj[MyConstants.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 testMoveActiveArm(show=True): rospy.init_node('testMoveActiveArm',anonymous=True) rp = RavenPlannerBSP([MyConstants.Arm.Left,MyConstants.Arm.Right],thread=True,simPlotting=True,stagePlotting=True) rospy.sleep(4) leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0) rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0) if rp.activeArm == MyConstants.Arm.Left: rp.getTrajectoryFromPose(MyConstants.Arm.Left, leftCurrPose-[0,0.05,0], n_steps=50, block=False) rp.getTrajectoryFromPose(MyConstants.Arm.Right, rightCurrPose, n_steps=50, block=False) else: rp.getTrajectoryFromPose(MyConstants.Arm.Left, leftCurrPose, n_steps=50, block=False) rp.getTrajectoryFromPose(MyConstants.Arm.Right, rightCurrPose-[0,0.05,0], n_steps=50, block=False) print 'waiting' rp.waitForTrajReady() print 'woooooooo' if rospy.is_shutdown(): return if not show: return rp.env.SetViewer('qtcoin') leftTraj = rp.jointTraj[MyConstants.Arm.Left] rightTraj = rp.jointTraj[MyConstants.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() IPython.embed()
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 = Util.convertToFrame(tfx.pose(startPose), self.commandFrame) endPose = 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 getGripperPose(self,frame=MyConstants.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 = Util.convertToFrame(tfx.pose(gripperPose), frame) return gripperPose
def _objectPoseCallback(self, msg): """ temp callback for creating box in between object and gripper """ box = rave.RaveCreateKinBody(self.env,'') box.SetName('testbox') box.InitFromBoxes(np.array([[0,0,0,0.01,0.01,0.05]]),True) pose = Util.convertToFrame(msg, MyConstants.Frames.Link0) #pose.position.x += .01 poseMatrix = np.array(pose.matrix) #convPoseMatrix = transformRelativePoseForIk(self.robot.GetManipulators()[0],poseMatrix,pose.frame,'world') poseMatInRef = poseMatrix refLinkName = pose.frame targLinkName = 'world' # ref -> world refFromWorld = self.robot.GetLink(refLinkName).GetTransform() # target -> world targFromWorld = self.robot.GetLink(targLinkName).GetTransform() # target -> ref targFromRef = np.dot(np.linalg.inv(targFromWorld), refFromWorld) poseMatInTarg = np.dot(targFromRef, poseMatInRef) convPose = tfx.pose(poseMatInTarg, frame=targLinkName) convPoseMatrix = convPose.matrix convPoseMatrix[:3,:3] = np.identity(3) box.SetTransform(np.array(convPoseMatrix)) rospy.loginfo('POSE.POSITION.X {0}'.format(pose.position.x)) if pose.position.x < -.05: p = tfx.pose(self.manip[MyConstants.Arm.Right].GetEndEffectorTransform()) p.position.x += .03 m = p.matrix m[:3,:3] = np.eye(3) box.SetTransform(np.array(m)) self.env.Add(box,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 = 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 = Util.convertToFrame(tfx.pose(startPose), self.commandFrame) endPoses = [] for deltaPose in stampedDeltaPoses: endPoses.append(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 rotateUntilUnseen(self, axis, rotateBy): firstPose = gripperPose = Util.convertToFrame(self.findNewGripper(), MyConstants.Frames.Link0) firstTfPose = Util.convertToFrame(self.ravenArm.getGripperPose(), MyConstants.Frames.Link0) self.pub.publish(firstPose.msg.PoseStamped()) if axis == 'pitch': tb_rotate = tfx.tb_angles(0,rotateBy,0) origAngle = tfx.tb_angles(firstPose.orientation).pitch_deg elif axis == 'roll': tb_rotate = tfx.tb_angles(0,0,rotateBy) origAngle = tfx.tb_angles(firstPose.orientation).roll_deg else: return 0.0 totalRotation = 0.0 rotatePose = tfx.pose([0,0,0], tb_rotate) lastOrientation = tfx.pose(self.ravenArm.getGripperPose()).orientation lastPose = None while gripperPose is not None: #gripperPose = Util.convertToFrame(gripperPose, self.rotFrame) #tfPose = tfx.pose([0,0,0],frame=self.rotFrame) #tapeRotation = gripperPose.orientation #tfRotation = tfPose.orientation #IPython.embed() #return #tfGripperPose = tfx.pose(self.ravenArm.getGripperPose()) #gripperPose = Util.convertToFrame(gripperPose, tfGripperPose.frame) #endPose = tfx.pose(tfGripperPose.position, rotatePose.orientation.matrix * gripperPose.orientation.matrix) #self.ravenArm.goToGripperPose(endPose, duration=1) #endPose = Util.endPose(gripperPose,tfx.pose([0,0,0],rotatePose.orientation, frame=self.camFrame),frame=MyConstants.Frames.Link0) #deltaPose = tfx.pose([0,0,0], endPose.orientation) #self.ravenArm.goToGripperPoseDelta(deltaPose, duration=1) tfGripperPose = tfx.pose(self.ravenArm.getGripperPose()) tfGripperPose = Util.convertToFrame(tfGripperPose, self.camFrame) endPose = Util.endPose(tfGripperPose, tfx.pose([0,0,0],tb_rotate,frame=self.camFrame)) endPose = Util.convertToFrame(endPose, MyConstants.Frames.Link0) endPose.position = firstTfPose.position self.ravenArm.goToGripperPose(endPose, duration=1) print('Rotating...') rospy.sleep(.25) #self.ravenArm.goToGripperPoseDelta(rotatePose, duration=1) totalRotation += rotateBy #print('Total rotation = {0}'.format(totalRotation)) currOrientation = tfx.pose(self.ravenArm.getGripperPose()).orientation if lastOrientation is not None: angleBetween = Util.angleBetweenQuaternions(lastOrientation.msg.Quaternion(), currOrientation.msg.Quaternion()) #print('angleBetween = {0}'.format(angleBetween)) if angleBetween < rotateBy*.5: self.file.write('Hit physical limit\n') print('Hit physical limit') break lastOrientation = currOrientation rospy.sleep(.5) lastPose = Util.convertToFrame(gripperPose, self.camFrame) gripperPose = self.findNewGripper() if lastPose is not None: firstPose = Util.convertToFrame(firstPose, self.camFrame) if axis == 'pitch': firstPitch = tfx.tb_angles(firstPose.orientation).pitch_deg lastPitch = tfx.tb_angles(lastPose.orientation).pitch_deg return copysign(fabs(lastPitch-firstPitch),rotateBy) elif axis == 'roll': firstRoll = tfx.tb_angles(firstPose.orientation).roll_deg lastRoll = tfx.tb_angles(lastPose.orientation).roll_deg return copysign(fabs(lastRoll-firstRoll),rotateBy) return totalRotation