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 = raven_util.endPose(startPose, deltaPose, self.commandFrame)
self.goToGripperPose(endPose, startPose=startPose, block=block, duration=duration, speed=speed, ignoreOrientation=ignoreOrientation)
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 = [raven_util.endPose(startPose, deltaPose, self.commandFrame) for deltaPose in deltaPoses]
#IPython.embed()
print
print 'START', startPose
print 'END', endPoses[-1]
print
'''IPython.embed()
'''
return self.executePoseTrajectory(endPoses, block=block, speed=speed, ignoreOrientation=ignoreOrientation)
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
def getFullTrajectoryFromPose(self, armName, endPose, startPose=None, endGrasp = None, n_steps=50, block=True, approachDir=None, speed=None, correctTrajectory=False):
success = False
numTries = 0
while not success and numTries < 10:
self.waitForState()
joints1 = self.getCurrentJoints(armName)
if startPose is None:
startPose = self.getCurrentPose(armName)
startGrasp = self.getCurrentGrasp(armName)
if endGrasp is None:
endGrasp = startGrasp
self.setStartAndEndPose(armName, startPose, endPose, startGrasp=startGrasp, endGrasp=endGrasp)
self.trajSteps[armName] = n_steps
with self.lock:
self.trajRequest[armName] = True
if self.trajEndJoints[armName] is None:
print armName, startPose, startGrasp, endPose, endGrasp
raise Exception()
self.approachDir[armName] = approachDir
self.start_pose_pubs[armName].publish(startPose.msg.PoseStamped())
self.end_pose_pubs[armName].publish(endPose.msg.PoseStamped())
if block:
print 'waiting for arm {} traj'.format(armName)
while self.trajRequest[armName] and not rospy.is_shutdown():
rospy.sleep(0.05)
print 'received arm {} traj'.format(armName)
endPoses = [raven_util.endPose(startPose, deltaPose) for deltaPose in self.deltaPoseTraj[armName]]
success = (endPoses[-1].position.distance(endPose.position) <= 0.005)
numTries = numTries+1
if endPoses[-1].position.distance(endPose.position) > 0.005:
print 'FAIL'
if correctTrajectory:
# run a second time to correct (needed the desired approach direction)
if self.errorModel is not None:
#startPoseCorrected, startPoseOffset = self.errorModel.predictSinglePose(armName, startPose, startPose)
dt = 1
secondLastPose = endPoses[-2]
lastPose = endPoses[-1]
if speed is not None:
dt = lastPose.position.distance(secondLastPose.position) / speed
endPoseCorrected, endPoseOffset = self.errorModel.predictSinglePose(armName, lastPose, secondLastPose, dt) # << this is wrong, need true delta
print 'End Before', self.poseTraj[armName][n_steps-1]
correctedTraj = self.getFullTrajectoryFromPose(armName, endPoseCorrected, startPose=startPose,
endGrasp=endGrasp, n_steps=n_steps, block=block, approachDir=approachDir, correctTrajectory=False)
print
print armName
for p in endPoses:
print 'Pose', p
print 'START', startPose
print 'END', self.poseTraj[armName][n_steps-1]
print 'ORIG', endPose
print 'dt', dt
print
# if armName == 'R':
# IPython.embed()
return correctedTraj
return (self.poseTraj[armName][0], self.deltaPoseTraj[armName])
