def execAndRecordTraj(arm=Constants.Arm.Right): rospy.init_node('execAndRecordTraj',anonymous=True) rospy.sleep(1) ravenArm = RavenArm(arm) ravenPlanner = RavenPlanner(arm) record = Test(arm) rospy.sleep(2) ravenArm.start() rospy.loginfo('Classes are initialized, press enter') raw_input() startPose = tfx.pose(ravenArm.getGripperPose()) x = .01 y = .06 z = .05 deltaPoses = [] deltaPoses.append(tfx.pose([x,0, -z],frame=Constants.Frames.Link0)) #deltaPoses.append(tfx.pose([0,-y, 0],frame=Constants.Frames.Link0)) deltaPoses.append(tfx.pose([-x,0,z],frame=Constants.Frames.Link0)) #deltaPoses.append(tfx.pose([0,y, 0],frame=Constants.Frames.Link0)) #record.startRecording() while not rospy.is_shutdown(): for deltaPose in deltaPoses: startPose = tfx.pose(ravenArm.getGripperPose()) endPose = tfx.pose(Util.endPose(startPose, deltaPose)) ravenArm.goToGripperPose(endPose) rospy.sleep(4) record.stopRecording() rospy.loginfo('Press enter to print and exit') raw_input() ravenArm.stop() record.printPoses() IPython.embed()
def invKinClient(): rospy.init_node('inv_kin_client',anonymous=True) rospy.sleep(4) ravenArm = RavenArm(MyConstants.Arm.Right) rospy.sleep(4) start = tfx.pose(ravenArm.getGripperPose()) end = start + [0,-.05, 0] try: rospy.wait_for_service('inv_kin_server',timeout=5) inv_kin_service = rospy.ServiceProxy('inv_kin_server', InvKinSrv) arm = Constants.ARM_TYPE_GREEN pose = tfx.pose(ravenArm.getGripperPose()) grasp = ravenArm.ravenController.currentGrasp rospy.loginfo('Find ik for ' + str(pose)) respStart = inv_kin_service(arm, grasp, start) respEnd = inv_kin_service(arm, grasp, end) rospy.loginfo('Called service') except (rospy.ServiceException, rospy.ROSException) as e: print "Service call failed: %s"%e return startJoints = [] for joint in respStart.joints: if joint.type in RavenPlanner.rosJointTypes: startJoints.append(joint.position) #print("({0},{1})".format(joint.type,joint.position)) endJoints = [] for joint in respEnd.joints: if joint.type in RavenPlanner.rosJointTypes: endJoints.append(joint.position) #print("({0},{1})".format(joint.type,joint.position)) startJoints = np.array(startJoints) endJoints = np.array(endJoints) timeSteps = 50 for i in range(timeSteps): print(list(startJoints + (float(i)/float(timeSteps)*(endJoints-startJoints))))
class GenerateTraj(): def __init__(self, arm=MyConstants.Arm.Right): self.arm = arm self.ravenArm = RavenArm(self.arm) self.ravenPlanner = RavenPlanner(self.arm) def generateTraj(self, deltaPose): startPose = tfx.pose(self.ravenArm.getGripperPose()) deltaPose = tfx.pose(deltaPose) #code.interact(local=locals()) startJoints = {0:0.51091998815536499, 1:1.6072717905044558, 2:-0.049991328269243247, 4:0.14740140736103061, 5:0.10896652936935426, 8:-0.31163200736045837} endJoints = {0:0.45221099211619786, 1:2.2917657932075581, 2:-0.068851854076958902, 4:0.44096283117933965, 5:0.32085205361054148, 8:-0.82079765727524379} endPose = Util.endPose(startPose, deltaPose) #endPose = startPose #IPython.embed() jointTraj = self.ravenPlanner.getTrajectoryFromPose(endPose, startJoints=startJoints, n_steps=15) """ n_steps = 15 self.ravenPlanner.updateOpenraveJoints(startJoints) endJointPositions = endJoints.values() request = Request.joints(n_steps, self.ravenPlanner.manip.GetName(), endJointPositions) # convert dictionary into json-formatted string s = json.dumps(request) # create object that stores optimization problem prob = trajoptpy.ConstructProblem(s, self.ravenPlanner.env) # do optimization result = trajoptpy.OptimizeProblem(prob) # check trajectory safety from trajoptpy.check_traj import traj_is_safe prob.SetRobotActiveDOFs() if not traj_is_safe(result.GetTraj(), self.ravenPlanner.robot): rospy.loginfo('Trajopt trajectory is not safe. Trajopt failed!') rospy.loginfo('Still returning trajectory') #return IPython.embed() return self.ravenPlanner.trajoptTrajToDicts(result.GetTraj()) """ return jointTraj
class GripperFOVClass(): def __init__(self, armName): print('Initializing GripperFOVClass') self.armName = armName self.ravenArm = RavenArm(self.armName) self.imageDetector = ImageDetector() self.camFrame = '/left_BC' self.desiredOrientation = tfx.tb_angles(90,0,0) # in self.camFrame self.posFrame = MyConstants.Frames.Link0 self.rotFrame = MyConstants.Frames.RightTool if self.armName == MyConstants.Arm.Right else MyConstants.Frames.LeftTool self.pub = rospy.Publisher('orig_gripper_pose',PoseStamped) self.file = open('/tmp/GripperFOVTest.txt','w') print('Initialized GripperFOVClass') 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 homePose(self): homePose = self.currentPose() homePose.orientation = self.desiredOrientation return homePose def findNewGripper(self, timeout=1): timeout = Util.Timeout(timeout) self.imageDetector.ignoreOldGripper(self.armName) timeout.start() while not timeout.hasTimedOut(): if self.imageDetector.hasFoundNewGripper(self.armName): return tfx.pose(self.imageDetector.getGripperPose(self.armName)) return None 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 def runTest(self): self.ravenArm.start() rospy.sleep(2) currPoseInCamera = self.currentPose() homePose = self.homePose() deltaPose = Util.deltaPose(currPoseInCamera, homePose, self.posFrame, self.rotFrame) self.ravenArm.goToGripperPoseDelta(deltaPose, duration=2) # self.ravenArm.goToGripperPoseDelta(tfx.pose([0,0,0],tfx.tb_angles(0,30,0)),duration=2) #rotatePose = tfx.pose([0,0,0],tfx.tb_angles(0,45,0)) #IPython.embed() #return # z must always stay the same z = currPoseInCamera.position.z xMove = .03 yMove = .01 gridMoves = [[-xMove, 0, 0], [0, yMove, 0], [xMove, 0, 0], [0, -yMove, 0]] rotationAxes = ['pitch', 'roll'] for gridMove in gridMoves: print('Go to next place on grid, press enter') raw_input() deltaPose = Util.deltaPose(self.currentPose(), self.homePose()+gridMove, self.posFrame, self.rotFrame) homePose = Util.endPose(self.ravenArm.getGripperPose(), deltaPose) self.ravenArm.goToGripperPose(homePose) self.file.write('Home pose {0}\n'.format(homePose)) print('Home pose {0}'.format(homePose)) for axis in rotationAxes: if rospy.is_shutdown(): return rotateBy = 5.0 self.file.write('Rotating in positive direction on {0} axis\n'.format(axis)) print('Rotate in positive direction on {0} axis'.format(axis)) #raw_input() maxRotation = self.rotateUntilUnseen(axis, rotateBy) self.ravenArm.goToGripperPose(homePose, duration=1) rospy.sleep(1) self.file.write('Rotating in negative direction on {0} axis\n'.format(axis)) print('Rotate in negative direction on {0} axis'.format(axis)) #raw_input() minRotation = self.rotateUntilUnseen(axis, -rotateBy) self.ravenArm.goToGripperPose(homePose, duration=1) rospy.sleep(1) self.file.write('{0} range of motion is ({1},{2})\n'.format(axis,minRotation,maxRotation)) print('{0} range of motion is ({1},{2})'.format(axis,minRotation,maxRotation)) self.file.close() self.ravenArm.stop()