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 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
