This repository has been archived by the owner on Nov 9, 2022. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathload_model.py
executable file
·124 lines (120 loc) · 3.98 KB
/
load_model.py
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
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#!/usr/bin/env python
"""
Load robot model with additional six DOFs for the floating base.
"""
import IPython
import openravepy
import os.path
import sys
free_flyer_xml = """
<environment>
<robot>
<kinbody>
<body name="FLYER_TX_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_TY_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_TZ_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_ROLL_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_PITCH_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_YAW_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<robot file="%s" name="%s">
<kinbody>
<joint name="FLYER_TX" type="slider" circular="true">
<body>FLYER_TX_LINK</body>
<body>FLYER_TY_LINK</body>
<axis>1 0 0</axis>
<limits>-10 +10</limits>
</joint>
<joint name="FLYER_TY" type="slider" circular="true">
<body>FLYER_TY_LINK</body>
<body>FLYER_TZ_LINK</body>
<axis>0 1 0</axis>
<limits>-10 +10</limits>
</joint>
<joint name="FLYER_TZ" type="slider" circular="true">
<body>FLYER_TZ_LINK</body>
<body>FLYER_ROLL_LINK</body>
<axis>0 0 1</axis>
<limits>-10 +10</limits>
</joint>
<joint name="FLYER_ROLL" type="hinge" circular="true">
<body>FLYER_ROLL_LINK</body>
<body>FLYER_PITCH_LINK</body>
<axis>1 0 0</axis>
</joint>
<joint name="FLYER_PITCH" type="hinge" circular="true">
<body>FLYER_PITCH_LINK</body>
<body>FLYER_YAW_LINK</body>
<axis>0 1 0</axis>
</joint>
<joint name="FLYER_YAW" type="hinge" circular="true">
<body>FLYER_YAW_LINK</body>
<body>%s</body>
<axis>0 0 1</axis>
</joint>
</kinbody>
</robot>
</robot>
</environment>
"""
if __name__ == "__main__":
if not sys.argv[-1].endswith('.dae'):
print "Usage: %s model.dae" % sys.argv[0]
sys.exit()
elif not os.path.isfile(sys.argv[-1]):
print "Could not open file %s" % sys.argv[-1]
sys.exit()
fpath = sys.argv[-1]
name = os.path.basename(os.path.splitext(fpath)[0])
root_body = \
'BODY' if 'JAXON' in name else \
'PELVIS_S' if 'JVRC' in name else \
'base_link' if 'Romeo' in name else \
'WAIST' # for NEXTAGE
env = openravepy.Environment()
env.LoadData(free_flyer_xml % (fpath, name, root_body))
env.SetViewer('qtcoin')
viewer = env.GetViewer()
viewer.SetBkgndColor([.8, .85, .9]) # RGB tuple
viewer.SetCamera(
[[-0.6292849, 0.34538411, -0.69621141, 1.87324536],
[0.7768542, 0.25382043, -0.57625753, 1.55049443],
[-0.02231752, -0.90348492, -0.42803846, 1.20491762],
[0., 0., 0., 1.]])
robot = env.GetRobots()[0]
if IPython.get_ipython() is None:
IPython.embed()