hrp2.py 4.2 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_pinocchio.humanoid_robot import AbstractHumanoidRobot
23
from dynamic_graph.ros import RosRobotModel
24
import pinocchio as se3
25
26
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
class Hrp2(AbstractHumanoidRobot):
    """
38
    This class instantiates a Hrp2 robot
39
    """
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
    def __init__(self, name, robotnumber,
69
                 device = None, tracer = None):
70
        
71
72
73
        AbstractHumanoidRobot.__init__ (self, name, tracer)

        self.OperationalPoints.append('waist')
74
        self.OperationalPoints.append('chest')
75
        self.device = device
76
        
77
78
79
80
81
82
        self.AdditionalFrames.append(
            ("accelerometer",
             matrixToTuple(self.accelerometerPosition), "chest"))
        self.AdditionalFrames.append(
            ("gyrometer",
             matrixToTuple(self.gyrometerPosition), "chest"))
83
84
85
86
87
        self.AdditionalFrames.append(
            ("leftFootForceSensor",
             self.forceSensorInLeftAnkle, "left-ankle"))
        self.AdditionalFrames.append(
            ("rightFootForceSensor",
88
89
90
91
92
93
94
95
96
97
             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))
98

99
        
100
101

        rospack = RosPack()
102
        self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(robotnumber)) + '/urdf/hrp2_{0}_reduced.urdf'.format(robotnumber)
103

104
105
106
107
        self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath, se3.JointModelFreeFlyer())
        self.pinocchioData = self.pinocchioModel.createData()
        self.dynamic.setModel(self.pinocchioModel)
        self.dynamic.setData(self.pinocchioData)
108
        self.dimension = self.dynamic.getDimension()
109
        self.plugVelocityFromDevice = True
110
        if self.dimension != len(self.halfSitting):
111
            raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dimension))
112
        self.initializeRobot()
113
        self.dynamic.displayModel()
114
__all__ = [Hrp2]