Commit 0c33ce83 authored by Thomas Moulard's avatar Thomas Moulard
Browse files

Merge pull request #4 from damien-petit/addCameraOpPoint

add camera frame in hrp2_10 close #3
parents 7beedfd7 5f16e67b
......@@ -45,6 +45,74 @@ class Robot (Hrp2):
xmlDir = hrp2_10_pkgdatarootdir,
device = None,
tracer = None):
# Define camera positions w.r.t gaze.
# These positions have been copied from hrp2_10.urdf
# except for cameraTopLeftPosition and cameraTopRightPosition
cameraBottomLeftPosition = np.matrix((
(0.98703661, 0.05887354, 0.14930717, 0.0514675),
(-0.06015316, 0.99818088, 0.00406493, 0.06761652),
(-0.14879625, -0.01299353, 0.98878251, 0.06929336 - 0.03),
(0., 0., 0., 1.),
))
cameraBottomRightPosition = np.matrix((
(0.97634419, -0.04283205, 0.21193734, 0.05735882),
(0.04475292, 0.99898895, -0.00427252, -0.07566727),
(-0.21154006, 0.01365627, 0.97727392, 0.07390919 - 0.03),
(0., 0., 0., 1.),
))
#very rough approximate coming from hrp2_14
cameraTopLeftPosition = np.matrix((
(0.99920, 0.00120, 0.03997, 0.01),
(0.00000, 0.99955,-0.03000, 0.029),
(-0.03999, 0.02997, 0.99875, 0.145 - 0.03),
(0., 0., 0., 1.),
))
#very rough approximate coming from hrp2_14
cameraTopRightPosition = np.matrix((
(0.99920, 0.00000, 0.03999, 0.01),
(0.00000, 1.00000, 0.00000, -0.029),
(-0.03999, 0.00000, 0.99920, 0.145 - 0.03),
(0., 0., 0., 1.),
))
cameraXtionRGBPosition = np.matrix((
(0.98162902, 0.02441221, 0.18923135, 0.0869229361),
(-0.02440555, 0.99969934, -0.00236575, 0.0149334883),
(-0.18923221, -0.002296, 0.98192968, 0.108828329 - 0.03),
(0., 0., 0., 1.),
))
# Frames re-orientation:
# Z = depth (increase from near to far)
# X = increase from left to right
# Y = increase from top to bottom
c1_M_c = np.matrix(
[[ 0., 0., 1., 0.],
[-1., 0., 0., 0.],
[ 0., -1., 0., 0.],
[ 0., 0., 0., 1.]])
for camera in [cameraBottomLeftPosition, cameraBottomRightPosition,
cameraTopLeftPosition, cameraTopRightPosition]:
camera = camera * c1_M_c
self.AdditionalFrames.append(
("cameraBottomLeft",
matrixToTuple(cameraBottomLeftPosition), "gaze"))
self.AdditionalFrames.append(
("cameraBottomRight",
matrixToTuple(cameraBottomRightPosition), "gaze"))
self.AdditionalFrames.append(
("cameraTopLeft",
matrixToTuple(cameraTopLeftPosition), "gaze"))
self.AdditionalFrames.append(
("cameraTopRight",
matrixToTuple(cameraTopRightPosition), "gaze"))
self.AdditionalFrames.append(
("cameraXtionRGB",
matrixToTuple(cameraXtionRGBPosition), "gaze"))
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10,
tracer)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment