robot.py 8.19 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 =[0.,0.,0.47, 0.,0.,0.,1., # FF
50
51
        0.04, 0.611, -1.0452,
        0.04, -0.853, 1.0847,
52
53
        -0.04, 0.74, -1.08,
        -0.04, -0.74, 1.08,
Pierre Fernbach's avatar
Pierre Fernbach committed
54
        ]
55
    
56
57
58
59
60
    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
61

62
    # informations required to generate the limbs databases the limbs : 
Pierre Fernbach's avatar
Pierre Fernbach committed
63
64
65
    nbSamples = 50000
    octreeSize = 0.01
    cType = "_3_DOF"
66
    offset = [0.,0.,-0.011]
Pierre Fernbach's avatar
Pierre Fernbach committed
67
68
69
70
71

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

76
    minDist = 0.2
Pierre Fernbach's avatar
Pierre Fernbach committed
77
78
79
80
81

    # 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
    limbs_names = [rArmId,lArmId,lLegId,rLegId]
Pierre Fernbach's avatar
Pierre Fernbach committed
82
    dict_limb_rootJoint = {rLegId:rleg, lLegId:lleg, rArmId:rarm, lArmId:larm}
83
84
85
86
    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)
87
    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
88
    #dict_size={rfoot:[0.01 , 0.01], lfoot:[0.01 , 0.01],rhand:[0.01 , 0.01],lhand:[0.01 , 0.01]}
89
90
91
92
93
94
95
    #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
96
    dict_limb_offset= {rLegId:rLegLimbOffset, lLegId:lLegLimbOffset, rArmId:rArmLimbOffset, lArmId:lArmLimbOffset}
Pierre Fernbach's avatar
Pierre Fernbach committed
97
    dict_normal = {rfoot:normal, lfoot:normal, rhand:normal, lhand:normal}
98
99
100
101
102
103
104
    # 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}

105
106
    kneeIds = {"LF":9,"LH":12,"RF":15,"RH":18}

107
108
109
110
111
112
    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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
        # 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
129
130

    def loadAllLimbs(self,heuristic, analysis = None, nbSamples = nbSamples, octreeSize = octreeSize):
Pierre Fernbach's avatar
Pierre Fernbach committed
131
132
133
134
135
136
137
138
139
        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
140
        for id in self.limbs_names:
Pierre Fernbach's avatar
Pierre Fernbach committed
141
            print "add limb : ",id
Pierre Fernbach's avatar
Pierre Fernbach committed
142
            eff = self.dict_limb_joint[id]
Pierre Fernbach's avatar
Pierre Fernbach committed
143
            print "effector name = ",eff
144
            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,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
145
146
            if analysis :
                self.runLimbSampleAnalysis(id, analysis, True)
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201

    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):
        self.setJointBounds('LF_HAA',[-1.,1.])
        self.setJointBounds('LF_HFE',[0,1.])
        self.setJointBounds('LF_KFE',[-2.35,0.])

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

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

        self.setJointBounds('RH_HAA',[-1.,1.])
        self.setJointBounds('RH_HFE',[-1.1,-0.2])
        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
202