robot.py 8.8 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
#!/usr/bin/env python
# Copyright (c) 2019 CNRS
# Author: Pierre Fernbach
#
# This file is part of hpp-rbprm-robot-data.
# hpp_tutorial 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.
#
# hpp_tutorial 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
# hpp_tutorial.  If not, see
# <http://www.gnu.org/licenses/>.

from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
from pinocchio import SE3, Quaternion
import numpy as np

class Robot (Parent):
    ##
    #  Information to retrieve urdf and srdf files.

27
28
    packageName = "anymal_data"
    meshPackageName = "anymal_data"
29
    rootJointType = "freeflyer"
30
31
    urdfName = "anymal"
    urdfSuffix = ""
Pierre Fernbach's avatar
Pierre Fernbach committed
32
    srdfSuffix = ""
33
34

    ## Information about the names of thes joints defining the limbs of the robot
Pierre Fernbach's avatar
Pierre Fernbach committed
35
36
    rLegId = 'RFleg'
    rleg = 'RF_HAA'
37
    rfoot = 'RF_ADAPTER_TO_FOOT'
Pierre Fernbach's avatar
Pierre Fernbach committed
38
39
    lLegId = 'LFleg'
    lleg = 'LF_HAA'
40
    lfoot = 'LF_ADAPTER_TO_FOOT'
Pierre Fernbach's avatar
Pierre Fernbach committed
41
42
    lArmId = 'LHleg'
    larm = 'LH_HAA'
43
    lhand = 'LH_ADAPTER_TO_FOOT'
Pierre Fernbach's avatar
Pierre Fernbach committed
44
45
    rArmId = 'RHleg'
    rarm = 'RH_HAA'
46
    rhand = 'RH_ADAPTER_TO_FOOT'
47

48
49
    
    referenceConfig_asymetric =[0.,0.,0.461, 0.,0.,0.,1., # FF
50
51
52
53
        0.0, 0.611, -1.0452,
        0.0, -0.853, 1.0847,
        -0.0, 0.74, -1.08,
        -0.0, -0.74, 1.08,
Pierre Fernbach's avatar
Pierre Fernbach committed
54
        ]
55
    
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
    """
    referenceConfig=[0,0,0.448,0,0,0,1,
        0.079,0.78,-1.1,
        0.079,-0.78,1.1,
        -0.079,0.78,-1.1,
        -0.079,-0.78,1.1
       ]
    """    
    """
    referenceConfig=[0,0,0.448,0,0,0,1,
        0.095,0.76,-1.074,
        0.095,-0.76,1.074,
        -0.095,0.76,-1.074,
        -0.095,-0.76,1.074
       ]
    """
    referenceConfig=[0,0,0.47,0,0,0,1,
        -0.12,0.724,-1.082,
        -0.12,-0.724,1.082,
        0.12,0.724,-1.082,
        0.12,-0.724,1.082
       ]
78
79
80
81
82
    postureWeights=[0,0,0,0,0,0, #FF
    100.,1.,1.,
    100.,1.,1.,
    100.,1.,1.,
    100.,1.,1.,]
Pierre Fernbach's avatar
Pierre Fernbach committed
83

84
    # informations required to generate the limbs databases the limbs : 
Pierre Fernbach's avatar
Pierre Fernbach committed
85
    nbSamples = 50000
86
    octreeSize = 0.002
Pierre Fernbach's avatar
Pierre Fernbach committed
87
    cType = "_3_DOF"
88
    offset = [0.,0.,-0.005] # was 0.005
Pierre Fernbach's avatar
Pierre Fernbach committed
89
90
91
92
93

    rLegLimbOffset = [0.373, 0.264, 0.]
    lLegLimbOffset = [0.373, -0.264,0.]
    rArmLimbOffset = [-0.373, 0.264, 0.]
    lArmLimbOffset = [-0.373, -0.264, 0.]
94
95
    normal = [0,0,1]
    legx = 0.02; legy = 0.02
Pierre Fernbach's avatar
Pierre Fernbach committed
96
    kinematicConstraintsPath="package://anymal-rbprm/com_inequalities/"
97

98
    minDist = 0.2
