hrp2.py 3.97 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
# -*- 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

import numpy as np

21
#Don't change this order
22
from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
23 24 25 26
from dynamic_graph.ros import RosRobotModel

from rospkg import RosPack

27

28 29 30 31 32 33 34 35
# Internal helper tool.
def matrixToTuple(M):
    tmp = M.tolist()
    res = []
    for i in tmp:
        res.append(tuple(i))
    return tuple(res)

36 37 38 39
class Hrp2(AbstractHumanoidRobot):
    """
    This class instanciates a Hrp2 robot
    """
40 41 42 43 44 45 46 47 48 49

    forceSensorInLeftAnkle =  ((1.,0.,0.,0.),
                               (0.,1.,0.,0.),
                               (0.,0.,1.,-0.105),
                               (0.,0.,0.,1.))
    forceSensorInRightAnkle = ((1.,0.,0.,0.),
                               (0.,1.,0.,0.),
                               (0.,0.,1.,-0.105),
                               (0.,0.,0.,1.))

50 51 52 53 54 55 56 57 58 59 60 61 62 63
    accelerometerPosition = np.matrix ((
            (1., 0., 0., -.13,),
            (0., 1., 0., 0.,),
            (0., 0., 1., .118,),
            (0., 0., 0., 1.,),
            ))

    gyrometerPosition = np.matrix ((
            (1., 0., 0., -.13,),
            (0., 1., 0., 0.,),
            (0., 0., 1., .118,),
            (0., 0., 0., 1.,),
            ))

64 65 66 67
    def smallToFull(self, config):
        res = (config + 10*(0.,))
        return res

68 69
    def __init__(self, name, robotnumber,
                 device = None,
70
                 tracer = None):
71
        
72 73 74
        AbstractHumanoidRobot.__init__ (self, name, tracer)

        self.OperationalPoints.append('waist')
75
        self.OperationalPoints.append('chest')
76
        self.device = device
77
        
78 79 80 81 82 83
        self.AdditionalFrames.append(
            ("accelerometer",
             matrixToTuple(self.accelerometerPosition), "chest"))
        self.AdditionalFrames.append(
            ("gyrometer",
             matrixToTuple(self.gyrometerPosition), "chest"))
84 85 86 87 88
        self.AdditionalFrames.append(
            ("leftFootForceSensor",
             self.forceSensorInLeftAnkle, "left-ankle"))
        self.AdditionalFrames.append(
            ("rightFootForceSensor",
89 90 91 92 93 94 95 96 97 98
             self.forceSensorInRightAnkle, "right-ankle"))
        self.OperationalPointsMap = {'left-wrist'  : 'LARM_JOINT5',
                                     'right-wrist' : 'RARM_JOINT5',
                                     'left-ankle'  : 'LLEG_JOINT5',
                                     'right-ankle' : 'RLEG_JOINT5',
                                     'gaze'        : 'HEAD_JOINT1',
                                     'waist'       : 'WAIST',
                                     'chest'       : 'CHEST_JOINT1'}

        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
99

100 101 102 103 104


        rospack = RosPack()
        self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(robotnumber)) + '/urdf/hrp2_{0}.urdf'.format(robotnumber)
        self.dynamic.loadUrdf(self.urdfPath)
105 106

        self.dimension = self.dynamic.getDimension()
107
        self.dynamic.displayModel()
108
        self.plugVelocityFromDevice = True
109
        if self.dimension != len(self.halfSitting):
110
            raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dimension))
111
        self.initializeRobot()
112
        
113
__all__ = [Hrp2]