def execute(self, userdata): if MasterClass.PAUSE_BETWEEN_STATES: pause_func(self) rospy.loginfo('Planning trajectory from gripper to object') objectPose = tfx.pose(userdata.objectPose) gripperPose = tfx.pose(userdata.gripperPose) #objectPose.orientation = gripperPose.orientation transBound = .006 rotBound = float("inf") if Util.withinBounds(gripperPose, objectPose, transBound, rotBound, self.transFrame, self.rotFrame): rospy.loginfo('Closing the gripper') self.ravenArm.closeGripper(duration=self.gripperOpenCloseDuration) userdata.vertAmount = .04 return 'reachedObject' deltaPose = tfx.pose(Util.deltaPose(gripperPose, objectPose, self.transFrame, self.rotFrame)) endPose = Util.endPose(self.ravenArm.getGripperPose(), deltaPose) n_steps = int(self.stepsPerMeter * deltaPose.position.norm) + 1 poseTraj = self.ravenPlanner.getTrajectoryFromPose(self.ravenArm.name, endPose, n_steps=n_steps) rospy.loginfo('deltaPose') rospy.loginfo(deltaPose) rospy.loginfo('n_steps') rospy.loginfo(n_steps) if poseTraj == None: return 'failure' userdata.poseTraj = poseTraj return 'notReachedObject'
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 testRotation(): rospy.init_node('ar_image_detection') imageDetector = ARImageDetector() listener = tf.TransformListener() tf_br = tf.TransformBroadcaster() while not rospy.is_shutdown(): if imageDetector.hasFoundGripper(Constants.Arm.Left): obj = tfx.pose([0,0,0], imageDetector.normal).msg.PoseStamped() gripper = imageDetector.getGripperPose(Constants.Arm.Left) print('gripper') print(gripper) # obj_tb = tfx.tb_angles(obj.pose.orientation) gripper_tb = tfx.tb_angles(gripper.pose.orientation) print "gripper ori", gripper_tb obj_tb = tfx.tb_angles(imageDetector.normal) print "obj ori", obj_tb pt = gripper.pose.position ori = imageDetector.normal tf_br.sendTransform((pt.x, pt.y, pt.z), (ori.x, ori.y, ori.z, ori.w), gripper.header.stamp, '/des_pose', Constants.AR.Frames.Base) between = Util.angleBetweenQuaternions(ori, gripper_tb.msg) print('Angle between') print(between) quat = tft.quaternion_multiply(gripper_tb.quaternion, tft.quaternion_inverse(obj_tb.quaternion)) print 'new', tfx.tb_angles(quat) #rot = gripper_tb.rotation_to(ori) rot = gripper_tb.rotation_to(obj_tb) print('Rotation from gripper to obj') print(rot) deltaPoseTb = tfx.pose(Util.deltaPose(gripper, obj)).orientation print('deltaPose') print(deltaPoseTb) deltaPose = tfx.pose([0,0,0], deltaPoseTb).msg.PoseStamped() time = listener.getLatestCommonTime('0_link', 'tool_L') deltaPose.header.stamp = time deltaPose.header.frame_id = '0_link' deltaPose = listener.transformPose('tool_L', deltaPose) print "transformed", tfx.tb_angles(deltaPose.pose.orientation) endQuatMat = gripper_tb.matrix * rot.matrix print 'desOri', tfx.tb_angles(endQuatMat) rospy.sleep(1)
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 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 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 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 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 posesToDeltaPoses(self, poseList): """ p0 -> p1 -> p2 -> .... to delta(p0->p1) -> delta(p0->p2) -> delta(p0->p3)... """ startPose = poseList[0] return [Util.deltaPose(startPose, pose) for pose in poseList[1:]]
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 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
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 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 = [Util.endPose(startPose, deltaPose, self.commandFrame) for deltaPose in deltaPoses] return self.executePoseTrajectory(endPoses, block=block, speed=speed, ignoreOrientation=ignoreOrientation)
def updateOpenraveJoints(self, armName, joints1=None, grasp=None): """ Updates the openrave raven model using rosJoints rosJoints is dictionary of {jointType : jointPos} jointType is from raven_2_msgs.msg.Constants jointPos is position in radians Updates based on: shoulder, elbow, insertion, rotation, pitch, yaw, grasp (self.currentGrasp) """ rosJoints = joints1 if rosJoints is None: rosJoints = self.getCurrentJoints(armName) if rosJoints is None: return if grasp is None: grasp = self.getCurrentGrasp(armName) if grasp is None: grasp = 0. raveJointTypes = [] jointPositions = [] for rosJointType, jointPos in rosJoints.items(): if self.rosJointTypesToRave[armName].has_key(rosJointType): raveJointType = self.rosJointTypesToRave[armName][rosJointType] raveJointTypes.append(raveJointType) # since not a hard limit, must do this if rosJointType == Constants.JOINT_TYPE_ROTATION: lim = self.robot.GetJointFromDOFIndex(raveJointType).GetLimits() limLower, limUpper = lim[0][0], lim[1][0] jointPos = Util.setWithinLimits(jointPos, limLower, limUpper, 2*pi) jointPositions.append(jointPos) # for opening the gripper raveJointTypes += self.raveGrasperJointTypes[armName] if armName == MyConstants.Arm.Left: jointPositions += [grasp/2, grasp/2] else: jointPositions += [grasp/2, -grasp/2] self.robot.SetJointValues(jointPositions, raveJointTypes)
def execute(self, userdata): if MasterClass.PAUSE_BETWEEN_STATES: pause_func(self) rospy.loginfo('Moving vertical with the object') # move vertical with the object deltaPose = tfx.pose([0,0,userdata.vertAmount]).msg.Pose() endPose = Util.endPose(self.ravenArm.getGripperPose(), deltaPose) endPoseTraj = self.ravenPlanner.getTrajectoryFromPose(self.ravenArm.name, endPose) if endPoseTraj != None: self.ravenArm.executePoseTrajectory(endPoseTraj) return 'success'
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 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 = Util.endPose(startPose, deltaPose, self.commandFrame) self.goToGripperPose(endPose, startPose=startPose, block=block, duration=duration, speed=speed, ignoreOrientation=ignoreOrientation)
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 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 moveNearObject(self, failMethod=None, successMethod=None): """ Goes near the object Returns servoToObject """ failMethod = failMethod or self.moveNearObject successMethod = successMethod or self.servoToObject rospy.loginfo('Moving near the object point') # go to near the object point deltaPose = Util.deltaPose(self.gripperPose, self.objectPose, self.transFrame, self.rotFrame) deltaPose.position.z += .03 self.ravenArm.goToGripperPoseDelta(deltaPose, speed=self.openLoopSpeed) return successMethod
def test(): rospy.init_node('testGripperPoseEstimator',anonymous=True) rospy.sleep(2) arms = Constants.Arm.Both gpe = GripperPoseEstimator(arms) rospy.sleep(2) estimatePub = dict() truthPub = dict() prevPoseAndIsEstimate = dict() for arm in arms: truthPub[arm] = rospy.Publisher('truth_gripper_pose_%s'%arm,PoseStamped) printEvery = Util.Timeout(1) printEvery.start() while not rospy.is_shutdown(): for arm in arms: truthPose = gpe.truthPose.get(arm) estimatedPoseisEstimate = gpe.estimatedPose.get(arm) if truthPose is not None: #print 'Publishing truthPose %s' % arm truthPub[arm].publish(truthPose.msg.PoseStamped()) if estimatedPoseisEstimate is not None: estimatedPose, isEstimate = estimatedPoseisEstimate #print 'Publish estimatedPose %s' % arm if estimatedPoseisEstimate is not None and prevPoseAndIsEstimate.has_key(arm): prevPose, prevIsEstimate = prevPoseAndIsEstimate[arm] if prevIsEstimate is True and isEstimate is False: deltaPose = Util.deltaPose(prevPose, estimatedPose) print 'deltaPose between last estimate and current truth' print deltaPose #print 'truthPose_{0}: {1}'.format(arm,truthPose) #print 'isEstimate: {0}'.format(isEstimate) #print 'estimatedPose_{0}: {1}'.format(arm,estimatedPose) if estimatedPoseisEstimate is not None: prevPoseAndIsEstimate[arm] = (estimatedPose, isEstimate) rospy.sleep(.02)
def foamCallback(self, msg): time = self.listener.getLatestCommonTime(Constants.AR.Frames.Base, msg.header.frame_id) msg.header.stamp = time self.objectPoint = self.listener.transformPoint(Constants.AR.Frames.Base,msg) """ tfxMsg = tfx.point(msg) tf_msgframe_to_base = tfx.lookupTransform(Constants.AR.Frames.Base, tfxMsg.frame, wait=10) tfxMsg = tf_msgframe_to_base * tfxMsg self.objectPoint = tfxMsg.msg.PointStamped() """ pose = self.getObjectPose() pos = pose.pose.position ori = pose.pose.orientation self.tf_br.sendTransform((pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w), rospy.Time.now(), 'object_frame', pose.header.frame_id) marker = Util.createMarker(self.getObjectPose(), 0) self.objPublisher.publish(marker)
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 _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 tapeCallback(self, msg): if not self.record: return self.recordCount += 1 rospy.loginfo('Received tape pose number {0}'.format(self.recordCount)) tapePose = tfx.pose(msg) stamp = tapePose.stamp tf_tape_to_link0 = tfx.lookupTransform(Constants.Frames.Link0, tapePose.frame, wait=4) tapePose = tf_tape_to_link0 * tapePose tfPose = tfx.pose([0,0,0], frame=self.toolFrame, stamp=stamp) tf_tfpose_to_link0 = tfx.lookupTransform(Constants.Frames.Link0, tfPose.frame, wait=4) tfPose = tf_tfpose_to_link0 * tfPose deltaPose = tfx.pose(Util.deltaPose(tfPose, tapePose)) self.tapePoses.append(tapePose) self.tfPoses.append(tfPose) self.deltaPoses.append(deltaPose)
def testFK(): rospy.init_node('test_IK',anonymous=True) rp = RavenPlanner(MyConstants.Arm.Both) rp.env.SetViewer('qtcoin') rospy.sleep(2) leftPose = rp.getCurrentPose(MyConstants.Arm.Left) rightPose = rp.getCurrentPose(MyConstants.Arm.Right) leftGrasp = rp.getCurrentGrasp(MyConstants.Arm.Left) rightGrasp = rp.getCurrentGrasp(MyConstants.Arm.Right) leftJoints = invArmKin(MyConstants.Arm.Left, leftPose, leftGrasp) rightJoints = invArmKin(MyConstants.Arm.Right, rightPose, rightGrasp) rp.updateOpenraveJoints(MyConstants.Arm.Left, joints1=leftJoints, grasp=leftGrasp) rp.updateOpenraveJoints(MyConstants.Arm.Right, joints1=rightJoints, grasp=rightGrasp) raveLeftMatrix = Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.LeftTool, MyConstants.Frames.Link0) raveRightMatrix = Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.RightTool, MyConstants.Frames.Link0) raveLeftPose = tfx.pose(raveLeftMatrix,frame=leftPose.frame) raveRightPose = tfx.pose(raveRightMatrix,frame=rightPose.frame) deltaLeft = Util.deltaPose(leftPose, raveLeftPose) deltaRight = Util.deltaPose(rightPose, raveRightPose) g = [] #g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.LeftTool, 'world')) #g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.RightTool, 'world')) g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, leftPose.matrix, leftPose.frame[1:], 'world')) g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, rightPose.matrix, rightPose.frame[1:], 'world')) IPython.embed()
def main(): import roslib roslib.load_manifest('RavenDebridement') from RavenDebridement.srv import InvKinSrv import rospy from RavenDebridement.RavenCommand.RavenArm import RavenArm import IPython rospy.init_node('testKinematics', anonymous=True) rospy.loginfo('Waiting for IK server...') rospy.wait_for_service('inv_kin_server', timeout=5) inv_kin_service = rospy.ServiceProxy('inv_kin_server', InvKinSrv) from std_msgs.msg import Header header = Header() header.frame_id = '/0_link' header.stamp = rospy.Time.now() min_limits = np.array([ SHOULDER_MIN_LIMIT, ELBOW_MIN_LIMIT, Z_INS_MIN_LIMIT, TOOL_ROLL_MIN_LIMIT, TOOL_WRIST_MIN_LIMIT, TOOL_GRASP1_MIN_LIMIT, TOOL_GRASP2_MIN_LIMIT ]) max_limits = np.array([ SHOULDER_MAX_LIMIT, ELBOW_MAX_LIMIT, Z_INS_MAX_LIMIT, TOOL_ROLL_MAX_LIMIT, TOOL_WRIST_MAX_LIMIT, TOOL_GRASP1_MAX_LIMIT, TOOL_GRASP2_MAX_LIMIT ]) armName = GOLD_ARM_ID for i in xrange(1): r = np.random.rand(len(min_limits)) val = min_limits + r * (max_limits - min_limits) joints1 = dict(zip([0, 1, 2, 4, 5, 6, 7], val.tolist())) joints1[TOOL_ROT] = fix_angle(joints1[TOOL_ROT]) pose, grasp = fwdArmKin(armName, joints1) joints2 = invArmKin(armName, pose, grasp) joints2[TOOL_ROT] = fix_angle(joints2[TOOL_ROT]) try: srvArmName = 0 if armName == GOLD_ARM_ID else 1 resp = inv_kin_service(header, srvArmName, grasp, pose.msg.Pose()) joints3 = dict( (joint.type, joint.position) for joint in resp.joints) except (rospy.ServiceException, rospy.ROSException) as e: rospy.loginfo("IK server failure: %s" % e) # print '\t'.join("%.3f" % v for v in joints1.values()) # print '\t'.join("%.3f" % v for v in joints2.values()) # print '\t'.join("%.3f" % v for v in joints3.values()) # print np.array(joints1.values()) - np.array(joints2.values()) def invArmKinSrv(armId, pose, grasp): srvArmName = 0 if armId == 'L' else 1 resp = inv_kin_service(header, srvArmName, grasp, pose.msg.Pose()) resp_joints = dict((joint.type, joint.position) for joint in resp.joints if joint.type <= 7) return resp_joints print '-----------------------------------------------' jl, pl, gl, jr, pr, gr = test_config4() """ la = RavenArm('L') #ra = RavenArm('R') la.start() #ra.start() rospy.sleep(2) la.stop() #ra.stop() print '\n------p->j------' print '--L--' jl1 = invArmKin('L', pl, gl) jl2 = invArmKinSrv('L', pl, gl) print 'actual:', joint_str(jointRad2deg(jl)) print 'calc: ', joint_str(jointRad2deg(jl1)) print 'calc2: ', joint_str(jointRad2deg(jl2)) """ print '\n------j->p------' print '--L--' pl1, gl1 = fwdArmKin('L', jl) print 'actual:', pl print 'calc: ', pl1 print 'actual:', gl print 'calc: ', gl1 print '--R--' pr1, gr1 = fwdArmKin('R', jr) print 'actual:', pr print 'calc: ', pr1 print 'actual:', gr print 'calc: ', gr1 print '\n------p->j------' print '--L--' jl1 = invArmKin('L', pl, gl) jl2 = invArmKinSrv('L', pl, gl) print 'actual:', joint_str(jl) print 'calc: ', joint_str(jl1) print 'calc2: ', joint_str(jl2) print '--R--' jr1 = invArmKin('R', pr, gr) jr2 = invArmKinSrv('R', pr, gr) print 'actual:', joint_str(jr) print 'calc: ', joint_str(jr1) print 'calc2: ', joint_str(jr2) print '\n------j->p->j------' print '--L--' jl1 = invArmKin('L', pl1, gl1) print 'actual:', joint_str(jl) print 'calc: ', joint_str(jl1) print '--R--' jr1 = invArmKin('R', pr1, gr1) print 'actual:', joint_str(jr) print 'calc: ', joint_str(jr1) print '\n------p->j->p------' print '--L--' pl1, gl1 = fwdArmKin('L', jl1) print 'actual:', pl print 'calc: ', pl1 print 'actual:', gl print 'calc: ', gl1 print '--R--' pr1, gr1 = fwdArmKin('R', jr1) print 'actual:', pr print 'calc: ', pr1 print 'actual:', gr print 'calc: ', gr1 from RavenDebridement.Utils import Util d = Util.deltaPose(pl, pl1) a = tfx.tb_angles(d.orientation) IPython.embed()