Pierre Fernbach's avatar
Pierre Fernbach committed
99
100
101
102

    # data used by scripts :,,,
    #limbs_names = [rArmId,lLegId,lArmId,rLegId] # reverse default order to try to remove contacts at the beginning of the contact plan
    #limbs_names = [lLegId,rArmId,rLegId,lArmId] # default order to try to remove contacts at the beginning of the contact plan
Pierre Fernbach's avatar
Pierre Fernbach committed
103
    limbs_names = [rArmId,rLegId,lArmId,lLegId]
Pierre Fernbach's avatar
Pierre Fernbach committed
104
    dict_limb_rootJoint = {rLegId:rleg, lLegId:lleg, rArmId:rarm, lArmId:larm}
105
106
107
108
    dict_limb_joint = {rLegId:rfoot, lLegId:lfoot, rArmId:rhand, lArmId:lhand}
    dict_limb_color_traj = {rfoot:[0,1,0,1], lfoot:[1,0,0,1],rhand:[0,0,1,1],lhand:[0.9,0.5,0,1]}
    FOOT_SAFETY_SIZE = 0.01
    # size of the contact surface (x,y)
109
    dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
Pierre Fernbach's avatar
Pierre Fernbach committed
110
    #dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
111
112
113
114
115
116
117
    #various offset used by scripts
    MRsole_offset = SE3.Identity()
    MRsole_offset.translation = np.matrix(offset).T
    MLsole_offset = MRsole_offset.copy()
    MRhand_offset = MRsole_offset.copy()
    MLhand_offset = MRsole_offset.copy()
    dict_offset = {rfoot:MRsole_offset, lfoot:MLsole_offset, rhand:MRhand_offset, lhand:MLhand_offset}
Pierre Fernbach's avatar
Pierre Fernbach committed
118
    dict_limb_offset= {rLegId:rLegLimbOffset, lLegId:lLegLimbOffset, rArmId:rArmLimbOffset, lArmId:lArmLimbOffset}
Pierre Fernbach's avatar
Pierre Fernbach committed
119
    dict_normal = {rfoot:normal, lfoot:normal, rhand:normal, lhand:normal}
120
121
122
123
124
125
126
    # display transform :
    MRsole_display = SE3.Identity()
    MLsole_display = SE3.Identity()
    MRhand_display = SE3.Identity()
    MLhand_display = SE3.Identity()
    dict_display_offset = {rfoot:MRsole_display, lfoot:MLsole_display, rhand:MRhand_display, lhand:MLhand_display}

127
128
    kneeIds = {"LF":9,"LH":12,"RF":15,"RH":18}

129
130
131
132
133
134
    def __init__ (self, name = None,load = True):
        Parent.__init__ (self,load)
        if load:
            self.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName, self.urdfSuffix, self.srdfSuffix)
        if name != None:
            self.name = name
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
        # save original bounds of the urdf for futur reset
        self.LF_HAA_bounds = self.getJointBounds('LF_HAA')
        self.LF_HFE_bounds = self.getJointBounds('LF_HFE')
        self.LF_HAA_bounds = self.getJointBounds('LF_KFE')

        self.RF_HAA_bounds = self.getJointBounds('RF_HAA')
        self.RF_HFE_bounds = self.getJointBounds('RF_HFE')
        self.RF_HAA_bounds = self.getJointBounds('RF_KFE')

        self.LH_HAA_bounds = self.getJointBounds('LH_HAA')
        self.LH_HFE_bounds = self.getJointBounds('LH_HFE')
        self.LH_HAA_bounds = self.getJointBounds('LH_KFE')

        self.RH_HAA_bounds = self.getJointBounds('RH_HAA')
        self.RH_HFE_bounds = self.getJointBounds('RH_HFE')
        self.RH_HAA_bounds = self.getJointBounds('RH_KFE')
Pierre Fernbach's avatar
Pierre Fernbach committed
151

152
    def loadAllLimbs(self,heuristic, analysis = None, nbSamples = nbSamples, octreeSize = octreeSize,disableEffectorCollision = False):
Pierre Fernbach's avatar
Pierre Fernbach committed
153
154
155
156
157
158
159
160
161
        if isinstance(heuristic,basestring):#only one heuristic name given assign it to all the limbs
            dict_heuristic = {}
            for id in self.limbs_names:
                dict_heuristic.update({id:heuristic})
        elif isinstance(heuristic,dict):
            dict_heuristic=heuristic
        else : 
            raise Exception("heuristic should be either a string or a map limbId:string")
        #dict_heuristic = {self.rLegId:"static", self.lLegId:"static", self.rArmId:"fixedStep04", self.lArmId:"fixedStep04"}
