def testConvertFromObjToAnotherObj(self):
arm_state = ArmState()
arm_state.ee_pose.position.x = 0.05
arm_state.ee_pose.position.y = 0
arm_state.ee_pose.position.z = 0
arm_state.ee_pose.orientation.w = 1
arm_state.refFrame = ArmState.OBJECT
arm_state.refFrameLandmark.name = 'landmark1'
arm_state.refFrameLandmark.pose.position.x = 0.15
arm_state.refFrameLandmark.pose.position.y = 0
arm_state.refFrameLandmark.pose.position.z = 0
arm_state.refFrameLandmark.pose.orientation.w = 1
landmark = Landmark()
landmark.name = 'landmark2'
landmark.pose.position.x = 0.2
landmark.pose.position.y = 0.1
landmark.pose.position.z = 0
landmark.pose.orientation.w = 1
rel_arm_state = world.convert_ref_frame(arm_state, ArmState.OBJECT,
landmark)
self.assertAlmostEqual(rel_arm_state.ee_pose.position.x, 0)
self.assertAlmostEqual(rel_arm_state.ee_pose.position.y, -0.1)
# Test copy behavior for passed-in landmark
rel_arm_state.refFrameLandmark.name = 'modified'
self.assertNotEqual(rel_arm_state.refFrameLandmark.name, landmark.name)
def testConvertFromBaseToObj(self):
arm_state = ArmState()
arm_state.ee_pose.position.x = 0.2
arm_state.ee_pose.position.y = 0
arm_state.ee_pose.position.z = 0
arm_state.ee_pose.orientation.w = 1
arm_state.refFrame = ArmState.ROBOT_BASE
landmark = Landmark()
landmark.name = 'landmark1'
landmark.pose.position.x = 0.15
landmark.pose.position.y = 0
landmark.pose.position.z = 0
landmark.pose.orientation.w = 1
rel_arm_state = world.convert_ref_frame(arm_state, ArmState.OBJECT,
landmark)
self.assertAlmostEqual(rel_arm_state.ee_pose.position.x, 0.05)
self.assertEqual(rel_arm_state.refFrame, ArmState.OBJECT)
self.assertEqual(rel_arm_state.refFrameLandmark.pose.position.x, 0.15)
def make_ref(self, parent):
"""Creates the reference frame for the side of an object.
Args:
parent (Marker)
Returns:
ref (Landmark)
"""
ref = Landmark()
ref.type = 1
ref.pose.position.x = parent.pose.position.x
ref.pose.position.y = parent.pose.position.y
ref.pose.position.z = parent.pose.position.z
ref.pose.orientation.x = parent.pose.orientation.x
ref.pose.orientation.y = parent.pose.orientation.y
ref.pose.orientation.z = parent.pose.orientation.z
ref.pose.orientation.w = parent.pose.orientation.w
ref.name = parent.ns + " Ref"
ref.dimensions.x = parent.scale.x
ref.dimensions.y = parent.scale.y
ref.dimensions.z = parent.scale.z
return ref