Pepper robot: on which body frame is "LandmarkDetection" based? - matrix

Question

For a while I have tried to figure out that on which coordinate or body frame the ”LandmarkDetection“ Module is based on . Because I need the (0, 0, 0) position for the calculation to achieve the self localization for the robot with the help of "Landmark detection", I think it's already important...
I have letted it working together with the Pepper robot under NAOqi 2.5.5 which I hope it's compatible.
You can read the whole code of "LandmarkDetection.py" here:
"LandmarkDetection.py":
#! /usr/bin/env python
# -*- encoding: UTF-8 -*-
"""Example: Demonstrates a way to localize the robot with
ALLandMarkDetection"""
import qi
import time
import sys
import argparse
import math
import almath
class LandmarkDetector(object):
"""
We first instantiate a proxy to the ALLandMarkDetection module
Note that this module should be loaded on the robot's naoqi.
The module output its results in ALMemory in a variable
called "LandmarkDetected".
We then read this ALMemory value and check whether we get
interesting things.
After that we get the related position of the landmark compared to robot.
"""
def __init__(self, app):
"""
Initialisation of qi framework and event detection.
"""
super(LandmarkDetector, self).__init__()
app.start()
session = app.session
# Get the service ALMemory.
self.memory = session.service("ALMemory")
# Connect the event callback.
# Get the services ALMotion & ALRobotPosture.
self.motion_service = session.service("ALMotion")
self.posture_service = session.service("ALRobotPosture")
self.subscriber = self.memory.subscriber("LandmarkDetected")
print "self.subscriber = self.memory.subscriber(LandmarkDetected)"
self.subscriber.signal.connect(self.on_landmark_detected)
print "self.subscriber.signal.connect(self.on_landmark_detected)"
# Get the services ALTextToSpeech, ALLandMarkDetection and ALMotion.
self.tts = session.service("ALTextToSpeech")
self.landmark_detection = session.service("ALLandMarkDetection")
# print "self.landmark_detection" is repr(self.landmark_detection)
self.motion_service = session.service("ALMotion")
self.landmark_detection.subscribe("LandmarkDetector", 500, 0.0 )
print "self.landmark_detection.subscribe(LandmarkDetector, 500, 0.0 )"
self.got_landmark = False
# Set here the size of the landmark in meters.
self.landmarkTheoreticalSize = 0.06 #in meters 0 #.05 or 0.06?
# Set here the current camera ("CameraTop" or "CameraBottom").
self.currentCamera = "CameraTop"
def on_landmark_detected(self, markData):
"""
Callback for event LandmarkDetected.
"""
while markData == [] : # empty value when the landmark disappears
self.got_landmark = False
self.motion_service.moveTo(0, 0, 0.1 * math.pi)
if not self.got_landmark: # only speak the first time a landmark appears
self.got_landmark = True
#stop.motion_service.moveTo
print "Ich sehe eine Landmarke! "
self.tts.say("Ich sehe eine Landmarke! ")
# Retrieve landmark center position in radians.
wzCamera = markData[1][0][0][1]
wyCamera = markData[1][0][0][2]
# Retrieve landmark angular size in radians.
angularSize = markData[1][0][0][3]
# Compute distance to landmark.
distanceFromCameraToLandmark = self.landmarkTheoreticalSize / ( 2 * math.tan( angularSize / 2))
# Get current camera position in NAO space.
transform = self.motion_service.getTransform(self.currentCamera, 2, True)
transformList = almath.vectorFloat(transform)
robotToCamera = almath.Transform(transformList)
# Compute the rotation to point towards the landmark.
cameraToLandmarkRotationTransform = almath.Transform_from3DRotation(0, wyCamera, wzCamera)
# Compute the translation to reach the landmark.
cameraToLandmarkTranslationTransform = almath.Transform(distanceFromCameraToLandmark, 0, 0)
# Combine all transformations to get the landmark position in NAO space.
robotToLandmark = robotToCamera * cameraToLandmarkRotationTransform *cameraToLandmarkTranslationTransform
# robotTurnAroundAngle = almath.rotationFromAngleDirection(0, 1, 1, 1)
# print "robotTurnAroundAngle = ", robotTurnAroundAngle
print "x " + str(robotToLandmark.r1_c4) + " (in meters)"
print "y " + str(robotToLandmark.r2_c4) + " (in meters)"
print "z " + str(robotToLandmark.r3_c4) + " (in meters)"
def run(self):
"""
Loop on, wait for events until manual interruption.
"""
# Wake up robot
self.motion_service.wakeUp()
# Send robot to Pose Init
self.posture_service.goToPosture("StandInit", 0.5)
# Example showing how to get a simplified robot position in world.
useSensorValues = False
result = self.motion_service.getRobotPosition(useSensorValues)
print "Robot Position", result
# Example showing how to use this information to know the robot's diplacement.
useSensorValues = False
# initRobotPosition = almath.Pose2D(self.motion_service.getRobotPosition(useSensorValues))
# Make the robot move
for i in range(1, 12, 1):
self.motion_service.moveTo(0, 0, 0.1 * math.pi)
print "self.motion_service.moveTo(0, 0, (0.1)*math.pi)"
print "Starting LandmarkDetector"
try:
while True:
time.sleep(1)
except KeyboardInterrupt:
print "Interrupted by user, stopping LandmarkDetector"
self.landmark_detection.unsubscribe("LandmarkDetector")
#stop
sys.exit(0)
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="10.0.0.10",
help="Robot IP address. On robot or Local Naoqi: use
'10.0.0.10'.")
parser.add_argument("--port", type=int, default=9559,
help="Naoqi port number")
args = parser.parse_args()
try:
# Initialize qi framework.
connection_url = "tcp://" + args.ip + ":" + str(args.port)
app = qi.Application(["LandmarkDetector", "--qi-url=" + connection_url])
except RuntimeError:
print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
"Please check your script arguments. Run with -h option for help.")
sys.exit(1)
landmark_detector = LandmarkDetector(app)
landmark_detector.run()
At line 69 like the picture shows it says NAO space, but after googling and looking after this concept at the offcial Aledebaran site, I did't find it.
At line 70 from the function "getTransform()" of " transform = self.motion_service.getTransform(self.currentCamera, 2, True)" , combining with the information of the Aldebaran website: Cartesian control API: the parameter "2" could represent "FRAME_ROBOT ".
Due to the official documentation,"FRAME_ROBOT"(I think for NAO) is one of the 3 spatial references used by ALMotion. This is average of the two feet positions projected around a vertical z axis. This space is useful, because the x axis is always forwards, so provides a natural ego-centric reference.
But which from the following body frames is the FRAME coordinate for Pepper?
Body frames - Aldebaran
And basically the sensor values should be based on the Coordinate like it in this picture:
I have just tracked the values of the transformation:
('wzCamera = markData[1][0][0][1] = :', 0.20727014541625977)
('wyCamera = markData[1][0][0][2] = :', 0.13570936024188995)
robotToCamera = [[0.984479, 0.00163947, -0.175494, 0.0191678]
[-0.00241521, 0.999988, -0.00420686, -0.00231434]
[0.175485, 0.00456542, 0.984471, 1.16607]]
cameraToLandmarkRotationTransform =[[0.969599, -0.205789, 0.132397, 0]
[0.203897, 0.978596, 0.0278419, 0]
[-0.135293, 0, 0.990806, 0]]
cameraToLandmarkTranslationTransform=[[1, 0, 0, 1.32545]
[0, 1, 0, 0]
[0, 0, 1, 0]]
robotToLandmark = [[0.978627, -0.200991, -0.0434926, 1.31629]
[0.202122, 0.979082, 0.0233536, 0.265588]
[0.0378889, -0.0316453, 0.998781, 1.21629]]
Combining these with the information of the following picture
from the video: Robotics: 3D World to First Person Transformation
I am really wondering whether the “robot“ of “robotToCamera” means the original cemara point… Has anyone idea how to understand these values of matrix from “robotToCamera”?
Observing the values of “robotTocamera”, it changes a little bit when the robot rotates himself(the whole body + head, he doesn’t nod at the same time) to look for the landmarks on the walls like this:
1. robotToCamera =
[[0.979136, 0.0719852, -0.190025, 0.0157459]
[-0.0700786, 0.997401, 0.0167426, -0.00282726]
[0.190737, -0.00307653, 0.981636, 1.16688]]
2. robotToCamera =
[[0.981156, 0.0583434, -0.184198, 0.0168686]
[-0.057959, 0.998291, 0.00747475, -0.00518941]
[0.184319, 0.00334202, 0.982861, 1.16652]]
3. robotToCamera =
[[0.981628, 0.0598889, -0.18116, 0.0196037]
[-0.0594456, 0.9982, 0.0078808, -0.00525115]
[0.181306, 0.00303314, 0.983422, 1.16649]]
4. robotToCamera =
[[0.982268, 0.0583533, -0.178169, 0.0191384]
[-0.0579817, 0.998291, 0.0072969, -0.00522032]
[0.17829, 0.00316301, 0.983973, 1.16627] ]
5. robotToCamera =
[[0.984, 0.021, -0.178, 0.016]
[-0.021, 1.00, 0.0027, -0.0019]
[0.178, 0.001, 0.984, 1.17] ]
Here are couple of my questions:
Could we just calculate the position of “Robot” with help of “robotToCamera”?
Could we get a value for “CameraToRobot” using the values of “robotToCamera”, e.g. calculating the inverse matrix or adjoint matrix ?
For the fifth matrix from “robotToCamera”, if we calculate the inverse matrix for the part
[[0.984, 0.021, -0.178]
[-0.021, 1.00, 0.0027,]
[0.178, 0.001, 0.984] ]
would be
[[0.984, -0.021,-0.178]
[0.021, 1.00, -0.001,]
[-0.178, 0.0027, 0.984] ]
equal to its adjoint matrix. Then it should be a Orthogonal matrix due to Wikipedia.
What info can I get from this ?
At the time 6:10 of the video, the teacher continues to explain something about lens:
In my situation, there is no need to learn about lens(L), or?

Answer 1

Landmark detection is detecting the NAOMarks with its top camera. The doc explains that you get a "Shape info" in the event value such as:
ShapeInfo = [ 1, alpha, beta, sizeX, sizeY, heading]
alpha and beta represent the Naomark’s location in terms of camera
angles
sizeX and sizeY are the mark’s size in camera angles
the heading angle describes how the Naomark is oriented about the
vertical axis with regards to the robot’s head.
The code then calls getTransform which will give you the direction of the camera, so you can relate the detected mark angles to an actual direction (X,Y,Z) in front of the robot (becoming the robot frame).
marks are here if needed: http://doc.aldebaran.com/2-5/naoqi/vision/allandmarkdetection.html