def testMoveStraightLine(arm=raven_constants.Arm.Right):
rospy.init_node('raven_commander', anonymous=True)
ravenArm = RavenArm(arm, False)
rospy.sleep(1)
ravenArm.start()
rospy.loginfo('Press enter to start')
raw_input()
startPose = ravenArm.ravenController.currentPose
delta = tfx.pose([0.01, 0.00, 0.00])
endPose = raven_util.endPose(startPose, delta)
print "Start Pose:"
print startPose
print "End Pose:"
print endPose
# ravenArm.ravenController.goToPose(endPose)
rospy.loginfo('Press enter to stop')
raw_input()
ravenArm.ravenController.stop()
rospy.loginfo('Press enter to exit')
raw_input()
def test_gripperMove():
rospy.init_node('raven_controller',anonymous=True)
leftArm = RavenController(raven_constants.Arm.Left, defaultPoseSpeed=0.05)
rospy.sleep(2)
rospy.loginfo('Press enter to start')
raw_input()
leftArm.start()
startPose = leftArm.currentPose
delta = tfx.pose([0.01, 0.01, 0.01])
endPose = raven_util.endPose(startPose, delta)
# print "--Start Pose"
# print startPose
# print endPose
# print "--End Pose"
# tfx.pose([-0.02, -0.08, -0.17], tfx.tb_angles(-90,90,0), frame=startPose.frame)#raven_util.endPose(startPose, tfx.pose([-0.02, -0.08, -0.17]), startPose.frame)
leftArm.goToPose(endPose)
rospy.loginfo('Press enter to stop')
raw_input()
leftArm.stop()
rospy.loginfo('Press enter to exit')
raw_input()
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 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 testMoveStraightLine(arm=raven_constants.Arm.Right):
rospy.init_node('raven_commander',anonymous=True)
ravenArm = RavenArm(arm, False)
rospy.sleep(1)
ravenArm.start()
rospy.loginfo('Press enter to start')
raw_input()
startPose = ravenArm.ravenController.currentPose
delta = tfx.pose([0.01, 0.00, 0.00])
endPose = raven_util.endPose(startPose, delta)
print "Start Pose:"
print startPose
print "End Pose:"
print endPose
# ravenArm.ravenController.goToPose(endPose)
rospy.loginfo('Press enter to stop')
raw_input()
ravenArm.ravenController.stop()
rospy.loginfo('Press enter to exit')
raw_input()
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 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 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