robot.py 4.04 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
olivier stasse's avatar
olivier stasse committed
19
import numpy as np
olivier stasse's avatar
olivier stasse committed
20

21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
# Internal helper tool.
def matrixToTuple(M):
    tmp = M.tolist()
    res = []
    for i in tmp:
        res.append(tuple(i))
    return tuple(res)

class Robot (Hrp2):
    """
    This class instanciates LAAS Hrp2 robot
    """
    halfSitting = (
        # Free flyer
        0., 0., 0.648702, 0., 0. , 0.,

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

40
        # Left Arm
41
        0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1,
42 43 44 45
        # Left hand
        #0.,0.,0.,0.,0.,
        
        #Right Arm
46
        0.261799, 0.17453,  0., -0.523599, 0., 0., 0.1,
47 48 49 50 51 52 53
        #Right Hand
        #0.,0.,0.,0.,0.,

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

54 55 56 57 58 59 60 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
        )

    def __init__(self, name,
                 device = None,
                 tracer = None):

        # Define camera positions w.r.t gaze.

        # These positions have been copied from hrp2.geom and
        # roughly checked. Do not trust them too much.
        cameraBottomLeftPosition = np.matrix((
                (0.98481, 0.00000, 0.17365, 0.035),
                (0.,      1.,      0.,      0.072),
                (-0.17365, 0.00000, 0.98481, 0.075 - 0.03),
                (0., 0., 0., 1.),
                ))
        cameraBottomRightPosition = np.matrix((
                (0.98481, 0.00000, 0.17365, 0.035),
                (0.,      1.,      0.,     -0.072),
                (-0.17365, 0.00000, 0.98481, 0.075 - 0.03),
                (0., 0., 0., 1.),
                ))
        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.),
                ))
        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.),
                ))

        # 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]:
101
            camera[:] = camera * c1_M_c
102 103 104 105 106 107 108 109 110 111 112 113 114 115

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

116
        Hrp2.__init__(self, name, "14", device, tracer)
117

118
__all__ = ["Robot"]