def test_moveGripperDeltaAR():
rospy.init_node('gripper_control',anonymous=True)
listener = tf.TransformListener()
gripperControl = GripperControlClass(arm, listener)
imageDetector = ARImageDetectionClass()
rospy.sleep(4)
currPose = imageDetector.getGripperPose(arm)
desPose = imageDetector.getObjectPose()
deltaPose = Util.deltaPose(currPose, desPose, MyConstants.Frames.Link0, MyConstants.Frames.LeftTool)
deltaPose.position.z += .03
#code.interact(local=locals())
#return
#gripperControl.start()
rospy.loginfo('Press enter')
raw_input()
gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(MyConstants.Frames.Link0), deltaPose)
rospy.loginfo('exit')
raw_input()
def test_rotation():
rospy.init_node('gripper_control',anonymous=True)
listener = tf.TransformListener()
tf_br = tf.TransformBroadcaster()
gripperControl = GripperControlClass(arm, listener)
imageDetector = ARImageDetectionClass()
if arm == MyConstants.Arm.Left:
frame = MyConstants.Frames.Link0
toolframe = MyConstants.Frames.LeftTool
else:
frame = MyConstants.Frames.Link0
toolframe = MyConstants.Frames.RightTool
rospy.sleep(3)
while not (imageDetector.hasFoundGripper(arm) and imageDetector.hasFoundObject()) and not rospy.is_shutdown():
print imageDetector.hasFoundGripper(arm), imageDetector.hasFoundObject()
print 'searching'
rospy.sleep(1)
print imageDetector.hasFoundGripper(arm), imageDetector.hasFoundObject()
currPose = imageDetector.getGripperPose(arm)
desPose = imageDetector.getObjectPose(toolframe)
code.interact(local=locals())
"""
desPose = tfx.pose([0,0,0], imageDetector.normal).msg.PoseStamped()
while not listener.canTransform('tool_L', '0_link', rospy.Time()) and not rospy.is_shutdown():
rospy.sleep(1)
time = listener.getLatestCommonTime('tool_L', '0_link')
print currPose.header.frame_id
currPose.header.stamp = time
desPose.header.stamp = time
desPose.header.frame_id = '0_link'
currPose = listener.transformPose('tool_L', currPose)
desPose = listener.transformPose('tool_L', desPose)
"""
deltaPose = Util.deltaPose(currPose, desPose)
deltaPose = tfx.pose([0,0,0],deltaPose.orientation).msg.PoseStamped()
deltaPoseTb = tfx.tb_angles(deltaPose.pose.orientation)
#deltaPose = tfx.pose([0,0,0],deltaPose.orientation).msg.Pose()
print 'deltaPoseTb', deltaPoseTb
"""
gripper_tb = tfx.tb_angles(currPose.pose.orientation)
obj_tb = tfx.tb_angles(desPose.pose.orientation)
print 'actual', gripper_tb
print obj_tb
deltaPoseTb = tfx.pose(Util.deltaPose(currPose, desPose)).orientation
print 'deltaPose', deltaPoseTb
endQuatMat = gripper_tb.matrix * deltaPoseTb
print 'desOri', tfx.tb_angles(endQuatMat)
time = listener.getLatestCommonTime('tool_L', '0_link')
deltaPose.header.frame_id = '0_link'
deltaPose.header.stamp = time
deltaPose = listener.transformPose('tool_L', deltaPose)
#deltaPose = tfx.pose([0,0,0], deltaPose.orientation).msg.Pose()
deltaPoseTb = tfx.tb_angles(deltaPose.pose.orientation)
print 'deltaPoseTb', deltaPoseTb
currPose = tfx.pose(currPose)
endQuatMat = currPose.orientation.matrix * deltaPoseTb.matrix
print 'desOri', tfx.tb_angles(endQuatMat)
"""
yaw = tfx.pose([0,0,0], tfx.tb_angles(deltaPoseTb.yaw_deg,0,0))
pitch = tfx.pose([0,0,0], tfx.tb_angles(0,deltaPoseTb.pitch_deg,0))
roll = tfx.pose([0,0,0], tfx.tb_angles(0,0,deltaPoseTb.roll_deg))
rate = rospy.Rate(1)
print "press enter"
raw_input()
"""
startPose = tfx.pose(gripperControl.getGripperPose(MyConstants.Frames.Link0))
print 'start', startPose
endQuatMat = startPose.orientation.matrix * tfx.tb_angles(deltaPose.orientation).matrix
print tfx.tb_angles(endQuatMat)
"""
gripperControl.start()
gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(frame), deltaPose, duration=4)
"""
#gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(MyConstants.Frames.Link0), deltaPose, duration=4)
print 'yaw', yaw
raw_input()
gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(frame), yaw, duration=4)
print 'pitch', pitch
gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(frame), pitch, duration=4)
print 'roll', roll
gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(frame), roll, duration=4)
"""
while not rospy.is_shutdown():
rospy.loginfo('loop')
currPose = tfx.pose(gripperControl.getGripperPose(frame))
rate.sleep()