robot.py.in 4.87 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
# General Lesser Public License for more details.  You should have
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.

from __future__ import print_function
from dynamic_graph.sot.hrp2 import Hrp2
19
import numpy as np
olivier stasse's avatar
olivier stasse committed
20
from dynamic_graph.sot.hrp2.dynamic_hrp2_10 import DynamicHrp2_10
21
hrp2_10_pkgdatarootdir = "@HRP2_10_PKGDATAROOTDIR@/hrp2-10"
22

Damien Petit's avatar
Damien Petit committed
23
24
25
26
27
28
29
30
# Internal helper tool.
def matrixToTuple(M):
    tmp = M.tolist()
    res = []
    for i in tmp:
        res.append(tuple(i))
    return tuple(res)

31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
class Robot (Hrp2):
    """
    This class instanciates LAAS Hrp2 robot
    """
    halfSitting = (
        # Free flyer
        0., 0., 0.648702, 0., 0. , 0.,

        # Legs
        0., 0., -0.453786, 0.872665, -0.418879, 0.,
        0., 0., -0.453786, 0.872665, -0.418879, 0.,

        # Chest and head
        0., 0., 0., 0.,

        # Arms
        0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1, 0.261799,
        0.261799, 0.17453,  0., -0.523599, 0., 0., 0.1, 0.261799,
        )

    def __init__(self, name,
                 modelDir = hrp2_10_pkgdatarootdir,
                 xmlDir = hrp2_10_pkgdatarootdir,
                 device = None,
                 tracer = None):
Damien Petit's avatar
Damien Petit committed
56
57
58

        # Define camera positions w.r.t gaze.

59
60
        # These positions have been copied from hrp2_10.urdf
        # except for cameraTopLeftPosition and cameraTopRightPosition
Damien Petit's avatar
Damien Petit committed
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
        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"))

124
125
126
        Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10,
                      tracer)

127
__all__ = ["Robot"]