Pierre Fernbach's avatar
Pierre Fernbach committed
162
        for id in self.limbs_names:
Pierre Fernbach's avatar
Pierre Fernbach committed
163
            print "add limb : ",id
Pierre Fernbach's avatar
Pierre Fernbach committed
164
            eff = self.dict_limb_joint[id]
Pierre Fernbach's avatar
Pierre Fernbach committed
165
            print "effector name = ",eff
166
            self.addLimb(id,self.dict_limb_rootJoint[id],eff,self.dict_offset[eff].translation.T.tolist()[0],self.dict_normal[eff],self.dict_size[eff][0]/2.,self.dict_size[eff][1]/2.,nbSamples,dict_heuristic[id],octreeSize,self.cType,disableEffectorCollision = disableEffectorCollision,kinematicConstraintsPath=self.kinematicConstraintsPath+self.dict_limb_rootJoint[id]+"_06_com_constraints.obj",limbOffset=self.dict_limb_offset[id],kinematicConstraintsMin=self.minDist)
Pierre Fernbach's avatar
Pierre Fernbach committed
167
168
            if analysis :
                self.runLimbSampleAnalysis(id, analysis, True)
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188

    def setConstrainedJointsBounds(self):
        self.setJointBounds('LF_HAA',[-1.,1.])
        self.setJointBounds('LF_HFE',[-0.25,2.35])
        self.setJointBounds('LF_KFE',[-2.35,0.])

        self.setJointBounds('RF_HAA',[-1.,1.])
        self.setJointBounds('RF_HFE',[-0.4,2.35])
        self.setJointBounds('RF_KFE',[-2.35,0.])

        self.setJointBounds('LH_HAA',[-1.,1.])
        self.setJointBounds('LH_HFE',[-2.35,0.4])
        self.setJointBounds('LH_KFE',[0.,2.35])

        self.setJointBounds('RH_HAA',[-1.,1.])
        self.setJointBounds('RH_HFE',[-2.35,0.25])
        self.setJointBounds('RH_KFE',[0.,2.35])


    def setVeryConstrainedJointsBounds(self):
189
        self.setJointBounds('LF_HAA',[-0.4,0.4])
190
        self.setJointBounds('LF_HFE',[0.2,0.95])
191
192
        self.setJointBounds('LF_KFE',[-2.35,0.])

193
        self.setJointBounds('RF_HAA',[-0.4,0.4])
194
        self.setJointBounds('RF_HFE',[0.2,0.95])
195
196
        self.setJointBounds('RF_KFE',[-2.35,0.])

197
        self.setJointBounds('LH_HAA',[-0.4,0.4])
198
        self.setJointBounds('LH_HFE',[-1.,-0.5])
199
200
        self.setJointBounds('LH_KFE',[0.,2.35])

201
        self.setJointBounds('RH_HAA',[-0.4,0.4])
202
        self.setJointBounds('RH_HFE',[-1.,-0.5])
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
        self.setJointBounds('RH_KFE',[0.,2.35])


    def resetJointsBounds(self):
        self.setJointBounds('LF_HAA',self.LF_HAA_bounds)
        self.setJointBounds('LF_HFE',self.LF_HFE_bounds)
        self.setJointBounds('LF_KFE',self.LF_HAA_bounds)

        self.setJointBounds('RF_HAA',self.RF_HAA_bounds)
        self.setJointBounds('RF_HFE',self.RF_HFE_bounds)
        self.setJointBounds('RF_KFE',self.RF_HAA_bounds)

        self.setJointBounds('LH_HAA',self.LH_HAA_bounds)
        self.setJointBounds('LH_HFE',self.LH_HFE_bounds)
        self.setJointBounds('LH_KFE',self.LH_HAA_bounds)

        self.setJointBounds('RH_HAA',self.RH_HAA_bounds)
        self.setJointBounds('RH_HFE',self.RH_HFE_bounds)
        self.setJointBounds('RH_KFE',self.RH_HAA_bounds)


Pierre Fernbach's avatar
Pierre Fernbach committed
224