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
|
#include "MultiBodyTreeDebugGraph.hpp"
#include <cstdio>
namespace btInverseDynamics
{
int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
const char* filename)
{
if (0x0 == tree)
{
bt_id_error_message("tree pointer is null\n");
return -1;
}
if (0x0 == filename)
{
bt_id_error_message("filename is null\n");
return -1;
}
FILE* fp = fopen(filename, "w");
if (NULL == fp)
{
bt_id_error_message("cannot open file %s for writing\n", filename);
return -1;
}
fprintf(fp,
"// to generate postscript file, run dot -Tps %s -o %s.ps\n"
"// details see graphviz documentation at http://graphviz.org\n"
"digraph tree {\n",
filename, filename);
for (int body = 0; body < tree->numBodies(); body++)
{
std::string name;
if (0x0 != map)
{
if (-1 == map->getBodyName(body, &name))
{
bt_id_error_message("can't get name of body %d\n", body);
return -1;
}
fprintf(fp, " %d [label=\"%d/%s\"];\n", body, body, name.c_str());
}
}
for (int body = 0; body < tree->numBodies(); body++)
{
int parent;
const char* joint_type;
int qi;
if (-1 == tree->getParentIndex(body, &parent))
{
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 == tree->getJointTypeStr(body, &joint_type))
{
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 == tree->getDoFOffset(body, &qi))
{
bt_id_error_message("indexing error\n");
return -1;
}
if (-1 != parent)
{
fprintf(fp, " %d -> %d [label= \"type:%s, q=%d\"];\n", parent, body,
joint_type, qi);
}
}
fprintf(fp, "}\n");
fclose(fp);
return 0;
}
} // namespace btInverseDynamics
|
