-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathexample__urdf__group_8cc_source.html
106 lines (104 loc) · 10.3 KB
/
example__urdf__group_8cc_source.html
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>lebai-motion-control: example_urdf_group.cc Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
$(document).ready(function() { init_search(); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
<tbody>
<tr style="height: 56px;">
<td id="projectlogo"><img alt="Logo" src="logo.png"/></td>
<td id="projectalign" style="padding-left: 0.5em;">
<div id="projectname">lebai-motion-control
 <span id="projectnumber">3.1.6</span>
</div>
<div id="projectbrief">lebai motion control C interface</div>
</td>
</tr>
</tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<div id="navrow1" class="tabs">
<ul class="tablist">
<li><a href="index.html"><span>Main Page</span></a></li>
<li><a href="pages.html"><span>Related Pages</span></a></li>
<li><a href="modules.html"><span>Modules</span></a></li>
<li><a href="annotated.html"><span>Data Structures</span></a></li>
<li class="current"><a href="files.html"><span>Files</span></a></li>
<li><a href="examples.html"><span>Examples</span></a></li>
<li>
<div id="MSearchBox" class="MSearchBoxInactive">
<span class="left">
<img id="MSearchSelect" src="search/mag_sel.png"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
alt=""/>
<input type="text" id="MSearchField" value="Search" accesskey="S"
onfocus="searchBox.OnSearchFieldFocus(true)"
onblur="searchBox.OnSearchFieldFocus(false)"
onkeyup="searchBox.OnSearchFieldChange(event)"/>
</span><span class="right">
<a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
</span>
</div>
</li>
</ul>
</div>
<div id="navrow2" class="tabs2">
<ul class="tablist">
<li><a href="files.html"><span>File List</span></a></li>
<li><a href="globals.html"><span>Globals</span></a></li>
</ul>
</div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
onmouseover="return searchBox.OnSearchSelectShow()"
onmouseout="return searchBox.OnSearchSelectHide()"
onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>
<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0"
name="MSearchResults" id="MSearchResults">
</iframe>
</div>
<div id="nav-path" class="navpath">
<ul>
<li class="navelem"><a class="el" href="dir_d28a4824dc47e487b107a5db32ef43c4.html">examples</a></li> </ul>
</div>
</div><!-- top -->
<div class="header">
<div class="headertitle">
<div class="title">example_urdf_group.cc</div> </div>
</div><!--header-->
<div class="contents">
<a href="example__urdf__group_8cc.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> <span class="preprocessor">#include "motion_control/lmc_group.h"</span></div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> <span class="preprocessor">#include <math.h></span></div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> <span class="preprocessor">#include <iostream></span></div><div class="line"><a name="l00004"></a><span class="lineno"> 4</span> </div><div class="line"><a name="l00016"></a><span class="lineno"><a class="line" href="example__urdf__group_8cc.html#a3c04138a5bfe5d72780bb7e82a18e627"> 16</a></span> <span class="keywordtype">int</span> <a class="code" href="example__urdf__group_8cc.html#a3c04138a5bfe5d72780bb7e82a18e627">main</a>(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span> **argv)</div><div class="line"><a name="l00017"></a><span class="lineno"> 17</span> {</div><div class="line"><a name="l00018"></a><span class="lineno"> 18</span>  lmc_group_model_t *group_model = lmc_group_model_new();</div><div class="line"><a name="l00019"></a><span class="lineno"> 19</span>  <span class="keywordtype">char</span> filename[] = <span class="stringliteral">"urdf/lm3.urdf"</span>;</div><div class="line"><a name="l00020"></a><span class="lineno"> 20</span>  lmc_group_model_init_from_urdf_file(group_model, filename);</div><div class="line"><a name="l00021"></a><span class="lineno"> 21</span>  </div><div class="line"><a name="l00022"></a><span class="lineno"> 22</span>  lmc_group_joint_info_t joint_info;</div><div class="line"><a name="l00023"></a><span class="lineno"> 23</span>  lmc_group_model_get_joint_info(group_model, &joint_info);</div><div class="line"><a name="l00024"></a><span class="lineno"> 24</span> </div><div class="line"><a name="l00025"></a><span class="lineno"> 25</span>  lmc_group_state_t *group_state = lmc_group_state_new();</div><div class="line"><a name="l00026"></a><span class="lineno"> 26</span>  lmc_group_state_init(group_state, group_model);</div><div class="line"><a name="l00027"></a><span class="lineno"> 27</span> </div><div class="line"><a name="l00028"></a><span class="lineno"> 28</span>  lmc_group_state_collide(group_state, NULL, NULL);</div><div class="line"><a name="l00029"></a><span class="lineno"> 29</span> </div><div class="line"><a name="l00030"></a><span class="lineno"> 30</span>  <span class="keywordtype">double</span> joint_pos[6] = {-11.0 / 180.0 * M_PI, 157.0 / 180.0 * M_PI, -209.0 / 180.0 * M_PI, 88.0 / 180.0 * M_PI, 100.0 / 180.0 * M_PI, -76.0 / 180.0 * M_PI};</div><div class="line"><a name="l00031"></a><span class="lineno"> 31</span>  lmc_group_state_set_jnt_pos(group_state, joint_pos);</div><div class="line"><a name="l00032"></a><span class="lineno"> 32</span>  </div><div class="line"><a name="l00033"></a><span class="lineno"> 33</span>  lmc_group_state_update(group_state, group_model);</div><div class="line"><a name="l00034"></a><span class="lineno"> 34</span>  </div><div class="line"><a name="l00035"></a><span class="lineno"> 35</span>  lmc_group_collide_req_t req;</div><div class="line"><a name="l00036"></a><span class="lineno"> 36</span>  lmc_group_collide_resp_t resp;</div><div class="line"><a name="l00037"></a><span class="lineno"> 37</span>  req.enable_contact = 1;</div><div class="line"><a name="l00038"></a><span class="lineno"> 38</span>  req.num_max_contacts = 2;</div><div class="line"><a name="l00039"></a><span class="lineno"> 39</span>  lmc_group_state_collide(group_state, &req, &resp);</div><div class="line"><a name="l00040"></a><span class="lineno"> 40</span>  <span class="keywordflow">for</span>(<span class="keywordtype">size_t</span> i = 0; i < resp.num_contacts; ++i)</div><div class="line"><a name="l00041"></a><span class="lineno"> 41</span>  {</div><div class="line"><a name="l00042"></a><span class="lineno"> 42</span>  std::cout<<<span class="stringliteral">"contact pair: "</span><<resp.contacts[i].link_a<<<span class="stringliteral">" "</span><<resp.contacts[i].link_b<<<span class="stringliteral">"\n"</span>;</div><div class="line"><a name="l00043"></a><span class="lineno"> 43</span>  }</div><div class="line"><a name="l00044"></a><span class="lineno"> 44</span>  <span class="comment">// EXPECT_STREQ("link_6", resp.contacts[0].link_a);</span></div><div class="line"><a name="l00045"></a><span class="lineno"> 45</span>  <span class="comment">// EXPECT_STREQ("link_2", resp.contacts[0].link_b);</span></div><div class="line"><a name="l00046"></a><span class="lineno"> 46</span>  <span class="comment">// EXPECT_STREQ("link_6", resp.contacts[1].link_a);</span></div><div class="line"><a name="l00047"></a><span class="lineno"> 47</span>  <span class="comment">// EXPECT_STREQ("link_2", resp.contacts[1].link_b);</span></div><div class="line"><a name="l00048"></a><span class="lineno"> 48</span>  lmc_group_state_delete(&group_state);</div><div class="line"><a name="l00049"></a><span class="lineno"> 49</span>  lmc_group_model_delete(&group_model);</div><div class="line"><a name="l00050"></a><span class="lineno"> 50</span>  <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l00051"></a><span class="lineno"> 51</span> }</div><div class="ttc" id="example__urdf__group_8cc_html_a3c04138a5bfe5d72780bb7e82a18e627"><div class="ttname"><a href="example__urdf__group_8cc.html#a3c04138a5bfe5d72780bb7e82a18e627">main</a></div><div class="ttdeci">int main(int argc, char **argv)</div><div class="ttdoc">Example of urdf group usage. </div><div class="ttdef"><b>Definition:</b> <a href="example__urdf__group_8cc_source.html#l00016">example_urdf_group.cc:16</a></div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by  <a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.11
</small></address>
</body>
</html>