00001 <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.1//EN" "http://www.w3.org/TR/xhtml11/DTD/xhtml11.dtd">
00002 <html xmlns="http://www.w3.org/1999/xhtml" xml:lang="en-US">
00003 <head>
00004 <link rel="icon" href="/robotmodel/robot_model/static/hgicon.png" type="image/png" />
00005 <meta name="robots" content="index, nofollow" />
00006 <link rel="stylesheet" href="/robotmodel/robot_model/static/style-paper.css" type="text/css" />
00007
00008 <title>robotmodel-robot_model: d33189ce4a62 collada_urdf/src/collada_urdf.cpp</title>
00009 <style type="text/css">
00010 div.kforgenav {
00011 position: relative;
00012 top: 0;
00013 bottom: 0;
00014 height: 20px;
00015 z-index: 1000;
00016 width: 100%;
00017 }
00018
00019 .kforgenav ul {
00020 padding: 0;
00021 margin: 0;
00022 list-style: none;
00023 line-height: 1;
00024 cursor: pointer;
00025 height: auto;
00026 }
00027
00028 .kforgenav li {
00029 padding: 0;
00030 margin: 0;
00031 list-style: none;
00032 float: left;
00033 position: relative;
00034 text-align: left;
00035 }
00036
00037 .kforgenav a {
00038 display: block;
00039 padding: 4px 15px 3px;
00040 text-decoration: none;
00041 font-size: 11px;
00042 color: #000;
00043 border: 0;
00044 background: #fff;
00045 }
00046 </style></head>
00047 <body><div class="kforgenav"><ul><li><a href="/">kforge.ros.org</a></li><li><a href="/people/home/">Profile</a></li><li><a href="/projects/">Projects</a></li><li><a href="/projects/robotmodel/">Robot Model</a></li><li><a href="/projects/robotmodel/services/">Services</a></li><li><a href="/projects/robotmodel/members/">Members</a></li></ul></div>
00048
00049 <div class="container">
00050 <div class="menu">
00051 <div class="logo">
00052 <a href="http://mercurial.selenic.com/">
00053 <img src="/robotmodel/robot_model/static/hglogo.png" alt="mercurial" /></a>
00054 </div>
00055 <ul>
00056 <li><a href="/robotmodel/robot_model/shortlog/d33189ce4a62">log</a></li>
00057 <li><a href="/robotmodel/robot_model/graph/d33189ce4a62">graph</a></li>
00058 <li><a href="/robotmodel/robot_model/tags">tags</a></li>
00059 <li><a href="/robotmodel/robot_model/branches">branches</a></li>
00060 </ul>
00061 <ul>
00062 <li><a href="/robotmodel/robot_model/rev/d33189ce4a62">changeset</a></li>
00063 <li><a href="/robotmodel/robot_model/file/d33189ce4a62/collada_urdf/src/">browse</a></li>
00064 </ul>
00065 <ul>
00066 <li class="active">file</li>
00067 <li><a href="/robotmodel/robot_model/file/tip/collada_urdf/src/collada_urdf.cpp">latest</a></li>
00068 <li><a href="/robotmodel/robot_model/diff/d33189ce4a62/collada_urdf/src/collada_urdf.cpp">diff</a></li>
00069 <li><a href="/robotmodel/robot_model/annotate/d33189ce4a62/collada_urdf/src/collada_urdf.cpp">annotate</a></li>
00070 <li><a href="/robotmodel/robot_model/log/d33189ce4a62/collada_urdf/src/collada_urdf.cpp">file log</a></li>
00071 <li><a href="/robotmodel/robot_model/raw-file/d33189ce4a62/collada_urdf/src/collada_urdf.cpp">raw</a></li>
00072 </ul>
00073 </div>
00074
00075 <div class="main">
00076 <h2><a href="/robotmodel/robot_model/">robotmodel-robot_model</a></h2>
00077 <h3>view collada_urdf/src/collada_urdf.cpp @ 291:d33189ce4a62</h3>
00078
00079 <form class="search" action="/robotmodel/robot_model/log">
00080
00081 <p><input name="rev" id="search1" type="text" size="30" /></p>
00082 <div id="hint">find changesets by author, revision,
00083 files, or words in the commit message</div>
00084 </form>
00085
00086 <div class="description">fix multiple visuals.</div>
00087
00088 <table id="changesetEntry">
00089 <tr>
00090 <th class="author">author</th>
00091 <td class="author">John Hsu <johnhsu@willowgarage.com></td>
00092 </tr>
00093 <tr>
00094 <th class="date">date</th>
00095 <td class="date">Wed Mar 16 20:52:57 2011 -0700 (2011-03-16)</td>
00096 </tr>
00097 <tr>
00098 <th class="author">parents</th>
00099 <td class="author"><a href="/robotmodel/robot_model/file/ba3671034cdb/collada_urdf/src/collada_urdf.cpp">ba3671034cdb</a> </td>
00100 </tr>
00101 <tr>
00102 <th class="author">children</th>
00103 <td class="author"><a href="/robotmodel/robot_model/file/4986ba77eafd/collada_urdf/src/collada_urdf.cpp">4986ba77eafd</a> </td>
00104 </tr>
00105
00106 </table>
00107
00108 <div class="overflow">
00109 <div class="sourcefirst"> line source</div>
00110
00111 <div class="parity0 source"><a href="#l1" id="l1"> 1</a>
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176 </div>
00177 <div class="parity1 source"><a href="#l34" id="l34"> 34</a>
00178 </div>
00179 <div class="parity0 source"><a href="#l35" id="l35"> 35</a>
00180 </div>
00181 <div class="parity1 source"><a href="#l36" id="l36"> 36</a>
00182 </div>
00183 <div class="parity0 source"><a href="#l37" id="l37"> 37</a> #include "collada_urdf/collada_urdf.h"
00184 </div>
00185 <div class="parity1 source"><a href="#l38" id="l38"> 38</a> #include <map>
00186 </div>
00187 <div class="parity0 source"><a href="#l39" id="l39"> 39</a> #include <vector>
00188 </div>
00189 <div class="parity1 source"><a href="#l40" id="l40"> 40</a> #include <list>
00190 </div>
00191 <div class="parity0 source"><a href="#l41" id="l41"> 41</a>
00192 </div>
00193 <div class="parity1 source"><a href="#l42" id="l42"> 42</a> #include <dae.h>
00194 </div>
00195 <div class="parity0 source"><a href="#l43" id="l43"> 43</a> #include <dae/daeDocument.h>
00196 </div>
00197 <div class="parity1 source"><a href="#l44" id="l44"> 44</a> #include <dae/daeErrorHandler.h>
00198 </div>
00199 <div class="parity0 source"><a href="#l45" id="l45"> 45</a> #include <dae/domAny.h>
00200 </div>
00201 <div class="parity1 source"><a href="#l46" id="l46"> 46</a> #include <dom/domCOLLADA.h>
00202 </div>
00203 <div class="parity0 source"><a href="#l47" id="l47"> 47</a> #include <dom/domConstants.h>
00204 </div>
00205 <div class="parity1 source"><a href="#l48" id="l48"> 48</a> #include <dom/domElements.h>
00206 </div>
00207 <div class="parity0 source"><a href="#l49" id="l49"> 49</a> #include <dom/domTriangles.h>
00208 </div>
00209 <div class="parity1 source"><a href="#l50" id="l50"> 50</a> #include <dom/domTypes.h>
00210 </div>
00211 <div class="parity0 source"><a href="#l51" id="l51"> 51</a> #include <resource_retriever/retriever.h>
00212 </div>
00213 <div class="parity1 source"><a href="#l52" id="l52"> 52</a> #include <urdf/model.h>
00214 </div>
00215 <div class="parity0 source"><a href="#l53" id="l53"> 53</a> #include <urdf/pose.h>
00216 </div>
00217 <div class="parity1 source"><a href="#l54" id="l54"> 54</a> #include <angles/angles.h>
00218 </div>
00219 <div class="parity0 source"><a href="#l55" id="l55"> 55</a>
00220 </div>
00221 <div class="parity1 source"><a href="#l56" id="l56"> 56</a> #include <boost/date_time/posix_time/posix_time.hpp>
00222 </div>
00223 <div class="parity0 source"><a href="#l57" id="l57"> 57</a> #include <boost/date_time/posix_time/posix_time_io.hpp>
00224 </div>
00225 <div class="parity1 source"><a href="#l58" id="l58"> 58</a> #include <boost/format.hpp>
00226 </div>
00227 <div class="parity0 source"><a href="#l59" id="l59"> 59</a>
00228 </div>
00229 <div class="parity1 source"><a href="#l60" id="l60"> 60</a> #include <assimp/assimp.hpp>
00230 </div>
00231 <div class="parity0 source"><a href="#l61" id="l61"> 61</a> #include <assimp/aiScene.h>
00232 </div>
00233 <div class="parity1 source"><a href="#l62" id="l62"> 62</a> #include <assimp/aiPostProcess.h>
00234 </div>
00235 <div class="parity0 source"><a href="#l63" id="l63"> 63</a> #include <assimp/IOStream.h>
00236 </div>
00237 <div class="parity1 source"><a href="#l64" id="l64"> 64</a> #include <assimp/IOSystem.h>
00238 </div>
00239 <div class="parity0 source"><a href="#l65" id="l65"> 65</a>
00240 </div>
00241 <div class="parity1 source"><a href="#l66" id="l66"> 66</a> #define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
00242 </div>
00243 <div class="parity0 source"><a href="#l67" id="l67"> 67</a> #define FOREACHC FOREACH
00244 </div>
00245 <div class="parity1 source"><a href="#l68" id="l68"> 68</a>
00246 </div>
00247 <div class="parity0 source"><a href="#l69" id="l69"> 69</a> using namespace std;
00248 </div>
00249 <div class="parity1 source"><a href="#l70" id="l70"> 70</a>
00250 </div>
00251 <div class="parity0 source"><a href="#l71" id="l71"> 71</a> namespace collada_urdf {
00252 </div>
00253 <div class="parity1 source"><a href="#l72" id="l72"> 72</a>
00254 </div>
00255 <div class="parity0 source"><a href="#l73" id="l73"> 73</a>
00256 </div>
00257 <div class="parity1 source"><a href="#l74" id="l74"> 74</a> class ResourceIOStream : public Assimp::IOStream
00258 </div>
00259 <div class="parity0 source"><a href="#l75" id="l75"> 75</a> {
00260 </div>
00261 <div class="parity1 source"><a href="#l76" id="l76"> 76</a> public:
00262 </div>
00263 <div class="parity0 source"><a href="#l77" id="l77"> 77</a> ResourceIOStream(const resource_retriever::MemoryResource& res)
00264 </div>
00265 <div class="parity1 source"><a href="#l78" id="l78"> 78</a> : res_(res)
00266 </div>
00267 <div class="parity0 source"><a href="#l79" id="l79"> 79</a> , pos_(res.data.get())
00268 </div>
00269 <div class="parity1 source"><a href="#l80" id="l80"> 80</a> {}
00270 </div>
00271 <div class="parity0 source"><a href="#l81" id="l81"> 81</a>
00272 </div>
00273 <div class="parity1 source"><a href="#l82" id="l82"> 82</a> ~ResourceIOStream()
00274 </div>
00275 <div class="parity0 source"><a href="#l83" id="l83"> 83</a> {}
00276 </div>
00277 <div class="parity1 source"><a href="#l84" id="l84"> 84</a>
00278 </div>
00279 <div class="parity0 source"><a href="#l85" id="l85"> 85</a> size_t Read(void* buffer, size_t size, size_t count)
00280 </div>
00281 <div class="parity1 source"><a href="#l86" id="l86"> 86</a> {
00282 </div>
00283 <div class="parity0 source"><a href="#l87" id="l87"> 87</a> size_t to_read = size * count;
00284 </div>
00285 <div class="parity1 source"><a href="#l88" id="l88"> 88</a> if (pos_ + to_read > res_.data.get() + res_.size)
00286 </div>
00287 <div class="parity0 source"><a href="#l89" id="l89"> 89</a> {
00288 </div>
00289 <div class="parity1 source"><a href="#l90" id="l90"> 90</a> to_read = res_.size - (pos_ - res_.data.get());
00290 </div>
00291 <div class="parity0 source"><a href="#l91" id="l91"> 91</a> }
00292 </div>
00293 <div class="parity1 source"><a href="#l92" id="l92"> 92</a>
00294 </div>
00295 <div class="parity0 source"><a href="#l93" id="l93"> 93</a> memcpy(buffer, pos_, to_read);
00296 </div>
00297 <div class="parity1 source"><a href="#l94" id="l94"> 94</a> pos_ += to_read;
00298 </div>
00299 <div class="parity0 source"><a href="#l95" id="l95"> 95</a>
00300 </div>
00301 <div class="parity1 source"><a href="#l96" id="l96"> 96</a> return to_read;
00302 </div>
00303 <div class="parity0 source"><a href="#l97" id="l97"> 97</a> }
00304 </div>
00305 <div class="parity1 source"><a href="#l98" id="l98"> 98</a>
00306 </div>
00307 <div class="parity0 source"><a href="#l99" id="l99"> 99</a> size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; }
00308 </div>
00309 <div class="parity1 source"><a href="#l100" id="l100"> 100</a>
00310 </div>
00311 <div class="parity0 source"><a href="#l101" id="l101"> 101</a> aiReturn Seek( size_t offset, aiOrigin origin)
00312 </div>
00313 <div class="parity1 source"><a href="#l102" id="l102"> 102</a> {
00314 </div>
00315 <div class="parity0 source"><a href="#l103" id="l103"> 103</a> uint8_t* new_pos = 0;
00316 </div>
00317 <div class="parity1 source"><a href="#l104" id="l104"> 104</a> switch (origin)
00318 </div>
00319 <div class="parity0 source"><a href="#l105" id="l105"> 105</a> {
00320 </div>
00321 <div class="parity1 source"><a href="#l106" id="l106"> 106</a> case aiOrigin_SET:
00322 </div>
00323 <div class="parity0 source"><a href="#l107" id="l107"> 107</a> new_pos = res_.data.get() + offset;
00324 </div>
00325 <div class="parity1 source"><a href="#l108" id="l108"> 108</a> break;
00326 </div>
00327 <div class="parity0 source"><a href="#l109" id="l109"> 109</a> case aiOrigin_CUR:
00328 </div>
00329 <div class="parity1 source"><a href="#l110" id="l110"> 110</a> new_pos = pos_ + offset;
00330 </div>
00331 <div class="parity0 source"><a href="#l111" id="l111"> 111</a> break;
00332 </div>
00333 <div class="parity1 source"><a href="#l112" id="l112"> 112</a> case aiOrigin_END:
00334 </div>
00335 <div class="parity0 source"><a href="#l113" id="l113"> 113</a> new_pos = res_.data.get() + res_.size - offset;
00336 </div>
00337 <div class="parity1 source"><a href="#l114" id="l114"> 114</a> break;
00338 </div>
00339 <div class="parity0 source"><a href="#l115" id="l115"> 115</a> default:
00340 </div>
00341 <div class="parity1 source"><a href="#l116" id="l116"> 116</a> ROS_BREAK();
00342 </div>
00343 <div class="parity0 source"><a href="#l117" id="l117"> 117</a> }
00344 </div>
00345 <div class="parity1 source"><a href="#l118" id="l118"> 118</a>
00346 </div>
00347 <div class="parity0 source"><a href="#l119" id="l119"> 119</a> if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
00348 </div>
00349 <div class="parity1 source"><a href="#l120" id="l120"> 120</a> {
00350 </div>
00351 <div class="parity0 source"><a href="#l121" id="l121"> 121</a> return aiReturn_FAILURE;
00352 </div>
00353 <div class="parity1 source"><a href="#l122" id="l122"> 122</a> }
00354 </div>
00355 <div class="parity0 source"><a href="#l123" id="l123"> 123</a>
00356 </div>
00357 <div class="parity1 source"><a href="#l124" id="l124"> 124</a> pos_ = new_pos;
00358 </div>
00359 <div class="parity0 source"><a href="#l125" id="l125"> 125</a> return aiReturn_SUCCESS;
00360 </div>
00361 <div class="parity1 source"><a href="#l126" id="l126"> 126</a> }
00362 </div>
00363 <div class="parity0 source"><a href="#l127" id="l127"> 127</a>
00364 </div>
00365 <div class="parity1 source"><a href="#l128" id="l128"> 128</a> size_t Tell() const
00366 </div>
00367 <div class="parity0 source"><a href="#l129" id="l129"> 129</a> {
00368 </div>
00369 <div class="parity1 source"><a href="#l130" id="l130"> 130</a> return pos_ - res_.data.get();
00370 </div>
00371 <div class="parity0 source"><a href="#l131" id="l131"> 131</a> }
00372 </div>
00373 <div class="parity1 source"><a href="#l132" id="l132"> 132</a>
00374 </div>
00375 <div class="parity0 source"><a href="#l133" id="l133"> 133</a> size_t FileSize() const
00376 </div>
00377 <div class="parity1 source"><a href="#l134" id="l134"> 134</a> {
00378 </div>
00379 <div class="parity0 source"><a href="#l135" id="l135"> 135</a> return res_.size;
00380 </div>
00381 <div class="parity1 source"><a href="#l136" id="l136"> 136</a> }
00382 </div>
00383 <div class="parity0 source"><a href="#l137" id="l137"> 137</a>
00384 </div>
00385 <div class="parity1 source"><a href="#l138" id="l138"> 138</a> void Flush() {}
00386 </div>
00387 <div class="parity0 source"><a href="#l139" id="l139"> 139</a>
00388 </div>
00389 <div class="parity1 source"><a href="#l140" id="l140"> 140</a> private:
00390 </div>
00391 <div class="parity0 source"><a href="#l141" id="l141"> 141</a> resource_retriever::MemoryResource res_;
00392 </div>
00393 <div class="parity1 source"><a href="#l142" id="l142"> 142</a> uint8_t* pos_;
00394 </div>
00395 <div class="parity0 source"><a href="#l143" id="l143"> 143</a> };
00396 </div>
00397 <div class="parity1 source"><a href="#l144" id="l144"> 144</a>
00398 </div>
00399 <div class="parity0 source"><a href="#l145" id="l145"> 145</a>
00400 </div>
00401 <div class="parity1 source"><a href="#l146" id="l146"> 146</a> class ResourceIOSystem : public Assimp::IOSystem
00402 </div>
00403 <div class="parity0 source"><a href="#l147" id="l147"> 147</a> {
00404 </div>
00405 <div class="parity1 source"><a href="#l148" id="l148"> 148</a> public:
00406 </div>
00407 <div class="parity0 source"><a href="#l149" id="l149"> 149</a> ResourceIOSystem()
00408 </div>
00409 <div class="parity1 source"><a href="#l150" id="l150"> 150</a> {
00410 </div>
00411 <div class="parity0 source"><a href="#l151" id="l151"> 151</a> }
00412 </div>
00413 <div class="parity1 source"><a href="#l152" id="l152"> 152</a>
00414 </div>
00415 <div class="parity0 source"><a href="#l153" id="l153"> 153</a> ~ResourceIOSystem()
00416 </div>
00417 <div class="parity1 source"><a href="#l154" id="l154"> 154</a> {
00418 </div>
00419 <div class="parity0 source"><a href="#l155" id="l155"> 155</a> }
00420 </div>
00421 <div class="parity1 source"><a href="#l156" id="l156"> 156</a>
00422 </div>
00423 <div class="parity0 source"><a href="#l157" id="l157"> 157</a>
00424 </div>
00425 <div class="parity1 source"><a href="#l158" id="l158"> 158</a> bool Exists(const char* file) const
00426 </div>
00427 <div class="parity0 source"><a href="#l159" id="l159"> 159</a> {
00428 </div>
00429 <div class="parity1 source"><a href="#l160" id="l160"> 160</a>
00430 </div>
00431 <div class="parity0 source"><a href="#l161" id="l161"> 161</a>
00432 </div>
00433 <div class="parity1 source"><a href="#l162" id="l162"> 162</a>
00434 </div>
00435 <div class="parity0 source"><a href="#l163" id="l163"> 163</a> resource_retriever::MemoryResource res;
00436 </div>
00437 <div class="parity1 source"><a href="#l164" id="l164"> 164</a> try {
00438 </div>
00439 <div class="parity0 source"><a href="#l165" id="l165"> 165</a> res = retriever_.get(file);
00440 </div>
00441 <div class="parity1 source"><a href="#l166" id="l166"> 166</a> }
00442 </div>
00443 <div class="parity0 source"><a href="#l167" id="l167"> 167</a> catch (resource_retriever::Exception& e) {
00444 </div>
00445 <div class="parity1 source"><a href="#l168" id="l168"> 168</a> return false;
00446 </div>
00447 <div class="parity0 source"><a href="#l169" id="l169"> 169</a> }
00448 </div>
00449 <div class="parity1 source"><a href="#l170" id="l170"> 170</a>
00450 </div>
00451 <div class="parity0 source"><a href="#l171" id="l171"> 171</a> return true;
00452 </div>
00453 <div class="parity1 source"><a href="#l172" id="l172"> 172</a> }
00454 </div>
00455 <div class="parity0 source"><a href="#l173" id="l173"> 173</a>
00456 </div>
00457 <div class="parity1 source"><a href="#l174" id="l174"> 174</a>
00458 </div>
00459 <div class="parity0 source"><a href="#l175" id="l175"> 175</a> char getOsSeparator() const
00460 </div>
00461 <div class="parity1 source"><a href="#l176" id="l176"> 176</a> {
00462 </div>
00463 <div class="parity0 source"><a href="#l177" id="l177"> 177</a> return '/';
00464 </div>
00465 <div class="parity1 source"><a href="#l178" id="l178"> 178</a> }
00466 </div>
00467 <div class="parity0 source"><a href="#l179" id="l179"> 179</a>
00468 </div>
00469 <div class="parity1 source"><a href="#l180" id="l180"> 180</a>
00470 </div>
00471 <div class="parity0 source"><a href="#l181" id="l181"> 181</a> Assimp::IOStream* Open(const char* file, const char* mode)
00472 </div>
00473 <div class="parity1 source"><a href="#l182" id="l182"> 182</a> {
00474 </div>
00475 <div class="parity0 source"><a href="#l183" id="l183"> 183</a> ROS_ASSERT(mode == std::string("r") || mode == std::string("rb"));
00476 </div>
00477 <div class="parity1 source"><a href="#l184" id="l184"> 184</a>
00478 </div>
00479 <div class="parity0 source"><a href="#l185" id="l185"> 185</a>
00480 </div>
00481 <div class="parity1 source"><a href="#l186" id="l186"> 186</a>
00482 </div>
00483 <div class="parity0 source"><a href="#l187" id="l187"> 187</a> resource_retriever::MemoryResource res;
00484 </div>
00485 <div class="parity1 source"><a href="#l188" id="l188"> 188</a> try {
00486 </div>
00487 <div class="parity0 source"><a href="#l189" id="l189"> 189</a> res = retriever_.get(file);
00488 </div>
00489 <div class="parity1 source"><a href="#l190" id="l190"> 190</a> }
00490 </div>
00491 <div class="parity0 source"><a href="#l191" id="l191"> 191</a> catch (resource_retriever::Exception& e) {
00492 </div>
00493 <div class="parity1 source"><a href="#l192" id="l192"> 192</a> return 0;
00494 </div>
00495 <div class="parity0 source"><a href="#l193" id="l193"> 193</a> }
00496 </div>
00497 <div class="parity1 source"><a href="#l194" id="l194"> 194</a>
00498 </div>
00499 <div class="parity0 source"><a href="#l195" id="l195"> 195</a> return new ResourceIOStream(res);
00500 </div>
00501 <div class="parity1 source"><a href="#l196" id="l196"> 196</a> }
00502 </div>
00503 <div class="parity0 source"><a href="#l197" id="l197"> 197</a>
00504 </div>
00505 <div class="parity1 source"><a href="#l198" id="l198"> 198</a> void Close(Assimp::IOStream* stream) { delete stream; }
00506 </div>
00507 <div class="parity0 source"><a href="#l199" id="l199"> 199</a>
00508 </div>
00509 <div class="parity1 source"><a href="#l200" id="l200"> 200</a> private:
00510 </div>
00511 <div class="parity0 source"><a href="#l201" id="l201"> 201</a> mutable resource_retriever::Retriever retriever_;
00512 </div>
00513 <div class="parity1 source"><a href="#l202" id="l202"> 202</a> };
00514 </div>
00515 <div class="parity0 source"><a href="#l203" id="l203"> 203</a>
00516 </div>
00517 <div class="parity1 source"><a href="#l204" id="l204"> 204</a>
00520 </div>
00521 <div class="parity1 source"><a href="#l206" id="l206"> 206</a> class ColladaWriter : public daeErrorHandler
00522 </div>
00523 <div class="parity0 source"><a href="#l207" id="l207"> 207</a> {
00524 </div>
00525 <div class="parity1 source"><a href="#l208" id="l208"> 208</a> private:
00526 </div>
00527 <div class="parity0 source"><a href="#l209" id="l209"> 209</a> struct SCENE
00528 </div>
00529 <div class="parity1 source"><a href="#l210" id="l210"> 210</a> {
00530 </div>
00531 <div class="parity0 source"><a href="#l211" id="l211"> 211</a> domVisual_sceneRef vscene;
00532 </div>
00533 <div class="parity1 source"><a href="#l212" id="l212"> 212</a> domKinematics_sceneRef kscene;
00534 </div>
00535 <div class="parity0 source"><a href="#l213" id="l213"> 213</a> domPhysics_sceneRef pscene;
00536 </div>
00537 <div class="parity1 source"><a href="#l214" id="l214"> 214</a> domInstance_with_extraRef viscene;
00538 </div>
00539 <div class="parity0 source"><a href="#l215" id="l215"> 215</a> domInstance_kinematics_sceneRef kiscene;
00540 </div>
00541 <div class="parity1 source"><a href="#l216" id="l216"> 216</a> domInstance_with_extraRef piscene;
00542 </div>
00543 <div class="parity0 source"><a href="#l217" id="l217"> 217</a> };
00544 </div>
00545 <div class="parity1 source"><a href="#l218" id="l218"> 218</a>
00546 </div>
00547 <div class="parity0 source"><a href="#l219" id="l219"> 219</a> struct LINKOUTPUT
00548 </div>
00549 <div class="parity1 source"><a href="#l220" id="l220"> 220</a> {
00550 </div>
00551 <div class="parity0 source"><a href="#l221" id="l221"> 221</a> list<pair<int,string> > listusedlinks;
00552 </div>
00553 <div class="parity1 source"><a href="#l222" id="l222"> 222</a> list<pair<int,string> > listprocesseddofs;
00554 </div>
00555 <div class="parity0 source"><a href="#l223" id="l223"> 223</a> daeElementRef plink;
00556 </div>
00557 <div class="parity1 source"><a href="#l224" id="l224"> 224</a> domNodeRef pnode;
00558 </div>
00559 <div class="parity0 source"><a href="#l225" id="l225"> 225</a> };
00560 </div>
00561 <div class="parity1 source"><a href="#l226" id="l226"> 226</a>
00562 </div>
00563 <div class="parity0 source"><a href="#l227" id="l227"> 227</a> struct kinematics_model_output
00564 </div>
00565 <div class="parity1 source"><a href="#l228" id="l228"> 228</a> {
00566 </div>
00567 <div class="parity0 source"><a href="#l229" id="l229"> 229</a> struct axis_output
00568 </div>
00569 <div class="parity1 source"><a href="#l230" id="l230"> 230</a> {
00570 </div>
00571 <div class="parity0 source"><a href="#l231" id="l231"> 231</a>
00572 </div>
00573 <div class="parity1 source"><a href="#l232" id="l232"> 232</a> axis_output() : iaxis(0) {}
00574 </div>
00575 <div class="parity0 source"><a href="#l233" id="l233"> 233</a> string sid, nodesid;
00576 </div>
00577 <div class="parity1 source"><a href="#l234" id="l234"> 234</a> boost::shared_ptr<const urdf::Joint> pjoint;
00578 </div>
00579 <div class="parity0 source"><a href="#l235" id="l235"> 235</a> int iaxis;
00580 </div>
00581 <div class="parity1 source"><a href="#l236" id="l236"> 236</a> string jointnodesid;
00582 </div>
00583 <div class="parity0 source"><a href="#l237" id="l237"> 237</a> };
00584 </div>
00585 <div class="parity1 source"><a href="#l238" id="l238"> 238</a> domKinematics_modelRef kmodel;
00586 </div>
00587 <div class="parity0 source"><a href="#l239" id="l239"> 239</a> std::vector<axis_output> vaxissids;
00588 </div>
00589 <div class="parity1 source"><a href="#l240" id="l240"> 240</a> std::vector<std::string > vlinksids;
00590 </div>
00591 <div class="parity0 source"><a href="#l241" id="l241"> 241</a> };
00592 </div>
00593 <div class="parity1 source"><a href="#l242" id="l242"> 242</a>
00594 </div>
00595 <div class="parity0 source"><a href="#l243" id="l243"> 243</a> struct axis_sids
00596 </div>
00597 <div class="parity1 source"><a href="#l244" id="l244"> 244</a> {
00598 </div>
00599 <div class="parity0 source"><a href="#l245" id="l245"> 245</a> axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {}
00600 </div>
00601 <div class="parity1 source"><a href="#l246" id="l246"> 246</a> string axissid, valuesid, jointnodesid;
00602 </div>
00603 <div class="parity0 source"><a href="#l247" id="l247"> 247</a> };
00604 </div>
00605 <div class="parity1 source"><a href="#l248" id="l248"> 248</a>
00606 </div>
00607 <div class="parity0 source"><a href="#l249" id="l249"> 249</a> struct instance_kinematics_model_output
00608 </div>
00609 <div class="parity1 source"><a href="#l250" id="l250"> 250</a> {
00610 </div>
00611 <div class="parity0 source"><a href="#l251" id="l251"> 251</a> domInstance_kinematics_modelRef ikm;
00612 </div>
00613 <div class="parity1 source"><a href="#l252" id="l252"> 252</a> std::vector<axis_sids> vaxissids;
00614 </div>
00615 <div class="parity0 source"><a href="#l253" id="l253"> 253</a> boost::shared_ptr<kinematics_model_output> kmout;
00616 </div>
00617 <div class="parity1 source"><a href="#l254" id="l254"> 254</a> std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
00618 </div>
00619 <div class="parity0 source"><a href="#l255" id="l255"> 255</a> };
00620 </div>
00621 <div class="parity1 source"><a href="#l256" id="l256"> 256</a>
00622 </div>
00623 <div class="parity0 source"><a href="#l257" id="l257"> 257</a> struct instance_articulated_system_output
00624 </div>
00625 <div class="parity1 source"><a href="#l258" id="l258"> 258</a> {
00626 </div>
00627 <div class="parity0 source"><a href="#l259" id="l259"> 259</a> domInstance_articulated_systemRef ias;
00628 </div>
00629 <div class="parity1 source"><a href="#l260" id="l260"> 260</a> std::vector<axis_sids> vaxissids;
00630 </div>
00631 <div class="parity0 source"><a href="#l261" id="l261"> 261</a> std::vector<std::string > vlinksids;
00632 </div>
00633 <div class="parity1 source"><a href="#l262" id="l262"> 262</a> std::vector<std::pair<std::string,std::string> > vkinematicsbindings;
00634 </div>
00635 <div class="parity0 source"><a href="#l263" id="l263"> 263</a> };
00636 </div>
00637 <div class="parity1 source"><a href="#l264" id="l264"> 264</a>
00638 </div>
00639 <div class="parity0 source"><a href="#l265" id="l265"> 265</a> public:
00640 </div>
00641 <div class="parity1 source"><a href="#l266" id="l266"> 266</a> ColladaWriter(const urdf::Model& robot, int writeoptions) : _writeoptions(writeoptions), _robot(robot), _dom(NULL) {
00642 </div>
00643 <div class="parity0 source"><a href="#l267" id="l267"> 267</a> daeErrorHandler::setErrorHandler(this);
00644 </div>
00645 <div class="parity1 source"><a href="#l268" id="l268"> 268</a> _collada.reset(new DAE);
00646 </div>
00647 <div class="parity0 source"><a href="#l269" id="l269"> 269</a> _collada->setIOPlugin(NULL);
00648 </div>
00649 <div class="parity1 source"><a href="#l270" id="l270"> 270</a> _collada->setDatabase(NULL);
00650 </div>
00651 <div class="parity0 source"><a href="#l271" id="l271"> 271</a> _importer.SetIOHandler(new ResourceIOSystem());
00652 </div>
00653 <div class="parity1 source"><a href="#l272" id="l272"> 272</a> }
00654 </div>
00655 <div class="parity0 source"><a href="#l273" id="l273"> 273</a> virtual ~ColladaWriter() {}
00656 </div>
00657 <div class="parity1 source"><a href="#l274" id="l274"> 274</a>
00658 </div>
00659 <div class="parity0 source"><a href="#l275" id="l275"> 275</a> boost::shared_ptr<DAE> convert()
00660 </div>
00661 <div class="parity1 source"><a href="#l276" id="l276"> 276</a> {
00662 </div>
00663 <div class="parity0 source"><a href="#l277" id="l277"> 277</a> try {
00664 </div>
00665 <div class="parity1 source"><a href="#l278" id="l278"> 278</a> const char* documentName = "urdf_snapshot";
00666 </div>
00667 <div class="parity0 source"><a href="#l279" id="l279"> 279</a> daeDocument *doc = NULL;
00668 </div>
00669 <div class="parity1 source"><a href="#l280" id="l280"> 280</a> daeInt error = _collada->getDatabase()->insertDocument(documentName, &doc );
00670 </div>
00671 <div class="parity0 source"><a href="#l281" id="l281"> 281</a> if (error != DAE_OK || doc == NULL) {
00672 </div>
00673 <div class="parity1 source"><a href="#l282" id="l282"> 282</a> throw ColladaUrdfException("Failed to create document");
00674 </div>
00675 <div class="parity0 source"><a href="#l283" id="l283"> 283</a> }
00676 </div>
00677 <div class="parity1 source"><a href="#l284" id="l284"> 284</a> _dom = daeSafeCast<domCOLLADA>(doc->getDomRoot());
00678 </div>
00679 <div class="parity0 source"><a href="#l285" id="l285"> 285</a> _dom->setAttribute("xmlns:math","http:
00680 </div>
00681 <div class="parity1 source"><a href="#l286" id="l286"> 286</a>
00682 </div>
00683 <div class="parity0 source"><a href="#l287" id="l287"> 287</a>
00684 </div>
00685 <div class="parity1 source"><a href="#l288" id="l288"> 288</a> domAssetRef asset = daeSafeCast<domAsset>( _dom->add( COLLADA_ELEMENT_ASSET ) );
00686 </div>
00687 <div class="parity0 source"><a href="#l289" id="l289"> 289</a> {
00688 </div>
00689 <div class="parity1 source"><a href="#l290" id="l290"> 290</a>
00690 </div>
00691 <div class="parity0 source"><a href="#l291" id="l291"> 291</a> boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s");
00692 </div>
00693 <div class="parity1 source"><a href="#l292" id="l292"> 292</a> std::stringstream ss;
00694 </div>
00695 <div class="parity0 source"><a href="#l293" id="l293"> 293</a> ss.imbue(std::locale(ss.getloc(), facet));
00696 </div>
00697 <div class="parity1 source"><a href="#l294" id="l294"> 294</a> ss << boost::posix_time::second_clock::local_time();
00698 </div>
00699 <div class="parity0 source"><a href="#l295" id="l295"> 295</a>
00700 </div>
00701 <div class="parity1 source"><a href="#l296" id="l296"> 296</a> domAsset::domCreatedRef created = daeSafeCast<domAsset::domCreated>( asset->add( COLLADA_ELEMENT_CREATED ) );
00702 </div>
00703 <div class="parity0 source"><a href="#l297" id="l297"> 297</a> created->setValue(ss.str().c_str());
00704 </div>
00705 <div class="parity1 source"><a href="#l298" id="l298"> 298</a> domAsset::domModifiedRef modified = daeSafeCast<domAsset::domModified>( asset->add( COLLADA_ELEMENT_MODIFIED ) );
00706 </div>
00707 <div class="parity0 source"><a href="#l299" id="l299"> 299</a> modified->setValue(ss.str().c_str());
00708 </div>
00709 <div class="parity1 source"><a href="#l300" id="l300"> 300</a>
00710 </div>
00711 <div class="parity0 source"><a href="#l301" id="l301"> 301</a> domAsset::domContributorRef contrib = daeSafeCast<domAsset::domContributor>( asset->add( COLLADA_TYPE_CONTRIBUTOR ) );
00712 </div>
00713 <div class="parity1 source"><a href="#l302" id="l302"> 302</a> domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast<domAsset::domContributor::domAuthoring_tool>( contrib->add( COLLADA_ELEMENT_AUTHORING_TOOL ) );
00714 </div>
00715 <div class="parity0 source"><a href="#l303" id="l303"> 303</a> authoringtool->setValue("URDF Collada Writer");
00716 </div>
00717 <div class="parity1 source"><a href="#l304" id="l304"> 304</a>
00718 </div>
00719 <div class="parity0 source"><a href="#l305" id="l305"> 305</a> domAsset::domUnitRef units = daeSafeCast<domAsset::domUnit>( asset->add( COLLADA_ELEMENT_UNIT ) );
00720 </div>
00721 <div class="parity1 source"><a href="#l306" id="l306"> 306</a> units->setMeter(1);
00722 </div>
00723 <div class="parity0 source"><a href="#l307" id="l307"> 307</a> units->setName("meter");
00724 </div>
00725 <div class="parity1 source"><a href="#l308" id="l308"> 308</a>
00726 </div>
00727 <div class="parity0 source"><a href="#l309" id="l309"> 309</a> domAsset::domUp_axisRef zup = daeSafeCast<domAsset::domUp_axis>( asset->add( COLLADA_ELEMENT_UP_AXIS ) );
00728 </div>
00729 <div class="parity1 source"><a href="#l310" id="l310"> 310</a> zup->setValue(UP_AXIS_Z_UP);
00730 </div>
00731 <div class="parity0 source"><a href="#l311" id="l311"> 311</a> }
00732 </div>
00733 <div class="parity1 source"><a href="#l312" id="l312"> 312</a>
00734 </div>
00735 <div class="parity0 source"><a href="#l313" id="l313"> 313</a> _globalscene = _dom->getScene();
00736 </div>
00737 <div class="parity1 source"><a href="#l314" id="l314"> 314</a> if( !_globalscene ) {
00738 </div>
00739 <div class="parity0 source"><a href="#l315" id="l315"> 315</a> _globalscene = daeSafeCast<domCOLLADA::domScene>( _dom->add( COLLADA_ELEMENT_SCENE ) );
00740 </div>
00741 <div class="parity1 source"><a href="#l316" id="l316"> 316</a> }
00742 </div>
00743 <div class="parity0 source"><a href="#l317" id="l317"> 317</a>
00744 </div>
00745 <div class="parity1 source"><a href="#l318" id="l318"> 318</a> _visualScenesLib = daeSafeCast<domLibrary_visual_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
00746 </div>
00747 <div class="parity0 source"><a href="#l319" id="l319"> 319</a> _visualScenesLib->setId("vscenes");
00748 </div>
00749 <div class="parity1 source"><a href="#l320" id="l320"> 320</a> _geometriesLib = daeSafeCast<domLibrary_geometries>(_dom->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
00750 </div>
00751 <div class="parity0 source"><a href="#l321" id="l321"> 321</a> _geometriesLib->setId("geometries");
00752 </div>
00753 <div class="parity1 source"><a href="#l322" id="l322"> 322</a> _effectsLib = daeSafeCast<domLibrary_effects>(_dom->add(COLLADA_ELEMENT_LIBRARY_EFFECTS));
00754 </div>
00755 <div class="parity0 source"><a href="#l323" id="l323"> 323</a> _effectsLib->setId("effects");
00756 </div>
00757 <div class="parity1 source"><a href="#l324" id="l324"> 324</a> _materialsLib = daeSafeCast<domLibrary_materials>(_dom->add(COLLADA_ELEMENT_LIBRARY_MATERIALS));
00758 </div>
00759 <div class="parity0 source"><a href="#l325" id="l325"> 325</a> _materialsLib->setId("materials");
00760 </div>
00761 <div class="parity1 source"><a href="#l326" id="l326"> 326</a> _kinematicsModelsLib = daeSafeCast<domLibrary_kinematics_models>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
00762 </div>
00763 <div class="parity0 source"><a href="#l327" id="l327"> 327</a> _kinematicsModelsLib->setId("kmodels");
00764 </div>
00765 <div class="parity1 source"><a href="#l328" id="l328"> 328</a> _articulatedSystemsLib = daeSafeCast<domLibrary_articulated_systems>(_dom->add(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS));
00766 </div>
00767 <div class="parity0 source"><a href="#l329" id="l329"> 329</a> _articulatedSystemsLib->setId("asystems");
00768 </div>
00769 <div class="parity1 source"><a href="#l330" id="l330"> 330</a> _kinematicsScenesLib = daeSafeCast<domLibrary_kinematics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
00770 </div>
00771 <div class="parity0 source"><a href="#l331" id="l331"> 331</a> _kinematicsScenesLib->setId("kscenes");
00772 </div>
00773 <div class="parity1 source"><a href="#l332" id="l332"> 332</a> _physicsScenesLib = daeSafeCast<domLibrary_physics_scenes>(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
00774 </div>
00775 <div class="parity0 source"><a href="#l333" id="l333"> 333</a> _physicsScenesLib->setId("pscenes");
00776 </div>
00777 <div class="parity1 source"><a href="#l334" id="l334"> 334</a> domExtraRef pextra_library_sensors = daeSafeCast<domExtra>(_dom->add(COLLADA_ELEMENT_EXTRA));
00778 </div>
00779 <div class="parity0 source"><a href="#l335" id="l335"> 335</a> pextra_library_sensors->setId("sensors");
00780 </div>
00781 <div class="parity1 source"><a href="#l336" id="l336"> 336</a> pextra_library_sensors->setType("library_sensors");
00782 </div>
00783 <div class="parity0 source"><a href="#l337" id="l337"> 337</a> _sensorsLib = daeSafeCast<domTechnique>(pextra_library_sensors->add(COLLADA_ELEMENT_TECHNIQUE));
00784 </div>
00785 <div class="parity1 source"><a href="#l338" id="l338"> 338</a> _sensorsLib->setProfile("OpenRAVE");
00786 </div>
00787 <div class="parity0 source"><a href="#l339" id="l339"> 339</a>
00788 </div>
00789 <div class="parity1 source"><a href="#l340" id="l340"> 340</a> _CreateScene();
00790 </div>
00791 <div class="parity0 source"><a href="#l341" id="l341"> 341</a> _WritePhysics();
00792 </div>
00793 <div class="parity1 source"><a href="#l342" id="l342"> 342</a> _WriteRobot();
00794 </div>
00795 <div class="parity0 source"><a href="#l343" id="l343"> 343</a> _WriteBindingsInstance_kinematics_scene();
00796 </div>
00797 <div class="parity1 source"><a href="#l344" id="l344"> 344</a> return _collada;
00798 </div>
00799 <div class="parity0 source"><a href="#l345" id="l345"> 345</a> }
00800 </div>
00801 <div class="parity1 source"><a href="#l346" id="l346"> 346</a> catch (ColladaUrdfException ex) {
00802 </div>
00803 <div class="parity0 source"><a href="#l347" id="l347"> 347</a> ROS_ERROR("Error converting: %s", ex.what());
00804 </div>
00805 <div class="parity1 source"><a href="#l348" id="l348"> 348</a> return boost::shared_ptr<DAE>();
00806 </div>
00807 <div class="parity0 source"><a href="#l349" id="l349"> 349</a> }
00808 </div>
00809 <div class="parity1 source"><a href="#l350" id="l350"> 350</a> }
00810 </div>
00811 <div class="parity0 source"><a href="#l351" id="l351"> 351</a>
00812 </div>
00813 <div class="parity1 source"><a href="#l352" id="l352"> 352</a> protected:
00814 </div>
00815 <div class="parity0 source"><a href="#l353" id="l353"> 353</a> virtual void handleError(daeString msg) { throw ColladaUrdfException(msg); }
00816 </div>
00817 <div class="parity1 source"><a href="#l354" id="l354"> 354</a> virtual void handleWarning(daeString msg) { std::cerr << "COLLADA DOM warning: " << msg << std::endl; }
00818 </div>
00819 <div class="parity0 source"><a href="#l355" id="l355"> 355</a>
00820 </div>
00821 <div class="parity1 source"><a href="#l356" id="l356"> 356</a> void _CreateScene()
00822 </div>
00823 <div class="parity0 source"><a href="#l357" id="l357"> 357</a> {
00824 </div>
00825 <div class="parity1 source"><a href="#l358" id="l358"> 358</a>
00826 </div>
00827 <div class="parity0 source"><a href="#l359" id="l359"> 359</a> _scene.vscene = daeSafeCast<domVisual_scene>(_visualScenesLib->add(COLLADA_ELEMENT_VISUAL_SCENE));
00828 </div>
00829 <div class="parity1 source"><a href="#l360" id="l360"> 360</a> _scene.vscene->setId("vscene");
00830 </div>
00831 <div class="parity0 source"><a href="#l361" id="l361"> 361</a> _scene.vscene->setName("URDF Visual Scene");
00832 </div>
00833 <div class="parity1 source"><a href="#l362" id="l362"> 362</a>
00834 </div>
00835 <div class="parity0 source"><a href="#l363" id="l363"> 363</a>
00836 </div>
00837 <div class="parity1 source"><a href="#l364" id="l364"> 364</a> _scene.kscene = daeSafeCast<domKinematics_scene>(_kinematicsScenesLib->add(COLLADA_ELEMENT_KINEMATICS_SCENE));
00838 </div>
00839 <div class="parity0 source"><a href="#l365" id="l365"> 365</a> _scene.kscene->setId("kscene");
00840 </div>
00841 <div class="parity1 source"><a href="#l366" id="l366"> 366</a> _scene.kscene->setName("URDF Kinematics Scene");
00842 </div>
00843 <div class="parity0 source"><a href="#l367" id="l367"> 367</a>
00844 </div>
00845 <div class="parity1 source"><a href="#l368" id="l368"> 368</a>
00846 </div>
00847 <div class="parity0 source"><a href="#l369" id="l369"> 369</a> _scene.pscene = daeSafeCast<domPhysics_scene>(_physicsScenesLib->add(COLLADA_ELEMENT_PHYSICS_SCENE));
00848 </div>
00849 <div class="parity1 source"><a href="#l370" id="l370"> 370</a> _scene.pscene->setId("pscene");
00850 </div>
00851 <div class="parity0 source"><a href="#l371" id="l371"> 371</a> _scene.pscene->setName("URDF Physics Scene");
00852 </div>
00853 <div class="parity1 source"><a href="#l372" id="l372"> 372</a>
00854 </div>
00855 <div class="parity0 source"><a href="#l373" id="l373"> 373</a>
00856 </div>
00857 <div class="parity1 source"><a href="#l374" id="l374"> 374</a> _scene.viscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE ));
00858 </div>
00859 <div class="parity0 source"><a href="#l375" id="l375"> 375</a> _scene.viscene->setUrl( (string("#") + string(_scene.vscene->getID())).c_str() );
00860 </div>
00861 <div class="parity1 source"><a href="#l376" id="l376"> 376</a>
00862 </div>
00863 <div class="parity0 source"><a href="#l377" id="l377"> 377</a>
00864 </div>
00865 <div class="parity1 source"><a href="#l378" id="l378"> 378</a> _scene.kiscene = daeSafeCast<domInstance_kinematics_scene>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE ));
00866 </div>
00867 <div class="parity0 source"><a href="#l379" id="l379"> 379</a> _scene.kiscene->setUrl( (string("#") + string(_scene.kscene->getID())).c_str() );
00868 </div>
00869 <div class="parity1 source"><a href="#l380" id="l380"> 380</a>
00870 </div>
00871 <div class="parity0 source"><a href="#l381" id="l381"> 381</a>
00872 </div>
00873 <div class="parity1 source"><a href="#l382" id="l382"> 382</a> _scene.piscene = daeSafeCast<domInstance_with_extra>(_globalscene->add( COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE ));
00874 </div>
00875 <div class="parity0 source"><a href="#l383" id="l383"> 383</a> _scene.piscene->setUrl( (string("#") + string(_scene.pscene->getID())).c_str() );
00876 </div>
00877 <div class="parity1 source"><a href="#l384" id="l384"> 384</a> }
00878 </div>
00879 <div class="parity0 source"><a href="#l385" id="l385"> 385</a>
00880 </div>
00881 <div class="parity1 source"><a href="#l386" id="l386"> 386</a> void _WritePhysics() {
00882 </div>
00883 <div class="parity0 source"><a href="#l387" id="l387"> 387</a> domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(_scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00884 </div>
00885 <div class="parity1 source"><a href="#l388" id="l388"> 388</a>
00886 </div>
00887 <div class="parity0 source"><a href="#l389" id="l389"> 389</a> domTargetable_float3Ref g = daeSafeCast<domTargetable_float3>(common->add(COLLADA_ELEMENT_GRAVITY));
00888 </div>
00889 <div class="parity1 source"><a href="#l390" id="l390"> 390</a> g->getValue().set3 (0,0,0);
00890 </div>
00891 <div class="parity0 source"><a href="#l391" id="l391"> 391</a> }
00892 </div>
00893 <div class="parity1 source"><a href="#l392" id="l392"> 392</a>
00894 </div>
00895 <div class="parity0 source"><a href="#l393" id="l393"> 393</a>
00896 </div>
00897 <div class="parity1 source"><a href="#l394" id="l394"> 394</a> void _WriteRobot(int id = 0)
00898 </div>
00899 <div class="parity0 source"><a href="#l395" id="l395"> 395</a> {
00900 </div>
00901 <div class="parity1 source"><a href="#l396" id="l396"> 396</a> ROS_DEBUG_STREAM(str(boost::format("writing robot as instance_articulated_system (%d) %s\n")%id%_robot.getName()));
00902 </div>
00903 <div class="parity0 source"><a href="#l397" id="l397"> 397</a> string asid = _ComputeId(str(boost::format("robot%d")%id));
00904 </div>
00905 <div class="parity1 source"><a href="#l398" id="l398"> 398</a> string askid = _ComputeId(str(boost::format("%s_kinematics")%asid));
00906 </div>
00907 <div class="parity0 source"><a href="#l399" id="l399"> 399</a> string asmid = _ComputeId(str(boost::format("%s_motion")%asid));
00908 </div>
00909 <div class="parity1 source"><a href="#l400" id="l400"> 400</a> string iassid = _ComputeId(str(boost::format("%s_inst")%asmid));
00910 </div>
00911 <div class="parity0 source"><a href="#l401" id="l401"> 401</a>
00912 </div>
00913 <div class="parity1 source"><a href="#l402" id="l402"> 402</a> domInstance_articulated_systemRef ias = daeSafeCast<domInstance_articulated_system>(_scene.kscene->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
00914 </div>
00915 <div class="parity0 source"><a href="#l403" id="l403"> 403</a> ias->setSid(iassid.c_str());
00916 </div>
00917 <div class="parity1 source"><a href="#l404" id="l404"> 404</a> ias->setUrl((string("#")+asmid).c_str());
00918 </div>
00919 <div class="parity0 source"><a href="#l405" id="l405"> 405</a> ias->setName(_robot.getName().c_str());
00920 </div>
00921 <div class="parity1 source"><a href="#l406" id="l406"> 406</a>
00922 </div>
00923 <div class="parity0 source"><a href="#l407" id="l407"> 407</a> _iasout.reset(new instance_articulated_system_output());
00924 </div>
00925 <div class="parity1 source"><a href="#l408" id="l408"> 408</a> _iasout->ias = ias;
00926 </div>
00927 <div class="parity0 source"><a href="#l409" id="l409"> 409</a>
00928 </div>
00929 <div class="parity1 source"><a href="#l410" id="l410"> 410</a>
00930 </div>
00931 <div class="parity0 source"><a href="#l411" id="l411"> 411</a> domArticulated_systemRef articulated_system_motion = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
00932 </div>
00933 <div class="parity1 source"><a href="#l412" id="l412"> 412</a> articulated_system_motion->setId(asmid.c_str());
00934 </div>
00935 <div class="parity0 source"><a href="#l413" id="l413"> 413</a> domMotionRef motion = daeSafeCast<domMotion>(articulated_system_motion->add(COLLADA_ELEMENT_MOTION));
00936 </div>
00937 <div class="parity1 source"><a href="#l414" id="l414"> 414</a> domMotion_techniqueRef mt = daeSafeCast<domMotion_technique>(motion->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00938 </div>
00939 <div class="parity0 source"><a href="#l415" id="l415"> 415</a> domInstance_articulated_systemRef ias_motion = daeSafeCast<domInstance_articulated_system>(motion->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM));
00940 </div>
00941 <div class="parity1 source"><a href="#l416" id="l416"> 416</a> ias_motion->setUrl(str(boost::format("#%s")%askid).c_str());
00942 </div>
00943 <div class="parity0 source"><a href="#l417" id="l417"> 417</a>
00944 </div>
00945 <div class="parity1 source"><a href="#l418" id="l418"> 418</a>
00946 </div>
00947 <div class="parity0 source"><a href="#l419" id="l419"> 419</a> domArticulated_systemRef articulated_system_kinematics = daeSafeCast<domArticulated_system>(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM));
00948 </div>
00949 <div class="parity1 source"><a href="#l420" id="l420"> 420</a> articulated_system_kinematics->setId(askid.c_str());
00950 </div>
00951 <div class="parity0 source"><a href="#l421" id="l421"> 421</a> domKinematicsRef kinematics = daeSafeCast<domKinematics>(articulated_system_kinematics->add(COLLADA_ELEMENT_KINEMATICS));
00952 </div>
00953 <div class="parity1 source"><a href="#l422" id="l422"> 422</a> domKinematics_techniqueRef kt = daeSafeCast<domKinematics_technique>(kinematics->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
00954 </div>
00955 <div class="parity0 source"><a href="#l423" id="l423"> 423</a>
00956 </div>
00957 <div class="parity1 source"><a href="#l424" id="l424"> 424</a> _WriteInstance_kinematics_model(kinematics,askid,id);
00958 </div>
00959 <div class="parity0 source"><a href="#l425" id="l425"> 425</a>
00960 </div>
00961 <div class="parity1 source"><a href="#l426" id="l426"> 426</a> for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
00962 </div>
00963 <div class="parity0 source"><a href="#l427" id="l427"> 427</a> string axis_infosid = _ComputeId(str(boost::format("axis_info_inst%d")%idof));
00964 </div>
00965 <div class="parity1 source"><a href="#l428" id="l428"> 428</a> boost::shared_ptr<const urdf::Joint> pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint;
00966 </div>
00967 <div class="parity0 source"><a href="#l429" id="l429"> 429</a> BOOST_ASSERT(_mapjointindices[pjoint] == (int)idof);
00968 </div>
00969 <div class="parity1 source"><a href="#l430" id="l430"> 430</a>
00970 </div>
00971 <div class="parity0 source"><a href="#l431" id="l431"> 431</a>
00972 </div>
00973 <div class="parity1 source"><a href="#l432" id="l432"> 432</a>
00974 </div>
00975 <div class="parity0 source"><a href="#l433" id="l433"> 433</a> domKinematics_axis_infoRef kai = daeSafeCast<domKinematics_axis_info>(kt->add(COLLADA_ELEMENT_AXIS_INFO));
00976 </div>
00977 <div class="parity1 source"><a href="#l434" id="l434"> 434</a> kai->setAxis(str(boost::format("%s/%s")%_ikmout->kmout->kmodel->getID()%_ikmout->kmout->vaxissids.at(idof).sid).c_str());
00978 </div>
00979 <div class="parity0 source"><a href="#l435" id="l435"> 435</a> kai->setSid(axis_infosid.c_str());
00980 </div>
00981 <div class="parity1 source"><a href="#l436" id="l436"> 436</a> bool bactive = !pjoint->mimic;
00982 </div>
00983 <div class="parity0 source"><a href="#l437" id="l437"> 437</a> double flower=0, fupper=0;
00984 </div>
00985 <div class="parity1 source"><a href="#l438" id="l438"> 438</a> if( pjoint->type != urdf::Joint::CONTINUOUS ) {
00986 </div>
00987 <div class="parity0 source"><a href="#l439" id="l439"> 439</a> if( !!pjoint->limits ) {
00988 </div>
00989 <div class="parity1 source"><a href="#l440" id="l440"> 440</a> flower = pjoint->limits->lower;
00990 </div>
00991 <div class="parity0 source"><a href="#l441" id="l441"> 441</a> fupper = pjoint->limits->upper;
00992 </div>
00993 <div class="parity1 source"><a href="#l442" id="l442"> 442</a> }
00994 </div>
00995 <div class="parity0 source"><a href="#l443" id="l443"> 443</a> if( flower == fupper ) {
00996 </div>
00997 <div class="parity1 source"><a href="#l444" id="l444"> 444</a> bactive = false;
00998 </div>
00999 <div class="parity0 source"><a href="#l445" id="l445"> 445</a> }
01000 </div>
01001 <div class="parity1 source"><a href="#l446" id="l446"> 446</a> double fmult = 1.0;
01002 </div>
01003 <div class="parity0 source"><a href="#l447" id="l447"> 447</a> if( pjoint->type != urdf::Joint::PRISMATIC ) {
01004 </div>
01005 <div class="parity1 source"><a href="#l448" id="l448"> 448</a> fmult = 180.0/M_PI;
01006 </div>
01007 <div class="parity0 source"><a href="#l449" id="l449"> 449</a> }
01008 </div>
01009 <div class="parity1 source"><a href="#l450" id="l450"> 450</a> domKinematics_limitsRef plimits = daeSafeCast<domKinematics_limits>(kai->add(COLLADA_ELEMENT_LIMITS));
01010 </div>
01011 <div class="parity0 source"><a href="#l451" id="l451"> 451</a> daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MIN)->add(COLLADA_ELEMENT_FLOAT))->setValue(flower*fmult);
01012 </div>
01013 <div class="parity1 source"><a href="#l452" id="l452"> 452</a> daeSafeCast<domCommon_float_or_param::domFloat>(plimits->add(COLLADA_ELEMENT_MAX)->add(COLLADA_ELEMENT_FLOAT))->setValue(fupper*fmult);
01014 </div>
01015 <div class="parity0 source"><a href="#l453" id="l453"> 453</a> }
01016 </div>
01017 <div class="parity1 source"><a href="#l454" id="l454"> 454</a>
01018 </div>
01019 <div class="parity0 source"><a href="#l455" id="l455"> 455</a> domCommon_bool_or_paramRef active = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_ACTIVE));
01020 </div>
01021 <div class="parity1 source"><a href="#l456" id="l456"> 456</a> daeSafeCast<domCommon_bool_or_param::domBool>(active->add(COLLADA_ELEMENT_BOOL))->setValue(bactive);
01022 </div>
01023 <div class="parity0 source"><a href="#l457" id="l457"> 457</a> domCommon_bool_or_paramRef locked = daeSafeCast<domCommon_bool_or_param>(kai->add(COLLADA_ELEMENT_LOCKED));
01024 </div>
01025 <div class="parity1 source"><a href="#l458" id="l458"> 458</a> daeSafeCast<domCommon_bool_or_param::domBool>(locked->add(COLLADA_ELEMENT_BOOL))->setValue(false);
01026 </div>
01027 <div class="parity0 source"><a href="#l459" id="l459"> 459</a>
01028 </div>
01029 <div class="parity1 source"><a href="#l460" id="l460"> 460</a>
01030 </div>
01031 <div class="parity0 source"><a href="#l461" id="l461"> 461</a> domMotion_axis_infoRef mai = daeSafeCast<domMotion_axis_info>(mt->add(COLLADA_ELEMENT_AXIS_INFO));
01032 </div>
01033 <div class="parity1 source"><a href="#l462" id="l462"> 462</a> mai->setAxis(str(boost::format("%s/%s")%askid%axis_infosid).c_str());
01034 </div>
01035 <div class="parity0 source"><a href="#l463" id="l463"> 463</a> if( !!pjoint->limits ) {
01036 </div>
01037 <div class="parity1 source"><a href="#l464" id="l464"> 464</a> domCommon_float_or_paramRef speed = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_SPEED));
01038 </div>
01039 <div class="parity0 source"><a href="#l465" id="l465"> 465</a> daeSafeCast<domCommon_float_or_param::domFloat>(speed->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->velocity);
01040 </div>
01041 <div class="parity1 source"><a href="#l466" id="l466"> 466</a> domCommon_float_or_paramRef accel = daeSafeCast<domCommon_float_or_param>(mai->add(COLLADA_ELEMENT_ACCELERATION));
01042 </div>
01043 <div class="parity0 source"><a href="#l467" id="l467"> 467</a> daeSafeCast<domCommon_float_or_param::domFloat>(accel->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->effort);
01044 </div>
01045 <div class="parity1 source"><a href="#l468" id="l468"> 468</a> }
01046 </div>
01047 <div class="parity0 source"><a href="#l469" id="l469"> 469</a> }
01048 </div>
01049 <div class="parity1 source"><a href="#l470" id="l470"> 470</a>
01050 </div>
01051 <div class="parity0 source"><a href="#l471" id="l471"> 471</a>
01052 </div>
01053 <div class="parity1 source"><a href="#l472" id="l472"> 472</a> string asmsym = _ComputeId(str(boost::format("%s_%s")%asmid%_ikmout->ikm->getSid()));
01054 </div>
01055 <div class="parity0 source"><a href="#l473" id="l473"> 473</a> string assym = _ComputeId(str(boost::format("%s_%s")%_scene.kscene->getID()%_ikmout->ikm->getSid()));
01056 </div>
01057 <div class="parity1 source"><a href="#l474" id="l474"> 474</a> FOREACH(it, _ikmout->vkinematicsbindings) {
01058 </div>
01059 <div class="parity0 source"><a href="#l475" id="l475"> 475</a> domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
01060 </div>
01061 <div class="parity1 source"><a href="#l476" id="l476"> 476</a> abm->setSid(asmsym.c_str());
01062 </div>
01063 <div class="parity0 source"><a href="#l477" id="l477"> 477</a> daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%it->first).c_str());
01064 </div>
01065 <div class="parity1 source"><a href="#l478" id="l478"> 478</a> domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
01066 </div>
01067 <div class="parity0 source"><a href="#l479" id="l479"> 479</a> ab->setSymbol(assym.c_str());
01068 </div>
01069 <div class="parity1 source"><a href="#l480" id="l480"> 480</a> daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s")%asmid%asmsym).c_str());
01070 </div>
01071 <div class="parity0 source"><a href="#l481" id="l481"> 481</a> _iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSymbol()), it->second));
01072 </div>
01073 <div class="parity1 source"><a href="#l482" id="l482"> 482</a> }
01074 </div>
01075 <div class="parity0 source"><a href="#l483" id="l483"> 483</a> for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) {
01076 </div>
01077 <div class="parity1 source"><a href="#l484" id="l484"> 484</a> const axis_sids& kas = _ikmout->vaxissids.at(idof);
01078 </div>
01079 <div class="parity0 source"><a href="#l485" id="l485"> 485</a> domKinematics_newparamRef abm = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
01080 </div>
01081 <div class="parity1 source"><a href="#l486" id="l486"> 486</a> abm->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.axissid)).c_str());
01082 </div>
01083 <div class="parity0 source"><a href="#l487" id="l487"> 487</a> daeSafeCast<domKinematics_newparam::domSIDREF>(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.axissid).c_str());
01084 </div>
01085 <div class="parity1 source"><a href="#l488" id="l488"> 488</a> domKinematics_bindRef ab = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
01086 </div>
01087 <div class="parity0 source"><a href="#l489" id="l489"> 489</a> ab->setSymbol(str(boost::format("%s_%s")%assym%kas.axissid).c_str());
01088 </div>
01089 <div class="parity1 source"><a href="#l490" id="l490"> 490</a> daeSafeCast<domKinematics_param>(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.axissid).c_str());
01090 </div>
01091 <div class="parity0 source"><a href="#l491" id="l491"> 491</a> string valuesid;
01092 </div>
01093 <div class="parity1 source"><a href="#l492" id="l492"> 492</a> if( kas.valuesid.size() > 0 ) {
01094 </div>
01095 <div class="parity0 source"><a href="#l493" id="l493"> 493</a> domKinematics_newparamRef abmvalue = daeSafeCast<domKinematics_newparam>(ias_motion->add(COLLADA_ELEMENT_NEWPARAM));
01096 </div>
01097 <div class="parity1 source"><a href="#l494" id="l494"> 494</a> abmvalue->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.valuesid)).c_str());
01098 </div>
01099 <div class="parity0 source"><a href="#l495" id="l495"> 495</a> daeSafeCast<domKinematics_newparam::domSIDREF>(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str());
01100 </div>
01101 <div class="parity1 source"><a href="#l496" id="l496"> 496</a> domKinematics_bindRef abvalue = daeSafeCast<domKinematics_bind>(ias->add(COLLADA_ELEMENT_BIND));
01102 </div>
01103 <div class="parity0 source"><a href="#l497" id="l497"> 497</a> valuesid = _ComputeId(str(boost::format("%s_%s")%assym%kas.valuesid));
01104 </div>
01105 <div class="parity1 source"><a href="#l498" id="l498"> 498</a> abvalue->setSymbol(valuesid.c_str());
01106 </div>
01107 <div class="parity0 source"><a href="#l499" id="l499"> 499</a> daeSafeCast<domKinematics_param>(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.valuesid).c_str());
01108 </div>
01109 <div class="parity1 source"><a href="#l500" id="l500"> 500</a> }
01110 </div>
01111 <div class="parity0 source"><a href="#l501" id="l501"> 501</a> _iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid));
01112 </div>
01113 <div class="parity1 source"><a href="#l502" id="l502"> 502</a> }
01114 </div>
01115 <div class="parity0 source"><a href="#l503" id="l503"> 503</a> }
01116 </div>
01117 <div class="parity1 source"><a href="#l504" id="l504"> 504</a>
01118 </div>
01119 <div class="parity0 source"><a href="#l505" id="l505"> 505</a>
01120 </div>
01121 <div class="parity1 source"><a href="#l506" id="l506"> 506</a> virtual void _WriteInstance_kinematics_model(daeElementRef parent, const string& sidscope, int id)
01122 </div>
01123 <div class="parity0 source"><a href="#l507" id="l507"> 507</a> {
01124 </div>
01125 <div class="parity1 source"><a href="#l508" id="l508"> 508</a> ROS_DEBUG_STREAM(str(boost::format("writing instance_kinematics_model %s\n")%_robot.getName()));
01126 </div>
01127 <div class="parity0 source"><a href="#l509" id="l509"> 509</a> boost::shared_ptr<kinematics_model_output> kmout = WriteKinematics_model(id);
01128 </div>
01129 <div class="parity1 source"><a href="#l510" id="l510"> 510</a>
01130 </div>
01131 <div class="parity0 source"><a href="#l511" id="l511"> 511</a> _ikmout.reset(new instance_kinematics_model_output());
01132 </div>
01133 <div class="parity1 source"><a href="#l512" id="l512"> 512</a> _ikmout->kmout = kmout;
01134 </div>
01135 <div class="parity0 source"><a href="#l513" id="l513"> 513</a> _ikmout->ikm = daeSafeCast<domInstance_kinematics_model>(parent->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
01136 </div>
01137 <div class="parity1 source"><a href="#l514" id="l514"> 514</a>
01138 </div>
01139 <div class="parity0 source"><a href="#l515" id="l515"> 515</a> string symscope, refscope;
01140 </div>
01141 <div class="parity1 source"><a href="#l516" id="l516"> 516</a> if( sidscope.size() > 0 ) {
01142 </div>
01143 <div class="parity0 source"><a href="#l517" id="l517"> 517</a> symscope = sidscope+string("_");
01144 </div>
01145 <div class="parity1 source"><a href="#l518" id="l518"> 518</a> refscope = sidscope+string("/");
01146 </div>
01147 <div class="parity0 source"><a href="#l519" id="l519"> 519</a> }
01148 </div>
01149 <div class="parity1 source"><a href="#l520" id="l520"> 520</a> string ikmsid = _ComputeId(str(boost::format("%s_inst")%kmout->kmodel->getID()));
01150 </div>
01151 <div class="parity0 source"><a href="#l521" id="l521"> 521</a> _ikmout->ikm->setUrl(str(boost::format("#%s")%kmout->kmodel->getID()).c_str());
01152 </div>
01153 <div class="parity1 source"><a href="#l522" id="l522"> 522</a> _ikmout->ikm->setSid(ikmsid.c_str());
01154 </div>
01155 <div class="parity0 source"><a href="#l523" id="l523"> 523</a>
01156 </div>
01157 <div class="parity1 source"><a href="#l524" id="l524"> 524</a> domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
01158 </div>
01159 <div class="parity0 source"><a href="#l525" id="l525"> 525</a> kbind->setSid(_ComputeId(symscope+ikmsid).c_str());
01160 </div>
01161 <div class="parity1 source"><a href="#l526" id="l526"> 526</a> daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str());
01162 </div>
01163 <div class="parity0 source"><a href="#l527" id="l527"> 527</a> _ikmout->vkinematicsbindings.push_back(make_pair(string(kbind->getSid()), str(boost::format("visual%d/node0")%id)));
01164 </div>
01165 <div class="parity1 source"><a href="#l528" id="l528"> 528</a>
01166 </div>
01167 <div class="parity0 source"><a href="#l529" id="l529"> 529</a> _ikmout->vaxissids.reserve(kmout->vaxissids.size());
01168 </div>
01169 <div class="parity1 source"><a href="#l530" id="l530"> 530</a> int i = 0;
01170 </div>
01171 <div class="parity0 source"><a href="#l531" id="l531"> 531</a> FOREACH(it,kmout->vaxissids) {
01172 </div>
01173 <div class="parity1 source"><a href="#l532" id="l532"> 532</a> domKinematics_newparamRef kbind = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
01174 </div>
01175 <div class="parity0 source"><a href="#l533" id="l533"> 533</a> string ref = it->sid;
01176 </div>
01177 <div class="parity1 source"><a href="#l534" id="l534"> 534</a> size_t index = ref.find("/");
01178 </div>
01179 <div class="parity0 source"><a href="#l535" id="l535"> 535</a> while(index != string::npos) {
01180 </div>
01181 <div class="parity1 source"><a href="#l536" id="l536"> 536</a> ref[index] = '.';
01182 </div>
01183 <div class="parity0 source"><a href="#l537" id="l537"> 537</a> index = ref.find("/",index+1);
01184 </div>
01185 <div class="parity1 source"><a href="#l538" id="l538"> 538</a> }
01186 </div>
01187 <div class="parity0 source"><a href="#l539" id="l539"> 539</a> string sid = _ComputeId(symscope+ikmsid+"_"+ref);
01188 </div>
01189 <div class="parity1 source"><a href="#l540" id="l540"> 540</a> kbind->setSid(sid.c_str());
01190 </div>
01191 <div class="parity0 source"><a href="#l541" id="l541"> 541</a> daeSafeCast<domKinematics_newparam::domSIDREF>(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str());
01192 </div>
01193 <div class="parity1 source"><a href="#l542" id="l542"> 542</a> double value=0;
01194 </div>
01195 <div class="parity0 source"><a href="#l543" id="l543"> 543</a> double flower=0, fupper=0;
01196 </div>
01197 <div class="parity1 source"><a href="#l544" id="l544"> 544</a> if( !!it->pjoint->limits ) {
01198 </div>
01199 <div class="parity0 source"><a href="#l545" id="l545"> 545</a> flower = it->pjoint->limits->lower;
01200 </div>
01201 <div class="parity1 source"><a href="#l546" id="l546"> 546</a> fupper = it->pjoint->limits->upper;
01202 </div>
01203 <div class="parity0 source"><a href="#l547" id="l547"> 547</a> if( flower > 0 || fupper < 0 ) {
01204 </div>
01205 <div class="parity1 source"><a href="#l548" id="l548"> 548</a> value = 0.5*(flower+fupper);
01206 </div>
01207 <div class="parity0 source"><a href="#l549" id="l549"> 549</a> }
01208 </div>
01209 <div class="parity1 source"><a href="#l550" id="l550"> 550</a> }
01210 </div>
01211 <div class="parity0 source"><a href="#l551" id="l551"> 551</a>
01212 </div>
01213 <div class="parity1 source"><a href="#l552" id="l552"> 552</a> domKinematics_newparamRef pvalueparam = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
01214 </div>
01215 <div class="parity0 source"><a href="#l553" id="l553"> 553</a> pvalueparam->setSid((sid+string("_value")).c_str());
01216 </div>
01217 <div class="parity1 source"><a href="#l554" id="l554"> 554</a> daeSafeCast<domKinematics_newparam::domFloat>(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(value);
01218 </div>
01219 <div class="parity0 source"><a href="#l555" id="l555"> 555</a> _ikmout->vaxissids.push_back(axis_sids(sid,pvalueparam->getSid(),kmout->vaxissids.at(i).jointnodesid));
01220 </div>
01221 <div class="parity1 source"><a href="#l556" id="l556"> 556</a> ++i;
01222 </div>
01223 <div class="parity0 source"><a href="#l557" id="l557"> 557</a> }
01224 </div>
01225 <div class="parity1 source"><a href="#l558" id="l558"> 558</a> }
01226 </div>
01227 <div class="parity0 source"><a href="#l559" id="l559"> 559</a>
01228 </div>
01229 <div class="parity1 source"><a href="#l560" id="l560"> 560</a> virtual boost::shared_ptr<kinematics_model_output> WriteKinematics_model(int id)
01230 </div>
01231 <div class="parity0 source"><a href="#l561" id="l561"> 561</a> {
01232 </div>
01233 <div class="parity1 source"><a href="#l562" id="l562"> 562</a> domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(_kinematicsModelsLib->add(COLLADA_ELEMENT_KINEMATICS_MODEL));
01234 </div>
01235 <div class="parity0 source"><a href="#l563" id="l563"> 563</a> string kmodelid = _ComputeId(str(boost::format("kmodel%d")%id));
01236 </div>
01237 <div class="parity1 source"><a href="#l564" id="l564"> 564</a> kmodel->setId(kmodelid.c_str());
01238 </div>
01239 <div class="parity0 source"><a href="#l565" id="l565"> 565</a> kmodel->setName(_robot.getName().c_str());
01240 </div>
01241 <div class="parity1 source"><a href="#l566" id="l566"> 566</a>
01242 </div>
01243 <div class="parity0 source"><a href="#l567" id="l567"> 567</a> domKinematics_model_techniqueRef ktec = daeSafeCast<domKinematics_model_technique>(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01244 </div>
01245 <div class="parity1 source"><a href="#l568" id="l568"> 568</a>
01246 </div>
01247 <div class="parity0 source"><a href="#l569" id="l569"> 569</a>
01248 </div>
01249 <div class="parity1 source"><a href="#l570" id="l570"> 570</a> domNodeRef pnoderoot = daeSafeCast<domNode>(_scene.vscene->add(COLLADA_ELEMENT_NODE));
01250 </div>
01251 <div class="parity0 source"><a href="#l571" id="l571"> 571</a> string bodyid = _ComputeId(str(boost::format("visual%d")%id));
01252 </div>
01253 <div class="parity1 source"><a href="#l572" id="l572"> 572</a> pnoderoot->setId(bodyid.c_str());
01254 </div>
01255 <div class="parity0 source"><a href="#l573" id="l573"> 573</a> pnoderoot->setSid(bodyid.c_str());
01256 </div>
01257 <div class="parity1 source"><a href="#l574" id="l574"> 574</a> pnoderoot->setName(_robot.getName().c_str());
01258 </div>
01259 <div class="parity0 source"><a href="#l575" id="l575"> 575</a>
01260 </div>
01261 <div class="parity1 source"><a href="#l576" id="l576"> 576</a>
01262 </div>
01263 <div class="parity0 source"><a href="#l577" id="l577"> 577</a> _mapjointindices.clear();
01264 </div>
01265 <div class="parity1 source"><a href="#l578" id="l578"> 578</a> int index=0;
01266 </div>
01267 <div class="parity0 source"><a href="#l579" id="l579"> 579</a> FOREACHC(itj, _robot.joints_) {
01268 </div>
01269 <div class="parity1 source"><a href="#l580" id="l580"> 580</a> _mapjointindices[itj->second] = index++;
01270 </div>
01271 <div class="parity0 source"><a href="#l581" id="l581"> 581</a> }
01272 </div>
01273 <div class="parity1 source"><a href="#l582" id="l582"> 582</a> _maplinkindices.clear();
01274 </div>
01275 <div class="parity0 source"><a href="#l583" id="l583"> 583</a> index=0;
01276 </div>
01277 <div class="parity1 source"><a href="#l584" id="l584"> 584</a> FOREACHC(itj, _robot.links_) {
01278 </div>
01279 <div class="parity0 source"><a href="#l585" id="l585"> 585</a> _maplinkindices[itj->second] = index++;
01280 </div>
01281 <div class="parity1 source"><a href="#l586" id="l586"> 586</a> }
01282 </div>
01283 <div class="parity0 source"><a href="#l587" id="l587"> 587</a> _mapmaterialindices.clear();
01284 </div>
01285 <div class="parity1 source"><a href="#l588" id="l588"> 588</a> index=0;
01286 </div>
01287 <div class="parity0 source"><a href="#l589" id="l589"> 589</a> FOREACHC(itj, _robot.materials_) {
01288 </div>
01289 <div class="parity1 source"><a href="#l590" id="l590"> 590</a> _mapmaterialindices[itj->second] = index++;
01290 </div>
01291 <div class="parity0 source"><a href="#l591" id="l591"> 591</a> }
01292 </div>
01293 <div class="parity1 source"><a href="#l592" id="l592"> 592</a>
01294 </div>
01295 <div class="parity0 source"><a href="#l593" id="l593"> 593</a> double lmin, lmax;
01296 </div>
01297 <div class="parity1 source"><a href="#l594" id="l594"> 594</a> vector<domJointRef> vdomjoints(_robot.joints_.size());
01298 </div>
01299 <div class="parity0 source"><a href="#l595" id="l595"> 595</a> boost::shared_ptr<kinematics_model_output> kmout(new kinematics_model_output());
01300 </div>
01301 <div class="parity1 source"><a href="#l596" id="l596"> 596</a> kmout->kmodel = kmodel;
01302 </div>
01303 <div class="parity0 source"><a href="#l597" id="l597"> 597</a> kmout->vaxissids.resize(_robot.joints_.size());
01304 </div>
01305 <div class="parity1 source"><a href="#l598" id="l598"> 598</a> kmout->vlinksids.resize(_robot.links_.size());
01306 </div>
01307 <div class="parity0 source"><a href="#l599" id="l599"> 599</a>
01308 </div>
01309 <div class="parity1 source"><a href="#l600" id="l600"> 600</a> FOREACHC(itjoint, _robot.joints_) {
01310 </div>
01311 <div class="parity0 source"><a href="#l601" id="l601"> 601</a> boost::shared_ptr<urdf::Joint> pjoint = itjoint->second;
01312 </div>
01313 <div class="parity1 source"><a href="#l602" id="l602"> 602</a> int index = _mapjointindices[itjoint->second];
01314 </div>
01315 <div class="parity0 source"><a href="#l603" id="l603"> 603</a> domJointRef pdomjoint = daeSafeCast<domJoint>(ktec->add(COLLADA_ELEMENT_JOINT));
01316 </div>
01317 <div class="parity1 source"><a href="#l604" id="l604"> 604</a> string jointid = _ComputeId(pjoint->name);
01318 </div>
01319 <div class="parity0 source"><a href="#l605" id="l605"> 605</a> pdomjoint->setSid(jointid.c_str() );
01320 </div>
01321 <div class="parity1 source"><a href="#l606" id="l606"> 606</a> pdomjoint->setName(pjoint->name.c_str());
01322 </div>
01323 <div class="parity0 source"><a href="#l607" id="l607"> 607</a> domAxis_constraintRef axis;
01324 </div>
01325 <div class="parity1 source"><a href="#l608" id="l608"> 608</a> if( !!pjoint->limits ) {
01326 </div>
01327 <div class="parity0 source"><a href="#l609" id="l609"> 609</a> lmin=pjoint->limits->lower;
01328 </div>
01329 <div class="parity1 source"><a href="#l610" id="l610"> 610</a> lmax=pjoint->limits->upper;
01330 </div>
01331 <div class="parity0 source"><a href="#l611" id="l611"> 611</a> }
01332 </div>
01333 <div class="parity1 source"><a href="#l612" id="l612"> 612</a> else {
01334 </div>
01335 <div class="parity0 source"><a href="#l613" id="l613"> 613</a> lmin = lmax = 0;
01336 </div>
01337 <div class="parity1 source"><a href="#l614" id="l614"> 614</a> }
01338 </div>
01339 <div class="parity0 source"><a href="#l615" id="l615"> 615</a>
01340 </div>
01341 <div class="parity1 source"><a href="#l616" id="l616"> 616</a> double fmult = 1.0;
01342 </div>
01343 <div class="parity0 source"><a href="#l617" id="l617"> 617</a> switch(pjoint->type) {
01344 </div>
01345 <div class="parity1 source"><a href="#l618" id="l618"> 618</a> case urdf::Joint::REVOLUTE:
01346 </div>
01347 <div class="parity0 source"><a href="#l619" id="l619"> 619</a> case urdf::Joint::CONTINUOUS:
01348 </div>
01349 <div class="parity1 source"><a href="#l620" id="l620"> 620</a> axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
01350 </div>
01351 <div class="parity0 source"><a href="#l621" id="l621"> 621</a> fmult = 180.0f/M_PI;
01352 </div>
01353 <div class="parity1 source"><a href="#l622" id="l622"> 622</a> lmin*=fmult;
01354 </div>
01355 <div class="parity0 source"><a href="#l623" id="l623"> 623</a> lmax*=fmult;
01356 </div>
01357 <div class="parity1 source"><a href="#l624" id="l624"> 624</a> break;
01358 </div>
01359 <div class="parity0 source"><a href="#l625" id="l625"> 625</a> case urdf::Joint::PRISMATIC:
01360 </div>
01361 <div class="parity1 source"><a href="#l626" id="l626"> 626</a> axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC));
01362 </div>
01363 <div class="parity0 source"><a href="#l627" id="l627"> 627</a> break;
01364 </div>
01365 <div class="parity1 source"><a href="#l628" id="l628"> 628</a> case urdf::Joint::FIXED:
01366 </div>
01367 <div class="parity0 source"><a href="#l629" id="l629"> 629</a> axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
01368 </div>
01369 <div class="parity1 source"><a href="#l630" id="l630"> 630</a> lmin = 0;
01370 </div>
01371 <div class="parity0 source"><a href="#l631" id="l631"> 631</a> lmax = 0;
01372 </div>
01373 <div class="parity1 source"><a href="#l632" id="l632"> 632</a> fmult = 0;
01374 </div>
01375 <div class="parity0 source"><a href="#l633" id="l633"> 633</a> break;
01376 </div>
01377 <div class="parity1 source"><a href="#l634" id="l634"> 634</a> default:
01378 </div>
01379 <div class="parity0 source"><a href="#l635" id="l635"> 635</a> ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type));
01380 </div>
01381 <div class="parity1 source"><a href="#l636" id="l636"> 636</a> break;
01382 </div>
01383 <div class="parity0 source"><a href="#l637" id="l637"> 637</a> }
01384 </div>
01385 <div class="parity1 source"><a href="#l638" id="l638"> 638</a>
01386 </div>
01387 <div class="parity0 source"><a href="#l639" id="l639"> 639</a> if( !axis ) {
01388 </div>
01389 <div class="parity1 source"><a href="#l640" id="l640"> 640</a> continue;
01390 </div>
01391 <div class="parity0 source"><a href="#l641" id="l641"> 641</a> }
01392 </div>
01393 <div class="parity1 source"><a href="#l642" id="l642"> 642</a>
01394 </div>
01395 <div class="parity0 source"><a href="#l643" id="l643"> 643</a> int ia = 0;
01396 </div>
01397 <div class="parity1 source"><a href="#l644" id="l644"> 644</a> string axisid = _ComputeId(str(boost::format("axis%d")%ia));
01398 </div>
01399 <div class="parity0 source"><a href="#l645" id="l645"> 645</a> axis->setSid(axisid.c_str());
01400 </div>
01401 <div class="parity1 source"><a href="#l646" id="l646"> 646</a> kmout->vaxissids.at(index).pjoint = pjoint;
01402 </div>
01403 <div class="parity0 source"><a href="#l647" id="l647"> 647</a> kmout->vaxissids.at(index).sid = jointid+string("/")+axisid;
01404 </div>
01405 <div class="parity1 source"><a href="#l648" id="l648"> 648</a> kmout->vaxissids.at(index).iaxis = ia;
01406 </div>
01407 <div class="parity0 source"><a href="#l649" id="l649"> 649</a> domAxisRef paxis = daeSafeCast<domAxis>(axis->add(COLLADA_ELEMENT_AXIS));
01408 </div>
01409 <div class="parity1 source"><a href="#l650" id="l650"> 650</a> paxis->getValue().setCount(3);
01410 </div>
01411 <div class="parity0 source"><a href="#l651" id="l651"> 651</a> paxis->getValue()[0] = pjoint->axis.x;
01412 </div>
01413 <div class="parity1 source"><a href="#l652" id="l652"> 652</a> paxis->getValue()[1] = pjoint->axis.y;
01414 </div>
01415 <div class="parity0 source"><a href="#l653" id="l653"> 653</a> paxis->getValue()[2] = pjoint->axis.z;
01416 </div>
01417 <div class="parity1 source"><a href="#l654" id="l654"> 654</a> if( pjoint->type != urdf::Joint::CONTINUOUS ) {
01418 </div>
01419 <div class="parity0 source"><a href="#l655" id="l655"> 655</a> domJoint_limitsRef plimits = daeSafeCast<domJoint_limits>(axis->add(COLLADA_TYPE_LIMITS));
01420 </div>
01421 <div class="parity1 source"><a href="#l656" id="l656"> 656</a> daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MIN))->getValue() = lmin;
01422 </div>
01423 <div class="parity0 source"><a href="#l657" id="l657"> 657</a> daeSafeCast<domMinmax>(plimits->add(COLLADA_ELEMENT_MAX))->getValue() = lmax;
01424 </div>
01425 <div class="parity1 source"><a href="#l658" id="l658"> 658</a> }
01426 </div>
01427 <div class="parity0 source"><a href="#l659" id="l659"> 659</a> vdomjoints.at(index) = pdomjoint;
01428 </div>
01429 <div class="parity1 source"><a href="#l660" id="l660"> 660</a> }
01430 </div>
01431 <div class="parity0 source"><a href="#l661" id="l661"> 661</a>
01432 </div>
01433 <div class="parity1 source"><a href="#l662" id="l662"> 662</a> LINKOUTPUT childinfo = _WriteLink(_robot.getRoot(), ktec, pnoderoot, kmodel->getID());
01434 </div>
01435 <div class="parity0 source"><a href="#l663" id="l663"> 663</a> FOREACHC(itused, childinfo.listusedlinks) {
01436 </div>
01437 <div class="parity1 source"><a href="#l664" id="l664"> 664</a> kmout->vlinksids.at(itused->first) = itused->second;
01438 </div>
01439 <div class="parity0 source"><a href="#l665" id="l665"> 665</a> }
01440 </div>
01441 <div class="parity1 source"><a href="#l666" id="l666"> 666</a> FOREACH(itprocessed,childinfo.listprocesseddofs) {
01442 </div>
01443 <div class="parity0 source"><a href="#l667" id="l667"> 667</a> kmout->vaxissids.at(itprocessed->first).jointnodesid = itprocessed->second;
01444 </div>
01445 <div class="parity1 source"><a href="#l668" id="l668"> 668</a> }
01446 </div>
01447 <div class="parity0 source"><a href="#l669" id="l669"> 669</a>
01448 </div>
01449 <div class="parity1 source"><a href="#l670" id="l670"> 670</a>
01450 </div>
01451 <div class="parity0 source"><a href="#l671" id="l671"> 671</a> FOREACHC(itjoint, _robot.joints_) {
01452 </div>
01453 <div class="parity1 source"><a href="#l672" id="l672"> 672</a> string jointsid = _ComputeId(itjoint->second->name);
01454 </div>
01455 <div class="parity0 source"><a href="#l673" id="l673"> 673</a> boost::shared_ptr<urdf::Joint> pjoint = itjoint->second;
01456 </div>
01457 <div class="parity1 source"><a href="#l674" id="l674"> 674</a> if( !pjoint->mimic ) {
01458 </div>
01459 <div class="parity0 source"><a href="#l675" id="l675"> 675</a> continue;
01460 </div>
01461 <div class="parity1 source"><a href="#l676" id="l676"> 676</a> }
01462 </div>
01463 <div class="parity0 source"><a href="#l677" id="l677"> 677</a>
01464 </div>
01465 <div class="parity1 source"><a href="#l678" id="l678"> 678</a> domFormulaRef pf = daeSafeCast<domFormula>(ktec->add(COLLADA_ELEMENT_FORMULA));
01466 </div>
01467 <div class="parity0 source"><a href="#l679" id="l679"> 679</a> string formulaid = _ComputeId(str(boost::format("%s_formula")%jointsid));
01468 </div>
01469 <div class="parity1 source"><a href="#l680" id="l680"> 680</a> pf->setSid(formulaid.c_str());
01470 </div>
01471 <div class="parity0 source"><a href="#l681" id="l681"> 681</a> domCommon_float_or_paramRef ptarget = daeSafeCast<domCommon_float_or_param>(pf->add(COLLADA_ELEMENT_TARGET));
01472 </div>
01473 <div class="parity1 source"><a href="#l682" id="l682"> 682</a> string targetjointid = str(boost::format("%s/%s")%kmodel->getID()%jointsid);
01474 </div>
01475 <div class="parity0 source"><a href="#l683" id="l683"> 683</a> daeSafeCast<domCommon_param>(ptarget->add(COLLADA_TYPE_PARAM))->setValue(targetjointid.c_str());
01476 </div>
01477 <div class="parity1 source"><a href="#l684" id="l684"> 684</a>
01478 </div>
01479 <div class="parity0 source"><a href="#l685" id="l685"> 685</a> domTechniqueRef pftec = daeSafeCast<domTechnique>(pf->add(COLLADA_ELEMENT_TECHNIQUE));
01480 </div>
01481 <div class="parity1 source"><a href="#l686" id="l686"> 686</a> pftec->setProfile("OpenRAVE");
01482 </div>
01483 <div class="parity0 source"><a href="#l687" id="l687"> 687</a>
01484 </div>
01485 <div class="parity1 source"><a href="#l688" id="l688"> 688</a> {
01486 </div>
01487 <div class="parity0 source"><a href="#l689" id="l689"> 689</a> daeElementRef poselt = pftec->add("equation");
01488 </div>
01489 <div class="parity1 source"><a href="#l690" id="l690"> 690</a> poselt->setAttribute("type","position");
01490 </div>
01491 <div class="parity0 source"><a href="#l691" id="l691"> 691</a>
01492 </div>
01493 <div class="parity1 source"><a href="#l692" id="l692"> 692</a>
01494 </div>
01495 <div class="parity0 source"><a href="#l693" id="l693"> 693</a> daeElementRef pmath_math = poselt->add("math");
01496 </div>
01497 <div class="parity1 source"><a href="#l694" id="l694"> 694</a> daeElementRef pmath_apply = pmath_math->add("apply");
01498 </div>
01499 <div class="parity0 source"><a href="#l695" id="l695"> 695</a> {
01500 </div>
01501 <div class="parity1 source"><a href="#l696" id="l696"> 696</a> daeElementRef pmath_plus = pmath_apply->add("plus");
01502 </div>
01503 <div class="parity0 source"><a href="#l697" id="l697"> 697</a> daeElementRef pmath_apply1 = pmath_apply->add("apply");
01504 </div>
01505 <div class="parity1 source"><a href="#l698" id="l698"> 698</a> {
01506 </div>
01507 <div class="parity0 source"><a href="#l699" id="l699"> 699</a> daeElementRef pmath_times = pmath_apply1->add("times");
01508 </div>
01509 <div class="parity1 source"><a href="#l700" id="l700"> 700</a> daeElementRef pmath_const0 = pmath_apply1->add("cn");
01510 </div>
01511 <div class="parity0 source"><a href="#l701" id="l701"> 701</a> pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
01512 </div>
01513 <div class="parity1 source"><a href="#l702" id="l702"> 702</a> daeElementRef pmath_symb = pmath_apply1->add("csymbol");
01514 </div>
01515 <div class="parity0 source"><a href="#l703" id="l703"> 703</a> pmath_symb->setAttribute("encoding","COLLADA");
01516 </div>
01517 <div class="parity1 source"><a href="#l704" id="l704"> 704</a> pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
01518 </div>
01519 <div class="parity0 source"><a href="#l705" id="l705"> 705</a> }
01520 </div>
01521 <div class="parity1 source"><a href="#l706" id="l706"> 706</a> daeElementRef pmath_const1 = pmath_apply->add("cn");
01522 </div>
01523 <div class="parity0 source"><a href="#l707" id="l707"> 707</a> pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));
01524 </div>
01525 <div class="parity1 source"><a href="#l708" id="l708"> 708</a> }
01526 </div>
01527 <div class="parity0 source"><a href="#l709" id="l709"> 709</a> }
01528 </div>
01529 <div class="parity1 source"><a href="#l710" id="l710"> 710</a>
01530 </div>
01531 <div class="parity0 source"><a href="#l711" id="l711"> 711</a> {
01532 </div>
01533 <div class="parity1 source"><a href="#l712" id="l712"> 712</a> daeElementRef derivelt = pftec->add("equation");
01534 </div>
01535 <div class="parity0 source"><a href="#l713" id="l713"> 713</a> derivelt->setAttribute("type","first_partial");
01536 </div>
01537 <div class="parity1 source"><a href="#l714" id="l714"> 714</a> derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
01538 </div>
01539 <div class="parity0 source"><a href="#l715" id="l715"> 715</a> daeElementRef pmath_const0 = derivelt->add("cn");
01540 </div>
01541 <div class="parity1 source"><a href="#l716" id="l716"> 716</a> pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
01542 </div>
01543 <div class="parity0 source"><a href="#l717" id="l717"> 717</a> }
01544 </div>
01545 <div class="parity1 source"><a href="#l718" id="l718"> 718</a>
01546 </div>
01547 <div class="parity0 source"><a href="#l719" id="l719"> 719</a> {
01548 </div>
01549 <div class="parity1 source"><a href="#l720" id="l720"> 720</a> daeElementRef derivelt = pftec->add("equation");
01550 </div>
01551 <div class="parity0 source"><a href="#l721" id="l721"> 721</a> derivelt->setAttribute("type","second_partial");
01552 </div>
01553 <div class="parity1 source"><a href="#l722" id="l722"> 722</a> derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str());
01554 </div>
01555 <div class="parity0 source"><a href="#l723" id="l723"> 723</a> daeElementRef pmath_const0 = derivelt->add("cn");
01556 </div>
01557 <div class="parity1 source"><a href="#l724" id="l724"> 724</a> pmath_const0->setCharData("0");
01558 </div>
01559 <div class="parity0 source"><a href="#l725" id="l725"> 725</a> }
01560 </div>
01561 <div class="parity1 source"><a href="#l726" id="l726"> 726</a>
01562 </div>
01563 <div class="parity0 source"><a href="#l727" id="l727"> 727</a> {
01564 </div>
01565 <div class="parity1 source"><a href="#l728" id="l728"> 728</a> domFormula_techniqueRef pfcommontec = daeSafeCast<domFormula_technique>(pf->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01566 </div>
01567 <div class="parity0 source"><a href="#l729" id="l729"> 729</a>
01568 </div>
01569 <div class="parity1 source"><a href="#l730" id="l730"> 730</a>
01570 </div>
01571 <div class="parity0 source"><a href="#l731" id="l731"> 731</a> daeElementRef pmath_math = pfcommontec->add("math");
01572 </div>
01573 <div class="parity1 source"><a href="#l732" id="l732"> 732</a> daeElementRef pmath_apply = pmath_math->add("apply");
01574 </div>
01575 <div class="parity0 source"><a href="#l733" id="l733"> 733</a> {
01576 </div>
01577 <div class="parity1 source"><a href="#l734" id="l734"> 734</a> daeElementRef pmath_plus = pmath_apply->add("plus");
01578 </div>
01579 <div class="parity0 source"><a href="#l735" id="l735"> 735</a> daeElementRef pmath_apply1 = pmath_apply->add("apply");
01580 </div>
01581 <div class="parity1 source"><a href="#l736" id="l736"> 736</a> {
01582 </div>
01583 <div class="parity0 source"><a href="#l737" id="l737"> 737</a> daeElementRef pmath_times = pmath_apply1->add("times");
01584 </div>
01585 <div class="parity1 source"><a href="#l738" id="l738"> 738</a> daeElementRef pmath_const0 = pmath_apply1->add("cn");
01586 </div>
01587 <div class="parity0 source"><a href="#l739" id="l739"> 739</a> pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
01588 </div>
01589 <div class="parity1 source"><a href="#l740" id="l740"> 740</a> daeElementRef pmath_symb = pmath_apply1->add("csymbol");
01590 </div>
01591 <div class="parity0 source"><a href="#l741" id="l741"> 741</a> pmath_symb->setAttribute("encoding","COLLADA");
01592 </div>
01593 <div class="parity1 source"><a href="#l742" id="l742"> 742</a> pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)));
01594 </div>
01595 <div class="parity0 source"><a href="#l743" id="l743"> 743</a> }
01596 </div>
01597 <div class="parity1 source"><a href="#l744" id="l744"> 744</a> daeElementRef pmath_const1 = pmath_apply->add("cn");
01598 </div>
01599 <div class="parity0 source"><a href="#l745" id="l745"> 745</a> pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));
01600 </div>
01601 <div class="parity1 source"><a href="#l746" id="l746"> 746</a> }
01602 </div>
01603 <div class="parity0 source"><a href="#l747" id="l747"> 747</a> }
01604 </div>
01605 <div class="parity1 source"><a href="#l748" id="l748"> 748</a> }
01606 </div>
01607 <div class="parity0 source"><a href="#l749" id="l749"> 749</a>
01608 </div>
01609 <div class="parity1 source"><a href="#l750" id="l750"> 750</a> return kmout;
01610 </div>
01611 <div class="parity0 source"><a href="#l751" id="l751"> 751</a> }
01612 </div>
01613 <div class="parity1 source"><a href="#l752" id="l752"> 752</a>
01614 </div>
01615 <div class="parity0 source"><a href="#l753" id="l753"> 753</a>
01616 </div>
01617 <div class="parity1 source"><a href="#l754" id="l754"> 754</a>
01618 </div>
01619 <div class="parity0 source"><a href="#l755" id="l755"> 755</a>
01620 </div>
01621 <div class="parity1 source"><a href="#l756" id="l756"> 756</a>
01622 </div>
01623 <div class="parity0 source"><a href="#l757" id="l757"> 757</a>
01624 </div>
01625 <div class="parity1 source"><a href="#l758" id="l758"> 758</a>
01626 </div>
01627 <div class="parity0 source"><a href="#l759" id="l759"> 759</a> virtual LINKOUTPUT _WriteLink(boost::shared_ptr<const urdf::Link> plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri)
01628 </div>
01629 <div class="parity1 source"><a href="#l760" id="l760"> 760</a> {
01630 </div>
01631 <div class="parity0 source"><a href="#l761" id="l761"> 761</a> LINKOUTPUT out;
01632 </div>
01633 <div class="parity1 source"><a href="#l762" id="l762"> 762</a> int linkindex = _maplinkindices[plink];
01634 </div>
01635 <div class="parity0 source"><a href="#l763" id="l763"> 763</a> string linksid = _ComputeId(plink->name);
01636 </div>
01637 <div class="parity1 source"><a href="#l764" id="l764"> 764</a> domLinkRef pdomlink = daeSafeCast<domLink>(pkinparent->add(COLLADA_ELEMENT_LINK));
01638 </div>
01639 <div class="parity0 source"><a href="#l765" id="l765"> 765</a> pdomlink->setName(plink->name.c_str());
01640 </div>
01641 <div class="parity1 source"><a href="#l766" id="l766"> 766</a> pdomlink->setSid(linksid.c_str());
01642 </div>
01643 <div class="parity0 source"><a href="#l767" id="l767"> 767</a>
01644 </div>
01645 <div class="parity1 source"><a href="#l768" id="l768"> 768</a> domNodeRef pnode = daeSafeCast<domNode>(pnodeparent->add(COLLADA_ELEMENT_NODE));
01646 </div>
01647 <div class="parity0 source"><a href="#l769" id="l769"> 769</a> string nodeid = _ComputeId(str(boost::format("v%s_node%d")%strModelUri%linkindex));
01648 </div>
01649 <div class="parity1 source"><a href="#l770" id="l770"> 770</a> pnode->setId( nodeid.c_str() );
01650 </div>
01651 <div class="parity0 source"><a href="#l771" id="l771"> 771</a> string nodesid = _ComputeId(str(boost::format("node%d")%linkindex));
01652 </div>
01653 <div class="parity1 source"><a href="#l772" id="l772"> 772</a> pnode->setSid(nodesid.c_str());
01654 </div>
01655 <div class="parity0 source"><a href="#l773" id="l773"> 773</a> pnode->setName(plink->name.c_str());
01656 </div>
01657 <div class="parity1 source"><a href="#l774" id="l774"> 774</a>
01658 </div>
01659 <div class="parity0 source"><a href="#l775" id="l775"> 775</a> boost::shared_ptr<urdf::Geometry> geometry;
01660 </div>
01661 <div class="parity1 source"><a href="#l776" id="l776"> 776</a> boost::shared_ptr<urdf::Material> material;
01662 </div>
01663 <div class="parity0 source"><a href="#l777" id="l777"> 777</a> urdf::Pose geometry_origin;
01664 </div>
01665 <div class="parity1 source"><a href="#l778" id="l778"> 778</a> if( !!plink->visual ) {
01666 </div>
01667 <div class="parity0 source"><a href="#l779" id="l779"> 779</a> geometry = plink->visual->geometry;
01668 </div>
01669 <div class="parity1 source"><a href="#l780" id="l780"> 780</a> material = plink->visual->material;
01670 </div>
01671 <div class="parity0 source"><a href="#l781" id="l781"> 781</a> geometry_origin = plink->visual->origin;
01672 </div>
01673 <div class="parity1 source"><a href="#l782" id="l782"> 782</a> }
01674 </div>
01675 <div class="parity0 source"><a href="#l783" id="l783"> 783</a> else if( !!plink->collision ) {
01676 </div>
01677 <div class="parity1 source"><a href="#l784" id="l784"> 784</a> geometry = plink->collision->geometry;
01678 </div>
01679 <div class="parity0 source"><a href="#l785" id="l785"> 785</a> geometry_origin = plink->collision->origin;
01680 </div>
01681 <div class="parity1 source"><a href="#l786" id="l786"> 786</a> }
01682 </div>
01683 <div class="parity0 source"><a href="#l787" id="l787"> 787</a>
01684 </div>
01685 <div class="parity1 source"><a href="#l788" id="l788"> 788</a> if( !!geometry ) {
01686 </div>
01687 <div class="parity0 source"><a href="#l789" id="l789"> 789</a> int igeom = 0;
01688 </div>
01689 <div class="parity1 source"><a href="#l790" id="l790"> 790</a> string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
01690 </div>
01691 <div class="parity0 source"><a href="#l791" id="l791"> 791</a> domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid);
01692 </div>
01693 <div class="parity1 source"><a href="#l792" id="l792"> 792</a> domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
01694 </div>
01695 <div class="parity0 source"><a href="#l793" id="l793"> 793</a> pinstgeom->setUrl((string("#")+geomid).c_str());
01696 </div>
01697 <div class="parity1 source"><a href="#l794" id="l794"> 794</a>
01698 </div>
01699 <div class="parity0 source"><a href="#l795" id="l795"> 795</a>
01700 </div>
01701 <div class="parity1 source"><a href="#l796" id="l796"> 796</a> _WriteMaterial(pdomgeom->getID(), material);
01702 </div>
01703 <div class="parity0 source"><a href="#l797" id="l797"> 797</a> domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
01704 </div>
01705 <div class="parity1 source"><a href="#l798" id="l798"> 798</a> domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
01706 </div>
01707 <div class="parity0 source"><a href="#l799" id="l799"> 799</a> domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
01708 </div>
01709 <div class="parity1 source"><a href="#l800" id="l800"> 800</a> pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat")));
01710 </div>
01711 <div class="parity0 source"><a href="#l801" id="l801"> 801</a> pinstmat->setSymbol("mat0");
01712 </div>
01713 <div class="parity1 source"><a href="#l802" id="l802"> 802</a> }
01714 </div>
01715 <div class="parity0 source"><a href="#l803" id="l803"> 803</a>
01716 </div>
01717 <div class="parity1 source"><a href="#l804" id="l804"> 804</a> _WriteTransformation(pnode, geometry_origin);
01718 </div>
01719 <div class="parity0 source"><a href="#l805" id="l805"> 805</a> urdf::Pose geometry_origin_inv = _poseInverse(geometry_origin);
01720 </div>
01721 <div class="parity1 source"><a href="#l806" id="l806"> 806</a>
01722 </div>
01723 <div class="parity0 source"><a href="#l807" id="l807"> 807</a>
01724 </div>
01725 <div class="parity1 source"><a href="#l808" id="l808"> 808</a> FOREACHC(itjoint, plink->child_joints) {
01726 </div>
01727 <div class="parity0 source"><a href="#l809" id="l809"> 809</a> boost::shared_ptr<urdf::Joint> pjoint = *itjoint;
01728 </div>
01729 <div class="parity1 source"><a href="#l810" id="l810"> 810</a> int index = _mapjointindices[pjoint];
01730 </div>
01731 <div class="parity0 source"><a href="#l811" id="l811"> 811</a>
01732 </div>
01733 <div class="parity1 source"><a href="#l812" id="l812"> 812</a>
01734 </div>
01735 <div class="parity0 source"><a href="#l813" id="l813"> 813</a> domLink::domAttachment_fullRef attachment_full = daeSafeCast<domLink::domAttachment_full>(pdomlink->add(COLLADA_ELEMENT_ATTACHMENT_FULL));
01736 </div>
01737 <div class="parity1 source"><a href="#l814" id="l814"> 814</a> string jointid = str(boost::format("%s/%s")%strModelUri%_ComputeId(pjoint->name));
01738 </div>
01739 <div class="parity0 source"><a href="#l815" id="l815"> 815</a> attachment_full->setJoint(jointid.c_str());
01740 </div>
01741 <div class="parity1 source"><a href="#l816" id="l816"> 816</a>
01742 </div>
01743 <div class="parity0 source"><a href="#l817" id="l817"> 817</a> LINKOUTPUT childinfo = _WriteLink(_robot.getLink(pjoint->child_link_name), attachment_full, pnode, strModelUri);
01744 </div>
01745 <div class="parity1 source"><a href="#l818" id="l818"> 818</a> FOREACH(itused,childinfo.listusedlinks) {
01746 </div>
01747 <div class="parity0 source"><a href="#l819" id="l819"> 819</a> out.listusedlinks.push_back(make_pair(itused->first,linksid+string("/")+itused->second));
01748 </div>
01749 <div class="parity1 source"><a href="#l820" id="l820"> 820</a> }
01750 </div>
01751 <div class="parity0 source"><a href="#l821" id="l821"> 821</a> FOREACH(itprocessed,childinfo.listprocesseddofs) {
01752 </div>
01753 <div class="parity1 source"><a href="#l822" id="l822"> 822</a> out.listprocesseddofs.push_back(make_pair(itprocessed->first,nodesid+string("/")+itprocessed->second));
01754 </div>
01755 <div class="parity0 source"><a href="#l823" id="l823"> 823</a> }
01756 </div>
01757 <div class="parity1 source"><a href="#l824" id="l824"> 824</a>
01758 </div>
01759 <div class="parity0 source"><a href="#l825" id="l825"> 825</a>
01760 </div>
01761 <div class="parity1 source"><a href="#l826" id="l826"> 826</a> string jointnodesid = _ComputeId(str(boost::format("node_%s_axis0")%pjoint->name));
01762 </div>
01763 <div class="parity0 source"><a href="#l827" id="l827"> 827</a> switch(pjoint->type) {
01764 </div>
01765 <div class="parity1 source"><a href="#l828" id="l828"> 828</a> case urdf::Joint::REVOLUTE:
01766 </div>
01767 <div class="parity0 source"><a href="#l829" id="l829"> 829</a> case urdf::Joint::CONTINUOUS:
01768 </div>
01769 <div class="parity1 source"><a href="#l830" id="l830"> 830</a> case urdf::Joint::FIXED: {
01770 </div>
01771 <div class="parity0 source"><a href="#l831" id="l831"> 831</a> domRotateRef protate = daeSafeCast<domRotate>(childinfo.pnode->add(COLLADA_ELEMENT_ROTATE,0));
01772 </div>
01773 <div class="parity1 source"><a href="#l832" id="l832"> 832</a> protate->setSid(jointnodesid.c_str());
01774 </div>
01775 <div class="parity0 source"><a href="#l833" id="l833"> 833</a> protate->getValue().setCount(4);
01776 </div>
01777 <div class="parity1 source"><a href="#l834" id="l834"> 834</a> protate->getValue()[0] = pjoint->axis.x;
01778 </div>
01779 <div class="parity0 source"><a href="#l835" id="l835"> 835</a> protate->getValue()[1] = pjoint->axis.y;
01780 </div>
01781 <div class="parity1 source"><a href="#l836" id="l836"> 836</a> protate->getValue()[2] = pjoint->axis.z;
01782 </div>
01783 <div class="parity0 source"><a href="#l837" id="l837"> 837</a> protate->getValue()[3] = 0;
01784 </div>
01785 <div class="parity1 source"><a href="#l838" id="l838"> 838</a> break;
01786 </div>
01787 <div class="parity0 source"><a href="#l839" id="l839"> 839</a> }
01788 </div>
01789 <div class="parity1 source"><a href="#l840" id="l840"> 840</a> case urdf::Joint::PRISMATIC: {
01790 </div>
01791 <div class="parity0 source"><a href="#l841" id="l841"> 841</a> domTranslateRef ptrans = daeSafeCast<domTranslate>(childinfo.pnode->add(COLLADA_ELEMENT_TRANSLATE,0));
01792 </div>
01793 <div class="parity1 source"><a href="#l842" id="l842"> 842</a> ptrans->setSid(jointnodesid.c_str());
01794 </div>
01795 <div class="parity0 source"><a href="#l843" id="l843"> 843</a> ptrans->getValue().setCount(3);
01796 </div>
01797 <div class="parity1 source"><a href="#l844" id="l844"> 844</a> ptrans->getValue()[0] = 0;
01798 </div>
01799 <div class="parity0 source"><a href="#l845" id="l845"> 845</a> ptrans->getValue()[1] = 0;
01800 </div>
01801 <div class="parity1 source"><a href="#l846" id="l846"> 846</a> ptrans->getValue()[2] = 0;
01802 </div>
01803 <div class="parity0 source"><a href="#l847" id="l847"> 847</a> break;
01804 </div>
01805 <div class="parity1 source"><a href="#l848" id="l848"> 848</a> }
01806 </div>
01807 <div class="parity0 source"><a href="#l849" id="l849"> 849</a> default:
01808 </div>
01809 <div class="parity1 source"><a href="#l850" id="l850"> 850</a> ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type));
01810 </div>
01811 <div class="parity0 source"><a href="#l851" id="l851"> 851</a> break;
01812 </div>
01813 <div class="parity1 source"><a href="#l852" id="l852"> 852</a> }
01814 </div>
01815 <div class="parity0 source"><a href="#l853" id="l853"> 853</a>
01816 </div>
01817 <div class="parity1 source"><a href="#l854" id="l854"> 854</a> _WriteTransformation(attachment_full, pjoint->parent_to_joint_origin_transform);
01818 </div>
01819 <div class="parity0 source"><a href="#l855" id="l855"> 855</a> _WriteTransformation(childinfo.pnode, pjoint->parent_to_joint_origin_transform);
01820 </div>
01821 <div class="parity1 source"><a href="#l856" id="l856"> 856</a> _WriteTransformation(childinfo.pnode, geometry_origin_inv);
01822 </div>
01823 <div class="parity0 source"><a href="#l857" id="l857"> 857</a> out.listprocesseddofs.push_back(make_pair(index,string(childinfo.pnode->getSid())+string("/")+jointnodesid));
01824 </div>
01825 <div class="parity1 source"><a href="#l858" id="l858"> 858</a>
01826 </div>
01827 <div class="parity0 source"><a href="#l859" id="l859"> 859</a> }
01828 </div>
01829 <div class="parity1 source"><a href="#l860" id="l860"> 860</a>
01830 </div>
01831 <div class="parity0 source"><a href="#l861" id="l861"> 861</a> out.listusedlinks.push_back(make_pair(linkindex,linksid));
01832 </div>
01833 <div class="parity1 source"><a href="#l862" id="l862"> 862</a> out.plink = pdomlink;
01834 </div>
01835 <div class="parity0 source"><a href="#l863" id="l863"> 863</a> out.pnode = pnode;
01836 </div>
01837 <div class="parity1 source"><a href="#l864" id="l864"> 864</a> return out;
01838 </div>
01839 <div class="parity0 source"><a href="#l865" id="l865"> 865</a> }
01840 </div>
01841 <div class="parity1 source"><a href="#l866" id="l866"> 866</a>
01842 </div>
01843 <div class="parity0 source"><a href="#l867" id="l867"> 867</a> domGeometryRef _WriteGeometry(boost::shared_ptr<urdf::Geometry> geometry, const std::string& geometry_id)
01844 </div>
01845 <div class="parity1 source"><a href="#l868" id="l868"> 868</a> {
01846 </div>
01847 <div class="parity0 source"><a href="#l869" id="l869"> 869</a> domGeometryRef cgeometry = daeSafeCast<domGeometry>(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY));
01848 </div>
01849 <div class="parity1 source"><a href="#l870" id="l870"> 870</a> cgeometry->setId(geometry_id.c_str());
01850 </div>
01851 <div class="parity0 source"><a href="#l871" id="l871"> 871</a> switch (geometry->type) {
01852 </div>
01853 <div class="parity1 source"><a href="#l872" id="l872"> 872</a> case urdf::Geometry::MESH: {
01854 </div>
01855 <div class="parity0 source"><a href="#l873" id="l873"> 873</a> urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get();
01856 </div>
01857 <div class="parity1 source"><a href="#l874" id="l874"> 874</a> _loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale);
01858 </div>
01859 <div class="parity0 source"><a href="#l875" id="l875"> 875</a> break;
01860 </div>
01861 <div class="parity1 source"><a href="#l876" id="l876"> 876</a> }
01862 </div>
01863 <div class="parity0 source"><a href="#l877" id="l877"> 877</a> case urdf::Geometry::SPHERE: {
01864 </div>
01865 <div class="parity1 source"><a href="#l878" id="l878"> 878</a> ROS_WARN_STREAM(str(boost::format("cannot export sphere geometries yet! %s")%geometry_id));
01866 </div>
01867 <div class="parity0 source"><a href="#l879" id="l879"> 879</a> break;
01868 </div>
01869 <div class="parity1 source"><a href="#l880" id="l880"> 880</a> }
01870 </div>
01871 <div class="parity0 source"><a href="#l881" id="l881"> 881</a> case urdf::Geometry::BOX: {
01872 </div>
01873 <div class="parity1 source"><a href="#l882" id="l882"> 882</a> ROS_WARN_STREAM(str(boost::format("cannot export box geometries yet! %s")%geometry_id));
01874 </div>
01875 <div class="parity0 source"><a href="#l883" id="l883"> 883</a> break;
01876 </div>
01877 <div class="parity1 source"><a href="#l884" id="l884"> 884</a> }
01878 </div>
01879 <div class="parity0 source"><a href="#l885" id="l885"> 885</a> case urdf::Geometry::CYLINDER: {
01880 </div>
01881 <div class="parity1 source"><a href="#l886" id="l886"> 886</a> ROS_WARN_STREAM(str(boost::format("cannot export cylinder geometries yet! %s")%geometry_id));
01882 </div>
01883 <div class="parity0 source"><a href="#l887" id="l887"> 887</a> break;
01884 </div>
01885 <div class="parity1 source"><a href="#l888" id="l888"> 888</a> }
01886 </div>
01887 <div class="parity0 source"><a href="#l889" id="l889"> 889</a> default: {
01888 </div>
01889 <div class="parity1 source"><a href="#l890" id="l890"> 890</a> throw ColladaUrdfException(str(boost::format("undefined geometry type %d, name %s")%(int)geometry->type%geometry_id));
01890 </div>
01891 <div class="parity0 source"><a href="#l891" id="l891"> 891</a> }
01892 </div>
01893 <div class="parity1 source"><a href="#l892" id="l892"> 892</a> }
01894 </div>
01895 <div class="parity0 source"><a href="#l893" id="l893"> 893</a> return cgeometry;
01896 </div>
01897 <div class="parity1 source"><a href="#l894" id="l894"> 894</a> }
01898 </div>
01899 <div class="parity0 source"><a href="#l895" id="l895"> 895</a>
01900 </div>
01901 <div class="parity1 source"><a href="#l896" id="l896"> 896</a> void _WriteMaterial(const string& geometry_id, boost::shared_ptr<urdf::Material> material)
01902 </div>
01903 <div class="parity0 source"><a href="#l897" id="l897"> 897</a> {
01904 </div>
01905 <div class="parity1 source"><a href="#l898" id="l898"> 898</a> string effid = geometry_id+string("_eff");
01906 </div>
01907 <div class="parity0 source"><a href="#l899" id="l899"> 899</a> string matid = geometry_id+string("_mat");
01908 </div>
01909 <div class="parity1 source"><a href="#l900" id="l900"> 900</a> domMaterialRef pdommat = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
01910 </div>
01911 <div class="parity0 source"><a href="#l901" id="l901"> 901</a> pdommat->setId(matid.c_str());
01912 </div>
01913 <div class="parity1 source"><a href="#l902" id="l902"> 902</a> domInstance_effectRef pdominsteff = daeSafeCast<domInstance_effect>(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
01914 </div>
01915 <div class="parity0 source"><a href="#l903" id="l903"> 903</a> pdominsteff->setUrl((string("#")+effid).c_str());
01916 </div>
01917 <div class="parity1 source"><a href="#l904" id="l904"> 904</a>
01918 </div>
01919 <div class="parity0 source"><a href="#l905" id="l905"> 905</a> urdf::Color ambient, diffuse;
01920 </div>
01921 <div class="parity1 source"><a href="#l906" id="l906"> 906</a> ambient.init("0.1 0.1 0.1 0");
01922 </div>
01923 <div class="parity0 source"><a href="#l907" id="l907"> 907</a> diffuse.init("1 1 1 0");
01924 </div>
01925 <div class="parity1 source"><a href="#l908" id="l908"> 908</a>
01926 </div>
01927 <div class="parity0 source"><a href="#l909" id="l909"> 909</a> if( !!material ) {
01928 </div>
01929 <div class="parity1 source"><a href="#l910" id="l910"> 910</a> ambient.r = diffuse.r = material->color.r;
01930 </div>
01931 <div class="parity0 source"><a href="#l911" id="l911"> 911</a> ambient.g = diffuse.g = material->color.g;
01932 </div>
01933 <div class="parity1 source"><a href="#l912" id="l912"> 912</a> ambient.b = diffuse.b = material->color.b;
01934 </div>
01935 <div class="parity0 source"><a href="#l913" id="l913"> 913</a> ambient.a = diffuse.a = material->color.a;
01936 </div>
01937 <div class="parity1 source"><a href="#l914" id="l914"> 914</a> }
01938 </div>
01939 <div class="parity0 source"><a href="#l915" id="l915"> 915</a>
01940 </div>
01941 <div class="parity1 source"><a href="#l916" id="l916"> 916</a> domEffectRef effect = _WriteEffect(geometry_id, ambient, diffuse);
01942 </div>
01943 <div class="parity0 source"><a href="#l917" id="l917"> 917</a>
01944 </div>
01945 <div class="parity1 source"><a href="#l918" id="l918"> 918</a>
01946 </div>
01947 <div class="parity0 source"><a href="#l919" id="l919"> 919</a> domMaterialRef dommaterial = daeSafeCast<domMaterial>(_materialsLib->add(COLLADA_ELEMENT_MATERIAL));
01948 </div>
01949 <div class="parity1 source"><a href="#l920" id="l920"> 920</a> string material_id = geometry_id + string("_mat");
01950 </div>
01951 <div class="parity0 source"><a href="#l921" id="l921"> 921</a> dommaterial->setId(material_id.c_str());
01952 </div>
01953 <div class="parity1 source"><a href="#l922" id="l922"> 922</a> {
01954 </div>
01955 <div class="parity0 source"><a href="#l923" id="l923"> 923</a>
01956 </div>
01957 <div class="parity1 source"><a href="#l924" id="l924"> 924</a> domInstance_effectRef instance_effect = daeSafeCast<domInstance_effect>(dommaterial->add(COLLADA_ELEMENT_INSTANCE_EFFECT));
01958 </div>
01959 <div class="parity0 source"><a href="#l925" id="l925"> 925</a> string effect_id(effect->getId());
01960 </div>
01961 <div class="parity1 source"><a href="#l926" id="l926"> 926</a> instance_effect->setUrl((string("#") + effect_id).c_str());
01962 </div>
01963 <div class="parity0 source"><a href="#l927" id="l927"> 927</a> }
01964 </div>
01965 <div class="parity1 source"><a href="#l928" id="l928"> 928</a>
01966 </div>
01967 <div class="parity0 source"><a href="#l929" id="l929"> 929</a>
01968 </div>
01969 <div class="parity1 source"><a href="#l930" id="l930"> 930</a> domEffectRef pdomeff = _WriteEffect(effid, ambient, diffuse);
01970 </div>
01971 <div class="parity0 source"><a href="#l931" id="l931"> 931</a> }
01972 </div>
01973 <div class="parity1 source"><a href="#l932" id="l932"> 932</a>
01974 </div>
01975 <div class="parity0 source"><a href="#l933" id="l933"> 933</a> void _loadMesh(std::string const& filename, domGeometryRef pdomgeom, const urdf::Vector3& scale)
01976 </div>
01977 <div class="parity1 source"><a href="#l934" id="l934"> 934</a> {
01978 </div>
01979 <div class="parity0 source"><a href="#l935" id="l935"> 935</a> const aiScene* scene = _importer.ReadFile(filename, aiProcess_SortByPType|aiProcess_Triangulate);
01980 </div>
01981 <div class="parity1 source"><a href="#l936" id="l936"> 936</a> if( !scene ) {
01982 </div>
01983 <div class="parity0 source"><a href="#l937" id="l937"> 937</a> ROS_WARN("failed to load resource %s",filename.c_str());
01984 </div>
01985 <div class="parity1 source"><a href="#l938" id="l938"> 938</a> return;
01986 </div>
01987 <div class="parity0 source"><a href="#l939" id="l939"> 939</a> }
01988 </div>
01989 <div class="parity1 source"><a href="#l940" id="l940"> 940</a> if( !scene->mRootNode ) {
01990 </div>
01991 <div class="parity0 source"><a href="#l941" id="l941"> 941</a> ROS_WARN("resource %s has no data",filename.c_str());
01992 </div>
01993 <div class="parity1 source"><a href="#l942" id="l942"> 942</a> return;
01994 </div>
01995 <div class="parity0 source"><a href="#l943" id="l943"> 943</a> }
01996 </div>
01997 <div class="parity1 source"><a href="#l944" id="l944"> 944</a> if (!scene->HasMeshes()) {
01998 </div>
01999 <div class="parity0 source"><a href="#l945" id="l945"> 945</a> ROS_WARN_STREAM(str(boost::format("No meshes found in file %s")%filename));
02000 </div>
02001 <div class="parity1 source"><a href="#l946" id="l946"> 946</a> return;
02002 </div>
02003 <div class="parity0 source"><a href="#l947" id="l947"> 947</a> }
02004 </div>
02005 <div class="parity1 source"><a href="#l948" id="l948"> 948</a> domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
02006 </div>
02007 <div class="parity0 source"><a href="#l949" id="l949"> 949</a> domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
02008 </div>
02009 <div class="parity1 source"><a href="#l950" id="l950"> 950</a> domAccessorRef pacc;
02010 </div>
02011 <div class="parity0 source"><a href="#l951" id="l951"> 951</a> domFloat_arrayRef parray;
02012 </div>
02013 <div class="parity1 source"><a href="#l952" id="l952"> 952</a> {
02014 </div>
02015 <div class="parity0 source"><a href="#l953" id="l953"> 953</a> pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
02016 </div>
02017 <div class="parity1 source"><a href="#l954" id="l954"> 954</a>
02018 </div>
02019 <div class="parity0 source"><a href="#l955" id="l955"> 955</a> parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
02020 </div>
02021 <div class="parity1 source"><a href="#l956" id="l956"> 956</a> parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str());
02022 </div>
02023 <div class="parity0 source"><a href="#l957" id="l957"> 957</a> parray->setDigits(6);
02024 </div>
02025 <div class="parity1 source"><a href="#l958" id="l958"> 958</a>
02026 </div>
02027 <div class="parity0 source"><a href="#l959" id="l959"> 959</a> domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
02028 </div>
02029 <div class="parity1 source"><a href="#l960" id="l960"> 960</a> pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
02030 </div>
02031 <div class="parity0 source"><a href="#l961" id="l961"> 961</a> pacc->setSource(xsAnyURI(*parray, string("#")+string(parray->getID())));
02032 </div>
02033 <div class="parity1 source"><a href="#l962" id="l962"> 962</a> pacc->setStride(3);
02034 </div>
02035 <div class="parity0 source"><a href="#l963" id="l963"> 963</a>
02036 </div>
02037 <div class="parity1 source"><a href="#l964" id="l964"> 964</a> domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
02038 </div>
02039 <div class="parity0 source"><a href="#l965" id="l965"> 965</a> px->setName("X"); px->setType("float");
02040 </div>
02041 <div class="parity1 source"><a href="#l966" id="l966"> 966</a> domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
02042 </div>
02043 <div class="parity0 source"><a href="#l967" id="l967"> 967</a> py->setName("Y"); py->setType("float");
02044 </div>
02045 <div class="parity1 source"><a href="#l968" id="l968"> 968</a> domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
02046 </div>
02047 <div class="parity0 source"><a href="#l969" id="l969"> 969</a> pz->setName("Z"); pz->setType("float");
02048 </div>
02049 <div class="parity1 source"><a href="#l970" id="l970"> 970</a> }
02050 </div>
02051 <div class="parity0 source"><a href="#l971" id="l971"> 971</a> domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
02052 </div>
02053 <div class="parity1 source"><a href="#l972" id="l972"> 972</a> {
02054 </div>
02055 <div class="parity0 source"><a href="#l973" id="l973"> 973</a> pverts->setId("vertices");
02056 </div>
02057 <div class="parity1 source"><a href="#l974" id="l974"> 974</a> domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT));
02058 </div>
02059 <div class="parity0 source"><a href="#l975" id="l975"> 975</a> pvertinput->setSemantic("POSITION");
02060 </div>
02061 <div class="parity1 source"><a href="#l976" id="l976"> 976</a> pvertinput->setSource(domUrifragment(*pvertsource, string("#")+string(pvertsource->getID())));
02062 </div>
02063 <div class="parity0 source"><a href="#l977" id="l977"> 977</a> }
02064 </div>
02065 <div class="parity1 source"><a href="#l978" id="l978"> 978</a> _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(),scale);
02066 </div>
02067 <div class="parity0 source"><a href="#l979" id="l979"> 979</a> pacc->setCount(parray->getCount());
02068 </div>
02069 <div class="parity1 source"><a href="#l980" id="l980"> 980</a> }
02070 </div>
02071 <div class="parity0 source"><a href="#l981" id="l981"> 981</a>
02072 </div>
02073 <div class="parity1 source"><a href="#l982" id="l982"> 982</a> void _buildAiMesh(const aiScene* scene, aiNode* node, domMeshRef pdommesh, domFloat_arrayRef parray, const string& geomid, const urdf::Vector3& scale)
02074 </div>
02075 <div class="parity0 source"><a href="#l983" id="l983"> 983</a> {
02076 </div>
02077 <div class="parity1 source"><a href="#l984" id="l984"> 984</a> if( !node ) {
02078 </div>
02079 <div class="parity0 source"><a href="#l985" id="l985"> 985</a> return;
02080 </div>
02081 <div class="parity1 source"><a href="#l986" id="l986"> 986</a> }
02082 </div>
02083 <div class="parity0 source"><a href="#l987" id="l987"> 987</a> aiMatrix4x4 transform = node->mTransformation;
02084 </div>
02085 <div class="parity1 source"><a href="#l988" id="l988"> 988</a> aiNode *pnode = node->mParent;
02086 </div>
02087 <div class="parity0 source"><a href="#l989" id="l989"> 989</a> while (pnode) {
02088 </div>
02089 <div class="parity1 source"><a href="#l990" id="l990"> 990</a>
02090 </div>
02091 <div class="parity0 source"><a href="#l991" id="l991"> 991</a>
02092 </div>
02093 <div class="parity1 source"><a href="#l992" id="l992"> 992</a> if (pnode->mParent != NULL) {
02094 </div>
02095 <div class="parity0 source"><a href="#l993" id="l993"> 993</a> transform = pnode->mTransformation * transform;
02096 </div>
02097 <div class="parity1 source"><a href="#l994" id="l994"> 994</a> }
02098 </div>
02099 <div class="parity0 source"><a href="#l995" id="l995"> 995</a> pnode = pnode->mParent;
02100 </div>
02101 <div class="parity1 source"><a href="#l996" id="l996"> 996</a> }
02102 </div>
02103 <div class="parity0 source"><a href="#l997" id="l997"> 997</a>
02104 </div>
02105 <div class="parity1 source"><a href="#l998" id="l998"> 998</a> {
02106 </div>
02107 <div class="parity0 source"><a href="#l999" id="l999"> 999</a> uint32_t vertexOffset = parray->getCount();
02108 </div>
02109 <div class="parity1 source"><a href="#l1000" id="l1000"> 1000</a> uint32_t nTotalVertices=0;
02110 </div>
02111 <div class="parity0 source"><a href="#l1001" id="l1001"> 1001</a> for (uint32_t i = 0; i < node->mNumMeshes; i++) {
02112 </div>
02113 <div class="parity1 source"><a href="#l1002" id="l1002"> 1002</a> aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
02114 </div>
02115 <div class="parity0 source"><a href="#l1003" id="l1003"> 1003</a> nTotalVertices += input_mesh->mNumVertices;
02116 </div>
02117 <div class="parity1 source"><a href="#l1004" id="l1004"> 1004</a> }
02118 </div>
02119 <div class="parity0 source"><a href="#l1005" id="l1005"> 1005</a>
02120 </div>
02121 <div class="parity1 source"><a href="#l1006" id="l1006"> 1006</a> parray->getValue().grow(parray->getCount()+nTotalVertices*3);
02122 </div>
02123 <div class="parity0 source"><a href="#l1007" id="l1007"> 1007</a> parray->setCount(parray->getCount()+nTotalVertices);
02124 </div>
02125 <div class="parity1 source"><a href="#l1008" id="l1008"> 1008</a>
02126 </div>
02127 <div class="parity0 source"><a href="#l1009" id="l1009"> 1009</a> for (uint32_t i = 0; i < node->mNumMeshes; i++) {
02128 </div>
02129 <div class="parity1 source"><a href="#l1010" id="l1010"> 1010</a> aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
02130 </div>
02131 <div class="parity0 source"><a href="#l1011" id="l1011"> 1011</a> for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) {
02132 </div>
02133 <div class="parity1 source"><a href="#l1012" id="l1012"> 1012</a> aiVector3D p = input_mesh->mVertices[j];
02134 </div>
02135 <div class="parity0 source"><a href="#l1013" id="l1013"> 1013</a> p *= transform;
02136 </div>
02137 <div class="parity1 source"><a href="#l1014" id="l1014"> 1014</a> parray->getValue().append(p.x*scale.x);
02138 </div>
02139 <div class="parity0 source"><a href="#l1015" id="l1015"> 1015</a> parray->getValue().append(p.y*scale.y);
02140 </div>
02141 <div class="parity1 source"><a href="#l1016" id="l1016"> 1016</a> parray->getValue().append(p.z*scale.z);
02142 </div>
02143 <div class="parity0 source"><a href="#l1017" id="l1017"> 1017</a> }
02144 </div>
02145 <div class="parity1 source"><a href="#l1018" id="l1018"> 1018</a> }
02146 </div>
02147 <div class="parity0 source"><a href="#l1019" id="l1019"> 1019</a>
02148 </div>
02149 <div class="parity1 source"><a href="#l1020" id="l1020"> 1020</a>
02150 </div>
02151 <div class="parity0 source"><a href="#l1021" id="l1021"> 1021</a>
02152 </div>
02153 <div class="parity1 source"><a href="#l1022" id="l1022"> 1022</a> vector<int> triangleindices, otherindices;
02154 </div>
02155 <div class="parity0 source"><a href="#l1023" id="l1023"> 1023</a> int nNumOtherPrimitives = 0;
02156 </div>
02157 <div class="parity1 source"><a href="#l1024" id="l1024"> 1024</a> for (uint32_t i = 0; i < node->mNumMeshes; i++) {
02158 </div>
02159 <div class="parity0 source"><a href="#l1025" id="l1025"> 1025</a> aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
02160 </div>
02161 <div class="parity1 source"><a href="#l1026" id="l1026"> 1026</a> uint32_t indexCount = 0, otherIndexCount = 0;
02162 </div>
02163 <div class="parity0 source"><a href="#l1027" id="l1027"> 1027</a> for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
02164 </div>
02165 <div class="parity1 source"><a href="#l1028" id="l1028"> 1028</a> aiFace& face = input_mesh->mFaces[j];
02166 </div>
02167 <div class="parity0 source"><a href="#l1029" id="l1029"> 1029</a> if( face.mNumIndices == 3 ) {
02168 </div>
02169 <div class="parity1 source"><a href="#l1030" id="l1030"> 1030</a> indexCount += face.mNumIndices;
02170 </div>
02171 <div class="parity0 source"><a href="#l1031" id="l1031"> 1031</a> }
02172 </div>
02173 <div class="parity1 source"><a href="#l1032" id="l1032"> 1032</a> else {
02174 </div>
02175 <div class="parity0 source"><a href="#l1033" id="l1033"> 1033</a> otherIndexCount += face.mNumIndices;
02176 </div>
02177 <div class="parity1 source"><a href="#l1034" id="l1034"> 1034</a> }
02178 </div>
02179 <div class="parity0 source"><a href="#l1035" id="l1035"> 1035</a> }
02180 </div>
02181 <div class="parity1 source"><a href="#l1036" id="l1036"> 1036</a> triangleindices.reserve(triangleindices.size()+indexCount);
02182 </div>
02183 <div class="parity0 source"><a href="#l1037" id="l1037"> 1037</a> otherindices.reserve(otherindices.size()+otherIndexCount);
02184 </div>
02185 <div class="parity1 source"><a href="#l1038" id="l1038"> 1038</a> for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
02186 </div>
02187 <div class="parity0 source"><a href="#l1039" id="l1039"> 1039</a> aiFace& face = input_mesh->mFaces[j];
02188 </div>
02189 <div class="parity1 source"><a href="#l1040" id="l1040"> 1040</a> if( face.mNumIndices == 3 ) {
02190 </div>
02191 <div class="parity0 source"><a href="#l1041" id="l1041"> 1041</a> triangleindices.push_back(vertexOffset+face.mIndices[0]);
02192 </div>
02193 <div class="parity1 source"><a href="#l1042" id="l1042"> 1042</a> triangleindices.push_back(vertexOffset+face.mIndices[1]);
02194 </div>
02195 <div class="parity0 source"><a href="#l1043" id="l1043"> 1043</a> triangleindices.push_back(vertexOffset+face.mIndices[2]);
02196 </div>
02197 <div class="parity1 source"><a href="#l1044" id="l1044"> 1044</a> }
02198 </div>
02199 <div class="parity0 source"><a href="#l1045" id="l1045"> 1045</a> else {
02200 </div>
02201 <div class="parity1 source"><a href="#l1046" id="l1046"> 1046</a> for (uint32_t k = 0; k < face.mNumIndices; ++k) {
02202 </div>
02203 <div class="parity0 source"><a href="#l1047" id="l1047"> 1047</a> otherindices.push_back(face.mIndices[k]+vertexOffset);
02204 </div>
02205 <div class="parity1 source"><a href="#l1048" id="l1048"> 1048</a> }
02206 </div>
02207 <div class="parity0 source"><a href="#l1049" id="l1049"> 1049</a> nNumOtherPrimitives++;
02208 </div>
02209 <div class="parity1 source"><a href="#l1050" id="l1050"> 1050</a> }
02210 </div>
02211 <div class="parity0 source"><a href="#l1051" id="l1051"> 1051</a> }
02212 </div>
02213 <div class="parity1 source"><a href="#l1052" id="l1052"> 1052</a> vertexOffset += input_mesh->mNumVertices;
02214 </div>
02215 <div class="parity0 source"><a href="#l1053" id="l1053"> 1053</a> }
02216 </div>
02217 <div class="parity1 source"><a href="#l1054" id="l1054"> 1054</a>
02218 </div>
02219 <div class="parity0 source"><a href="#l1055" id="l1055"> 1055</a> if( triangleindices.size() > 0 ) {
02220 </div>
02221 <div class="parity1 source"><a href="#l1056" id="l1056"> 1056</a> domTrianglesRef ptris = daeSafeCast<domTriangles>(pdommesh->add(COLLADA_ELEMENT_TRIANGLES));
02222 </div>
02223 <div class="parity0 source"><a href="#l1057" id="l1057"> 1057</a> ptris->setCount(triangleindices.size()/3);
02224 </div>
02225 <div class="parity1 source"><a href="#l1058" id="l1058"> 1058</a> ptris->setMaterial("mat0");
02226 </div>
02227 <div class="parity0 source"><a href="#l1059" id="l1059"> 1059</a> domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
02228 </div>
02229 <div class="parity1 source"><a href="#l1060" id="l1060"> 1060</a> pvertoffset->setSemantic("VERTEX");
02230 </div>
02231 <div class="parity0 source"><a href="#l1061" id="l1061"> 1061</a> pvertoffset->setOffset(0);
02232 </div>
02233 <div class="parity1 source"><a href="#l1062" id="l1062"> 1062</a> pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid)));
02234 </div>
02235 <div class="parity0 source"><a href="#l1063" id="l1063"> 1063</a> domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
02236 </div>
02237 <div class="parity1 source"><a href="#l1064" id="l1064"> 1064</a> pindices->getValue().setCount(triangleindices.size());
02238 </div>
02239 <div class="parity0 source"><a href="#l1065" id="l1065"> 1065</a> for(size_t ind = 0; ind < triangleindices.size(); ++ind) {
02240 </div>
02241 <div class="parity1 source"><a href="#l1066" id="l1066"> 1066</a> pindices->getValue()[ind] = triangleindices[ind];
02242 </div>
02243 <div class="parity0 source"><a href="#l1067" id="l1067"> 1067</a> }
02244 </div>
02245 <div class="parity1 source"><a href="#l1068" id="l1068"> 1068</a> }
02246 </div>
02247 <div class="parity0 source"><a href="#l1069" id="l1069"> 1069</a>
02248 </div>
02249 <div class="parity1 source"><a href="#l1070" id="l1070"> 1070</a> if( nNumOtherPrimitives > 0 ) {
02250 </div>
02251 <div class="parity0 source"><a href="#l1071" id="l1071"> 1071</a> domPolylistRef ptris = daeSafeCast<domPolylist>(pdommesh->add(COLLADA_ELEMENT_POLYLIST));
02252 </div>
02253 <div class="parity1 source"><a href="#l1072" id="l1072"> 1072</a> ptris->setCount(nNumOtherPrimitives);
02254 </div>
02255 <div class="parity0 source"><a href="#l1073" id="l1073"> 1073</a> ptris->setMaterial("mat0");
02256 </div>
02257 <div class="parity1 source"><a href="#l1074" id="l1074"> 1074</a> domInput_local_offsetRef pvertoffset = daeSafeCast<domInput_local_offset>(ptris->add(COLLADA_ELEMENT_INPUT));
02258 </div>
02259 <div class="parity0 source"><a href="#l1075" id="l1075"> 1075</a> pvertoffset->setSemantic("VERTEX");
02260 </div>
02261 <div class="parity1 source"><a href="#l1076" id="l1076"> 1076</a> pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid)));
02262 </div>
02263 <div class="parity0 source"><a href="#l1077" id="l1077"> 1077</a> domPRef pindices = daeSafeCast<domP>(ptris->add(COLLADA_ELEMENT_P));
02264 </div>
02265 <div class="parity1 source"><a href="#l1078" id="l1078"> 1078</a> pindices->getValue().setCount(otherindices.size());
02266 </div>
02267 <div class="parity0 source"><a href="#l1079" id="l1079"> 1079</a> for(size_t ind = 0; ind < otherindices.size(); ++ind) {
02268 </div>
02269 <div class="parity1 source"><a href="#l1080" id="l1080"> 1080</a> pindices->getValue()[ind] = otherindices[ind];
02270 </div>
02271 <div class="parity0 source"><a href="#l1081" id="l1081"> 1081</a> }
02272 </div>
02273 <div class="parity1 source"><a href="#l1082" id="l1082"> 1082</a>
02274 </div>
02275 <div class="parity0 source"><a href="#l1083" id="l1083"> 1083</a> domPolylist::domVcountRef pcount = daeSafeCast<domPolylist::domVcount>(ptris->add(COLLADA_ELEMENT_VCOUNT));
02276 </div>
02277 <div class="parity1 source"><a href="#l1084" id="l1084"> 1084</a> pcount->getValue().setCount(nNumOtherPrimitives);
02278 </div>
02279 <div class="parity0 source"><a href="#l1085" id="l1085"> 1085</a> uint32_t offset = 0;
02280 </div>
02281 <div class="parity1 source"><a href="#l1086" id="l1086"> 1086</a> for (uint32_t i = 0; i < node->mNumMeshes; i++) {
02282 </div>
02283 <div class="parity0 source"><a href="#l1087" id="l1087"> 1087</a> aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
02284 </div>
02285 <div class="parity1 source"><a href="#l1088" id="l1088"> 1088</a> for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) {
02286 </div>
02287 <div class="parity0 source"><a href="#l1089" id="l1089"> 1089</a> aiFace& face = input_mesh->mFaces[j];
02288 </div>
02289 <div class="parity1 source"><a href="#l1090" id="l1090"> 1090</a> if( face.mNumIndices > 3 ) {
02290 </div>
02291 <div class="parity0 source"><a href="#l1091" id="l1091"> 1091</a> pcount->getValue()[offset++] = face.mNumIndices;
02292 </div>
02293 <div class="parity1 source"><a href="#l1092" id="l1092"> 1092</a> }
02294 </div>
02295 <div class="parity0 source"><a href="#l1093" id="l1093"> 1093</a> }
02296 </div>
02297 <div class="parity1 source"><a href="#l1094" id="l1094"> 1094</a> }
02298 </div>
02299 <div class="parity0 source"><a href="#l1095" id="l1095"> 1095</a> }
02300 </div>
02301 <div class="parity1 source"><a href="#l1096" id="l1096"> 1096</a> }
02302 </div>
02303 <div class="parity0 source"><a href="#l1097" id="l1097"> 1097</a>
02304 </div>
02305 <div class="parity1 source"><a href="#l1098" id="l1098"> 1098</a> for (uint32_t i=0; i < node->mNumChildren; ++i) {
02306 </div>
02307 <div class="parity0 source"><a href="#l1099" id="l1099"> 1099</a> _buildAiMesh(scene, node->mChildren[i], pdommesh,parray,geomid,scale);
02308 </div>
02309 <div class="parity1 source"><a href="#l1100" id="l1100"> 1100</a> }
02310 </div>
02311 <div class="parity0 source"><a href="#l1101" id="l1101"> 1101</a> }
02312 </div>
02313 <div class="parity1 source"><a href="#l1102" id="l1102"> 1102</a>
02314 </div>
02315 <div class="parity0 source"><a href="#l1103" id="l1103"> 1103</a>
02316 </div>
02317 <div class="parity1 source"><a href="#l1104" id="l1104"> 1104</a> domEffectRef _WriteEffect(std::string const& effect_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse)
02318 </div>
02319 <div class="parity0 source"><a href="#l1105" id="l1105"> 1105</a> {
02320 </div>
02321 <div class="parity1 source"><a href="#l1106" id="l1106"> 1106</a>
02322 </div>
02323 <div class="parity0 source"><a href="#l1107" id="l1107"> 1107</a> domEffectRef effect = daeSafeCast<domEffect>(_effectsLib->add(COLLADA_ELEMENT_EFFECT));
02324 </div>
02325 <div class="parity1 source"><a href="#l1108" id="l1108"> 1108</a> effect->setId(effect_id.c_str());
02326 </div>
02327 <div class="parity0 source"><a href="#l1109" id="l1109"> 1109</a> {
02328 </div>
02329 <div class="parity1 source"><a href="#l1110" id="l1110"> 1110</a>
02330 </div>
02331 <div class="parity0 source"><a href="#l1111" id="l1111"> 1111</a> domProfile_commonRef profile = daeSafeCast<domProfile_common>(effect->add(COLLADA_ELEMENT_PROFILE_COMMON));
02332 </div>
02333 <div class="parity1 source"><a href="#l1112" id="l1112"> 1112</a> {
02334 </div>
02335 <div class="parity0 source"><a href="#l1113" id="l1113"> 1113</a>
02336 </div>
02337 <div class="parity1 source"><a href="#l1114" id="l1114"> 1114</a> domProfile_common::domTechniqueRef technique = daeSafeCast<domProfile_common::domTechnique>(profile->add(COLLADA_ELEMENT_TECHNIQUE));
02338 </div>
02339 <div class="parity0 source"><a href="#l1115" id="l1115"> 1115</a> {
02340 </div>
02341 <div class="parity1 source"><a href="#l1116" id="l1116"> 1116</a>
02342 </div>
02343 <div class="parity0 source"><a href="#l1117" id="l1117"> 1117</a> domProfile_common::domTechnique::domPhongRef phong = daeSafeCast<domProfile_common::domTechnique::domPhong>(technique->add(COLLADA_ELEMENT_PHONG));
02344 </div>
02345 <div class="parity1 source"><a href="#l1118" id="l1118"> 1118</a> {
02346 </div>
02347 <div class="parity0 source"><a href="#l1119" id="l1119"> 1119</a>
02348 </div>
02349 <div class="parity1 source"><a href="#l1120" id="l1120"> 1120</a> domFx_common_color_or_textureRef ambient = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_AMBIENT));
02350 </div>
02351 <div class="parity0 source"><a href="#l1121" id="l1121"> 1121</a> {
02352 </div>
02353 <div class="parity1 source"><a href="#l1122" id="l1122"> 1122</a>
02354 </div>
02355 <div class="parity0 source"><a href="#l1123" id="l1123"> 1123</a> domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast<domFx_common_color_or_texture::domColor>(ambient->add(COLLADA_ELEMENT_COLOR));
02356 </div>
02357 <div class="parity1 source"><a href="#l1124" id="l1124"> 1124</a> ambient_color->getValue().setCount(4);
02358 </div>
02359 <div class="parity0 source"><a href="#l1125" id="l1125"> 1125</a> ambient_color->getValue()[0] = color_ambient.r;
02360 </div>
02361 <div class="parity1 source"><a href="#l1126" id="l1126"> 1126</a> ambient_color->getValue()[1] = color_ambient.g;
02362 </div>
02363 <div class="parity0 source"><a href="#l1127" id="l1127"> 1127</a> ambient_color->getValue()[2] = color_ambient.b;
02364 </div>
02365 <div class="parity1 source"><a href="#l1128" id="l1128"> 1128</a> ambient_color->getValue()[3] = color_ambient.a;
02366 </div>
02367 <div class="parity0 source"><a href="#l1129" id="l1129"> 1129</a>
02368 </div>
02369 <div class="parity1 source"><a href="#l1130" id="l1130"> 1130</a> }
02370 </div>
02371 <div class="parity0 source"><a href="#l1131" id="l1131"> 1131</a>
02372 </div>
02373 <div class="parity1 source"><a href="#l1132" id="l1132"> 1132</a>
02374 </div>
02375 <div class="parity0 source"><a href="#l1133" id="l1133"> 1133</a>
02376 </div>
02377 <div class="parity1 source"><a href="#l1134" id="l1134"> 1134</a> domFx_common_color_or_textureRef diffuse = daeSafeCast<domFx_common_color_or_texture>(phong->add(COLLADA_ELEMENT_DIFFUSE));
02378 </div>
02379 <div class="parity0 source"><a href="#l1135" id="l1135"> 1135</a> {
02380 </div>
02381 <div class="parity1 source"><a href="#l1136" id="l1136"> 1136</a>
02382 </div>
02383 <div class="parity0 source"><a href="#l1137" id="l1137"> 1137</a> domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast<domFx_common_color_or_texture::domColor>(diffuse->add(COLLADA_ELEMENT_COLOR));
02384 </div>
02385 <div class="parity1 source"><a href="#l1138" id="l1138"> 1138</a> diffuse_color->getValue().setCount(4);
02386 </div>
02387 <div class="parity0 source"><a href="#l1139" id="l1139"> 1139</a> diffuse_color->getValue()[0] = color_diffuse.r;
02388 </div>
02389 <div class="parity1 source"><a href="#l1140" id="l1140"> 1140</a> diffuse_color->getValue()[1] = color_diffuse.g;
02390 </div>
02391 <div class="parity0 source"><a href="#l1141" id="l1141"> 1141</a> diffuse_color->getValue()[2] = color_diffuse.b;
02392 </div>
02393 <div class="parity1 source"><a href="#l1142" id="l1142"> 1142</a> diffuse_color->getValue()[3] = color_diffuse.a;
02394 </div>
02395 <div class="parity0 source"><a href="#l1143" id="l1143"> 1143</a>
02396 </div>
02397 <div class="parity1 source"><a href="#l1144" id="l1144"> 1144</a> }
02398 </div>
02399 <div class="parity0 source"><a href="#l1145" id="l1145"> 1145</a>
02400 </div>
02401 <div class="parity1 source"><a href="#l1146" id="l1146"> 1146</a> }
02402 </div>
02403 <div class="parity0 source"><a href="#l1147" id="l1147"> 1147</a>
02404 </div>
02405 <div class="parity1 source"><a href="#l1148" id="l1148"> 1148</a> }
02406 </div>
02407 <div class="parity0 source"><a href="#l1149" id="l1149"> 1149</a>
02408 </div>
02409 <div class="parity1 source"><a href="#l1150" id="l1150"> 1150</a> }
02410 </div>
02411 <div class="parity0 source"><a href="#l1151" id="l1151"> 1151</a>
02412 </div>
02413 <div class="parity1 source"><a href="#l1152" id="l1152"> 1152</a> }
02414 </div>
02415 <div class="parity0 source"><a href="#l1153" id="l1153"> 1153</a>
02416 </div>
02417 <div class="parity1 source"><a href="#l1154" id="l1154"> 1154</a>
02418 </div>
02419 <div class="parity0 source"><a href="#l1155" id="l1155"> 1155</a> return effect;
02420 </div>
02421 <div class="parity1 source"><a href="#l1156" id="l1156"> 1156</a> }
02422 </div>
02423 <div class="parity0 source"><a href="#l1157" id="l1157"> 1157</a>
02424 </div>
02425 <div class="parity1 source"><a href="#l1158" id="l1158"> 1158</a>
02426 </div>
02427 <div class="parity0 source"><a href="#l1159" id="l1159"> 1159</a>
02428 </div>
02429 <div class="parity1 source"><a href="#l1160" id="l1160"> 1160</a>
02430 </div>
02431 <div class="parity0 source"><a href="#l1161" id="l1161"> 1161</a> void _WriteTransformation(daeElementRef pelt, const urdf::Pose& t)
02432 </div>
02433 <div class="parity1 source"><a href="#l1162" id="l1162"> 1162</a> {
02434 </div>
02435 <div class="parity0 source"><a href="#l1163" id="l1163"> 1163</a> domRotateRef prot = daeSafeCast<domRotate>(pelt->add(COLLADA_ELEMENT_ROTATE,0));
02436 </div>
02437 <div class="parity1 source"><a href="#l1164" id="l1164"> 1164</a> domTranslateRef ptrans = daeSafeCast<domTranslate>(pelt->add(COLLADA_ELEMENT_TRANSLATE,0));
02438 </div>
02439 <div class="parity0 source"><a href="#l1165" id="l1165"> 1165</a> ptrans->getValue().setCount(3);
02440 </div>
02441 <div class="parity1 source"><a href="#l1166" id="l1166"> 1166</a> ptrans->getValue()[0] = t.position.x;
02442 </div>
02443 <div class="parity0 source"><a href="#l1167" id="l1167"> 1167</a> ptrans->getValue()[1] = t.position.y;
02444 </div>
02445 <div class="parity1 source"><a href="#l1168" id="l1168"> 1168</a> ptrans->getValue()[2] = t.position.z;
02446 </div>
02447 <div class="parity0 source"><a href="#l1169" id="l1169"> 1169</a>
02448 </div>
02449 <div class="parity1 source"><a href="#l1170" id="l1170"> 1170</a> prot->getValue().setCount(4);
02450 </div>
02451 <div class="parity0 source"><a href="#l1171" id="l1171"> 1171</a>
02452 </div>
02453 <div class="parity1 source"><a href="#l1172" id="l1172"> 1172</a> double sinang = t.rotation.x*t.rotation.x+t.rotation.y*t.rotation.y+t.rotation.z*t.rotation.z;
02454 </div>
02455 <div class="parity0 source"><a href="#l1173" id="l1173"> 1173</a> if( std::fabs(sinang) < 1e-10 ) {
02456 </div>
02457 <div class="parity1 source"><a href="#l1174" id="l1174"> 1174</a> prot->getValue()[0] = 1;
02458 </div>
02459 <div class="parity0 source"><a href="#l1175" id="l1175"> 1175</a> prot->getValue()[1] = 0;
02460 </div>
02461 <div class="parity1 source"><a href="#l1176" id="l1176"> 1176</a> prot->getValue()[2] = 0;
02462 </div>
02463 <div class="parity0 source"><a href="#l1177" id="l1177"> 1177</a> prot->getValue()[3] = 0;
02464 </div>
02465 <div class="parity1 source"><a href="#l1178" id="l1178"> 1178</a> }
02466 </div>
02467 <div class="parity0 source"><a href="#l1179" id="l1179"> 1179</a> else {
02468 </div>
02469 <div class="parity1 source"><a href="#l1180" id="l1180"> 1180</a> urdf::Rotation quat;
02470 </div>
02471 <div class="parity0 source"><a href="#l1181" id="l1181"> 1181</a> if( t.rotation.w < 0 ) {
02472 </div>
02473 <div class="parity1 source"><a href="#l1182" id="l1182"> 1182</a> quat.x = -t.rotation.x;
02474 </div>
02475 <div class="parity0 source"><a href="#l1183" id="l1183"> 1183</a> quat.y = -t.rotation.y;
02476 </div>
02477 <div class="parity1 source"><a href="#l1184" id="l1184"> 1184</a> quat.z = -t.rotation.z;
02478 </div>
02479 <div class="parity0 source"><a href="#l1185" id="l1185"> 1185</a> quat.w = -t.rotation.w;
02480 </div>
02481 <div class="parity1 source"><a href="#l1186" id="l1186"> 1186</a> }
02482 </div>
02483 <div class="parity0 source"><a href="#l1187" id="l1187"> 1187</a> else {
02484 </div>
02485 <div class="parity1 source"><a href="#l1188" id="l1188"> 1188</a> quat = t.rotation;
02486 </div>
02487 <div class="parity0 source"><a href="#l1189" id="l1189"> 1189</a> }
02488 </div>
02489 <div class="parity1 source"><a href="#l1190" id="l1190"> 1190</a> sinang = std::sqrt(sinang);
02490 </div>
02491 <div class="parity0 source"><a href="#l1191" id="l1191"> 1191</a> prot->getValue()[0] = quat.x/sinang;
02492 </div>
02493 <div class="parity1 source"><a href="#l1192" id="l1192"> 1192</a> prot->getValue()[1] = quat.y/sinang;
02494 </div>
02495 <div class="parity0 source"><a href="#l1193" id="l1193"> 1193</a> prot->getValue()[2] = quat.z/sinang;
02496 </div>
02497 <div class="parity1 source"><a href="#l1194" id="l1194"> 1194</a> prot->getValue()[3] = angles::to_degrees(2.0*std::atan2(sinang,quat.w));
02498 </div>
02499 <div class="parity0 source"><a href="#l1195" id="l1195"> 1195</a> }
02500 </div>
02501 <div class="parity1 source"><a href="#l1196" id="l1196"> 1196</a> }
02502 </div>
02503 <div class="parity0 source"><a href="#l1197" id="l1197"> 1197</a>
02504 </div>
02505 <div class="parity1 source"><a href="#l1198" id="l1198"> 1198</a>
02506 </div>
02507 <div class="parity0 source"><a href="#l1199" id="l1199"> 1199</a> void _WriteBindingsInstance_kinematics_scene()
02508 </div>
02509 <div class="parity1 source"><a href="#l1200" id="l1200"> 1200</a> {
02510 </div>
02511 <div class="parity0 source"><a href="#l1201" id="l1201"> 1201</a> FOREACHC(it, _iasout->vkinematicsbindings) {
02512 </div>
02513 <div class="parity1 source"><a href="#l1202" id="l1202"> 1202</a> domBind_kinematics_modelRef pmodelbind = daeSafeCast<domBind_kinematics_model>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL));
02514 </div>
02515 <div class="parity0 source"><a href="#l1203" id="l1203"> 1203</a> pmodelbind->setNode(it->second.c_str());
02516 </div>
02517 <div class="parity1 source"><a href="#l1204" id="l1204"> 1204</a> daeSafeCast<domCommon_param>(pmodelbind->add(COLLADA_ELEMENT_PARAM))->setValue(it->first.c_str());
02518 </div>
02519 <div class="parity0 source"><a href="#l1205" id="l1205"> 1205</a> }
02520 </div>
02521 <div class="parity1 source"><a href="#l1206" id="l1206"> 1206</a> FOREACHC(it, _iasout->vaxissids) {
02522 </div>
02523 <div class="parity0 source"><a href="#l1207" id="l1207"> 1207</a> domBind_joint_axisRef pjointbind = daeSafeCast<domBind_joint_axis>(_scene.kiscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS));
02524 </div>
02525 <div class="parity1 source"><a href="#l1208" id="l1208"> 1208</a> pjointbind->setTarget(it->jointnodesid.c_str());
02526 </div>
02527 <div class="parity0 source"><a href="#l1209" id="l1209"> 1209</a> daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_AXIS)->add(COLLADA_TYPE_PARAM))->setValue(it->axissid.c_str());
02528 </div>
02529 <div class="parity1 source"><a href="#l1210" id="l1210"> 1210</a> daeSafeCast<domCommon_param>(pjointbind->add(COLLADA_ELEMENT_VALUE)->add(COLLADA_TYPE_PARAM))->setValue(it->valuesid.c_str());
02530 </div>
02531 <div class="parity0 source"><a href="#l1211" id="l1211"> 1211</a> }
02532 </div>
02533 <div class="parity1 source"><a href="#l1212" id="l1212"> 1212</a> }
02534 </div>
02535 <div class="parity0 source"><a href="#l1213" id="l1213"> 1213</a>
02536 </div>
02537 <div class="parity1 source"><a href="#l1214" id="l1214"> 1214</a> private:
02538 </div>
02539 <div class="parity0 source"><a href="#l1215" id="l1215"> 1215</a> static urdf::Vector3 _poseMult(const urdf::Pose& p, const urdf::Vector3& v)
02540 </div>
02541 <div class="parity1 source"><a href="#l1216" id="l1216"> 1216</a> {
02542 </div>
02543 <div class="parity0 source"><a href="#l1217" id="l1217"> 1217</a> double ww = 2 * p.rotation.x * p.rotation.x;
02544 </div>
02545 <div class="parity1 source"><a href="#l1218" id="l1218"> 1218</a> double wx = 2 * p.rotation.x * p.rotation.y;
02546 </div>
02547 <div class="parity0 source"><a href="#l1219" id="l1219"> 1219</a> double wy = 2 * p.rotation.x * p.rotation.z;
02548 </div>
02549 <div class="parity1 source"><a href="#l1220" id="l1220"> 1220</a> double wz = 2 * p.rotation.x * p.rotation.w;
02550 </div>
02551 <div class="parity0 source"><a href="#l1221" id="l1221"> 1221</a> double xx = 2 * p.rotation.y * p.rotation.y;
02552 </div>
02553 <div class="parity1 source"><a href="#l1222" id="l1222"> 1222</a> double xy = 2 * p.rotation.y * p.rotation.z;
02554 </div>
02555 <div class="parity0 source"><a href="#l1223" id="l1223"> 1223</a> double xz = 2 * p.rotation.y * p.rotation.w;
02556 </div>
02557 <div class="parity1 source"><a href="#l1224" id="l1224"> 1224</a> double yy = 2 * p.rotation.z * p.rotation.z;
02558 </div>
02559 <div class="parity0 source"><a href="#l1225" id="l1225"> 1225</a> double yz = 2 * p.rotation.z * p.rotation.w;
02560 </div>
02561 <div class="parity1 source"><a href="#l1226" id="l1226"> 1226</a> urdf::Vector3 vnew;
02562 </div>
02563 <div class="parity0 source"><a href="#l1227" id="l1227"> 1227</a> vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x;
02564 </div>
02565 <div class="parity1 source"><a href="#l1228" id="l1228"> 1228</a> vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y;
02566 </div>
02567 <div class="parity0 source"><a href="#l1229" id="l1229"> 1229</a> vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z;
02568 </div>
02569 <div class="parity1 source"><a href="#l1230" id="l1230"> 1230</a> return vnew;
02570 </div>
02571 <div class="parity0 source"><a href="#l1231" id="l1231"> 1231</a> }
02572 </div>
02573 <div class="parity1 source"><a href="#l1232" id="l1232"> 1232</a>
02574 </div>
02575 <div class="parity0 source"><a href="#l1233" id="l1233"> 1233</a> static urdf::Pose _poseInverse(const urdf::Pose& p)
02576 </div>
02577 <div class="parity1 source"><a href="#l1234" id="l1234"> 1234</a> {
02578 </div>
02579 <div class="parity0 source"><a href="#l1235" id="l1235"> 1235</a> urdf::Pose pinv;
02580 </div>
02581 <div class="parity1 source"><a href="#l1236" id="l1236"> 1236</a> pinv.rotation.x = -p.rotation.x;
02582 </div>
02583 <div class="parity0 source"><a href="#l1237" id="l1237"> 1237</a> pinv.rotation.y = -p.rotation.y;
02584 </div>
02585 <div class="parity1 source"><a href="#l1238" id="l1238"> 1238</a> pinv.rotation.z = -p.rotation.z;
02586 </div>
02587 <div class="parity0 source"><a href="#l1239" id="l1239"> 1239</a> pinv.rotation.w = p.rotation.w;
02588 </div>
02589 <div class="parity1 source"><a href="#l1240" id="l1240"> 1240</a> urdf::Vector3 t = _poseMult(pinv,p.position);
02590 </div>
02591 <div class="parity0 source"><a href="#l1241" id="l1241"> 1241</a> pinv.position.x = -t.x;
02592 </div>
02593 <div class="parity1 source"><a href="#l1242" id="l1242"> 1242</a> pinv.position.y = -t.y;
02594 </div>
02595 <div class="parity0 source"><a href="#l1243" id="l1243"> 1243</a> pinv.position.z = -t.z;
02596 </div>
02597 <div class="parity1 source"><a href="#l1244" id="l1244"> 1244</a> return pinv;
02598 </div>
02599 <div class="parity0 source"><a href="#l1245" id="l1245"> 1245</a> }
02600 </div>
02601 <div class="parity1 source"><a href="#l1246" id="l1246"> 1246</a>
02602 </div>
02603 <div class="parity0 source"><a href="#l1247" id="l1247"> 1247</a>
02604 </div>
02605 <div class="parity1 source"><a href="#l1248" id="l1248"> 1248</a> static std::string _ComputeId(const std::string& name)
02606 </div>
02607 <div class="parity0 source"><a href="#l1249" id="l1249"> 1249</a> {
02608 </div>
02609 <div class="parity1 source"><a href="#l1250" id="l1250"> 1250</a> std::string newname = name;
02610 </div>
02611 <div class="parity0 source"><a href="#l1251" id="l1251"> 1251</a> for(size_t i = 0; i < newname.size(); ++i) {
02612 </div>
02613 <div class="parity1 source"><a href="#l1252" id="l1252"> 1252</a> if( newname[i] == '/' || newname[i] == ' ' || newname[i] == '.' ) {
02614 </div>
02615 <div class="parity0 source"><a href="#l1253" id="l1253"> 1253</a> newname[i] = '_';
02616 </div>
02617 <div class="parity1 source"><a href="#l1254" id="l1254"> 1254</a> }
02618 </div>
02619 <div class="parity0 source"><a href="#l1255" id="l1255"> 1255</a> }
02620 </div>
02621 <div class="parity1 source"><a href="#l1256" id="l1256"> 1256</a> return newname;
02622 </div>
02623 <div class="parity0 source"><a href="#l1257" id="l1257"> 1257</a> }
02624 </div>
02625 <div class="parity1 source"><a href="#l1258" id="l1258"> 1258</a>
02626 </div>
02627 <div class="parity0 source"><a href="#l1259" id="l1259"> 1259</a> int _writeoptions;
02628 </div>
02629 <div class="parity1 source"><a href="#l1260" id="l1260"> 1260</a>
02630 </div>
02631 <div class="parity0 source"><a href="#l1261" id="l1261"> 1261</a> const urdf::Model& _robot;
02632 </div>
02633 <div class="parity1 source"><a href="#l1262" id="l1262"> 1262</a> boost::shared_ptr<DAE> _collada;
02634 </div>
02635 <div class="parity0 source"><a href="#l1263" id="l1263"> 1263</a> domCOLLADA* _dom;
02636 </div>
02637 <div class="parity1 source"><a href="#l1264" id="l1264"> 1264</a> domCOLLADA::domSceneRef _globalscene;
02638 </div>
02639 <div class="parity0 source"><a href="#l1265" id="l1265"> 1265</a>
02640 </div>
02641 <div class="parity1 source"><a href="#l1266" id="l1266"> 1266</a> domLibrary_visual_scenesRef _visualScenesLib;
02642 </div>
02643 <div class="parity0 source"><a href="#l1267" id="l1267"> 1267</a> domLibrary_kinematics_scenesRef _kinematicsScenesLib;
02644 </div>
02645 <div class="parity1 source"><a href="#l1268" id="l1268"> 1268</a> domLibrary_kinematics_modelsRef _kinematicsModelsLib;
02646 </div>
02647 <div class="parity0 source"><a href="#l1269" id="l1269"> 1269</a> domLibrary_articulated_systemsRef _articulatedSystemsLib;
02648 </div>
02649 <div class="parity1 source"><a href="#l1270" id="l1270"> 1270</a> domLibrary_physics_scenesRef _physicsScenesLib;
02650 </div>
02651 <div class="parity0 source"><a href="#l1271" id="l1271"> 1271</a> domLibrary_materialsRef _materialsLib;
02652 </div>
02653 <div class="parity1 source"><a href="#l1272" id="l1272"> 1272</a> domLibrary_effectsRef _effectsLib;
02654 </div>
02655 <div class="parity0 source"><a href="#l1273" id="l1273"> 1273</a> domLibrary_geometriesRef _geometriesLib;
02656 </div>
02657 <div class="parity1 source"><a href="#l1274" id="l1274"> 1274</a> domTechniqueRef _sensorsLib;
02658 </div>
02659 <div class="parity0 source"><a href="#l1275" id="l1275"> 1275</a> SCENE _scene;
02660 </div>
02661 <div class="parity1 source"><a href="#l1276" id="l1276"> 1276</a>
02662 </div>
02663 <div class="parity0 source"><a href="#l1277" id="l1277"> 1277</a> boost::shared_ptr<instance_kinematics_model_output> _ikmout;
02664 </div>
02665 <div class="parity1 source"><a href="#l1278" id="l1278"> 1278</a> boost::shared_ptr<instance_articulated_system_output> _iasout;
02666 </div>
02667 <div class="parity0 source"><a href="#l1279" id="l1279"> 1279</a> std::map< boost::shared_ptr<const urdf::Joint>, int > _mapjointindices;
02668 </div>
02669 <div class="parity1 source"><a href="#l1280" id="l1280"> 1280</a> std::map< boost::shared_ptr<const urdf::Link>, int > _maplinkindices;
02670 </div>
02671 <div class="parity0 source"><a href="#l1281" id="l1281"> 1281</a> std::map< boost::shared_ptr<const urdf::Material>, int > _mapmaterialindices;
02672 </div>
02673 <div class="parity1 source"><a href="#l1282" id="l1282"> 1282</a> Assimp::Importer _importer;
02674 </div>
02675 <div class="parity0 source"><a href="#l1283" id="l1283"> 1283</a> };
02676 </div>
02677 <div class="parity1 source"><a href="#l1284" id="l1284"> 1284</a>
02678 </div>
02679 <div class="parity0 source"><a href="#l1285" id="l1285"> 1285</a>
02680 </div>
02681 <div class="parity1 source"><a href="#l1286" id="l1286"> 1286</a> ColladaUrdfException::ColladaUrdfException(std::string const& what)
02682 </div>
02683 <div class="parity0 source"><a href="#l1287" id="l1287"> 1287</a> : std::runtime_error(what)
02684 </div>
02685 <div class="parity1 source"><a href="#l1288" id="l1288"> 1288</a> {
02686 </div>
02687 <div class="parity0 source"><a href="#l1289" id="l1289"> 1289</a> }
02688 </div>
02689 <div class="parity1 source"><a href="#l1290" id="l1290"> 1290</a>
02690 </div>
02691 <div class="parity0 source"><a href="#l1291" id="l1291"> 1291</a> bool colladaFromUrdfFile(string const& file, boost::shared_ptr<DAE>& dom) {
02692 </div>
02693 <div class="parity1 source"><a href="#l1292" id="l1292"> 1292</a> TiXmlDocument urdf_xml;
02694 </div>
02695 <div class="parity0 source"><a href="#l1293" id="l1293"> 1293</a> if (!urdf_xml.LoadFile(file)) {
02696 </div>
02697 <div class="parity1 source"><a href="#l1294" id="l1294"> 1294</a> ROS_ERROR("Could not load XML file");
02698 </div>
02699 <div class="parity0 source"><a href="#l1295" id="l1295"> 1295</a> return false;
02700 </div>
02701 <div class="parity1 source"><a href="#l1296" id="l1296"> 1296</a> }
02702 </div>
02703 <div class="parity0 source"><a href="#l1297" id="l1297"> 1297</a>
02704 </div>
02705 <div class="parity1 source"><a href="#l1298" id="l1298"> 1298</a> return colladaFromUrdfXml(&urdf_xml, dom);
02706 </div>
02707 <div class="parity0 source"><a href="#l1299" id="l1299"> 1299</a> }
02708 </div>
02709 <div class="parity1 source"><a href="#l1300" id="l1300"> 1300</a>
02710 </div>
02711 <div class="parity0 source"><a href="#l1301" id="l1301"> 1301</a> bool colladaFromUrdfString(string const& xml, boost::shared_ptr<DAE>& dom) {
02712 </div>
02713 <div class="parity1 source"><a href="#l1302" id="l1302"> 1302</a> TiXmlDocument urdf_xml;
02714 </div>
02715 <div class="parity0 source"><a href="#l1303" id="l1303"> 1303</a> if (urdf_xml.Parse(xml.c_str()) == 0) {
02716 </div>
02717 <div class="parity1 source"><a href="#l1304" id="l1304"> 1304</a> ROS_ERROR("Could not parse XML document");
02718 </div>
02719 <div class="parity0 source"><a href="#l1305" id="l1305"> 1305</a> return false;
02720 </div>
02721 <div class="parity1 source"><a href="#l1306" id="l1306"> 1306</a> }
02722 </div>
02723 <div class="parity0 source"><a href="#l1307" id="l1307"> 1307</a>
02724 </div>
02725 <div class="parity1 source"><a href="#l1308" id="l1308"> 1308</a> return colladaFromUrdfXml(&urdf_xml, dom);
02726 </div>
02727 <div class="parity0 source"><a href="#l1309" id="l1309"> 1309</a> }
02728 </div>
02729 <div class="parity1 source"><a href="#l1310" id="l1310"> 1310</a>
02730 </div>
02731 <div class="parity0 source"><a href="#l1311" id="l1311"> 1311</a> bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr<DAE>& dom) {
02732 </div>
02733 <div class="parity1 source"><a href="#l1312" id="l1312"> 1312</a> urdf::Model robot_model;
02734 </div>
02735 <div class="parity0 source"><a href="#l1313" id="l1313"> 1313</a> if (!robot_model.initXml(xml_doc)) {
02736 </div>
02737 <div class="parity1 source"><a href="#l1314" id="l1314"> 1314</a> ROS_ERROR("Could not generate robot model");
02738 </div>
02739 <div class="parity0 source"><a href="#l1315" id="l1315"> 1315</a> return false;
02740 </div>
02741 <div class="parity1 source"><a href="#l1316" id="l1316"> 1316</a> }
02742 </div>
02743 <div class="parity0 source"><a href="#l1317" id="l1317"> 1317</a>
02744 </div>
02745 <div class="parity1 source"><a href="#l1318" id="l1318"> 1318</a> return colladaFromUrdfModel(robot_model, dom);
02746 </div>
02747 <div class="parity0 source"><a href="#l1319" id="l1319"> 1319</a> }
02748 </div>
02749 <div class="parity1 source"><a href="#l1320" id="l1320"> 1320</a>
02750 </div>
02751 <div class="parity0 source"><a href="#l1321" id="l1321"> 1321</a> bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr<DAE>& dom) {
02752 </div>
02753 <div class="parity1 source"><a href="#l1322" id="l1322"> 1322</a> ColladaWriter writer(robot_model,0);
02754 </div>
02755 <div class="parity0 source"><a href="#l1323" id="l1323"> 1323</a> dom = writer.convert();
02756 </div>
02757 <div class="parity1 source"><a href="#l1324" id="l1324"> 1324</a> return dom != boost::shared_ptr<DAE>();
02758 </div>
02759 <div class="parity0 source"><a href="#l1325" id="l1325"> 1325</a> }
02760 </div>
02761 <div class="parity1 source"><a href="#l1326" id="l1326"> 1326</a>
02762 </div>
02763 <div class="parity0 source"><a href="#l1327" id="l1327"> 1327</a> bool colladaToFile(boost::shared_ptr<DAE> dom, string const& file) {
02764 </div>
02765 <div class="parity1 source"><a href="#l1328" id="l1328"> 1328</a> daeString uri = dom->getDoc(0)->getDocumentURI()->getURI();
02766 </div>
02767 <div class="parity0 source"><a href="#l1329" id="l1329"> 1329</a> return dom->writeTo(uri, file);
02768 </div>
02769 <div class="parity1 source"><a href="#l1330" id="l1330"> 1330</a> }
02770 </div>
02771 <div class="parity0 source"><a href="#l1331" id="l1331"> 1331</a>
02772 </div>
02773 <div class="parity1 source"><a href="#l1332" id="l1332"> 1332</a> }
02774 </div>
02775 <div class="sourcelast"></div>
02776 </div>
02777 </div>
02778 </div>
02779
02780
02781
02782 </body>
02783 </html>
02784