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
