diff --git a/data/kuka_iiwa/kuka_world.sdf b/data/kuka_iiwa/kuka_world.sdf new file mode 100644 index 000000000..d48d51382 --- /dev/null +++ b/data/kuka_iiwa/kuka_world.sdf @@ -0,0 +1,414 @@ + + + + + world + lbr_iiwa_link_0 + + + 0 0 0 0 -0 0 + + -0.1 0 0.07 0 -0 0 + 0.01 + + 0.05 + 0 + 0 + 0.06 + 0 + 0.03 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_0.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_0.stl + + + + + + 0 0 0.1575 0 -0 0 + + 0 -0.03 0.12 0 -0 0 + 4 + + 0.1 + 0 + 0 + 0.09 + 0 + 0.02 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_1.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.stl + + + + + + lbr_iiwa_link_1 + lbr_iiwa_link_0 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.36 1.5708 -0 -3.14159 + + 0.0003 0.059 0.042 0 -0 0 + 4 + + 0.05 + 0 + 0 + 0.018 + 0 + 0.044 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_2.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.stl + + + + + + lbr_iiwa_link_2 + lbr_iiwa_link_1 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.5645 0 0 0 + + 0 0.03 0.13 0 -0 0 + 3 + + 0.08 + 0 + 0 + 0.075 + 0 + 0.01 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_3.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.stl + + + + + + lbr_iiwa_link_3 + lbr_iiwa_link_2 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.78 1.5708 0 0 + + 0 0.067 0.034 0 -0 0 + 2.7 + + 0.03 + 0 + 0 + 0.01 + 0 + 0.029 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_4.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.stl + + + + + + lbr_iiwa_link_4 + lbr_iiwa_link_3 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.9645 0 -0 -3.14159 + + 0.0001 0.021 0.076 0 -0 0 + 1.7 + + 0.02 + 0 + 0 + 0.018 + 0 + 0.005 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_5.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.stl + + + + + + lbr_iiwa_link_5 + lbr_iiwa_link_4 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.18 1.5708 -0 -3.14159 + + 0 0.0006 0.0004 0 -0 0 + 1.8 + + 0.005 + 0 + 0 + 0.0036 + 0 + 0.0047 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_6.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.stl + + + + + + lbr_iiwa_link_6 + lbr_iiwa_link_5 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.261 0 0 0 + + 0 0 0.02 0 -0 0 + 0.3 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_7.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.stl + + + + + + lbr_iiwa_link_7 + lbr_iiwa_link_6 + + 0 0 1 + + -3.05433 + 3.05433 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + + \ No newline at end of file diff --git a/data/kuka_iiwa/meshes/coarse/link_0.stl b/data/kuka_iiwa/meshes/coarse/link_0.stl new file mode 100644 index 000000000..84b8ea5d2 Binary files /dev/null and b/data/kuka_iiwa/meshes/coarse/link_0.stl differ diff --git a/data/kuka_iiwa/meshes/coarse/link_1.stl b/data/kuka_iiwa/meshes/coarse/link_1.stl new file mode 100644 index 000000000..ffe3ec981 Binary files /dev/null and b/data/kuka_iiwa/meshes/coarse/link_1.stl differ diff --git a/data/kuka_iiwa/meshes/coarse/link_2.stl b/data/kuka_iiwa/meshes/coarse/link_2.stl new file mode 100644 index 000000000..4a51b27ef Binary files /dev/null and b/data/kuka_iiwa/meshes/coarse/link_2.stl differ diff --git a/data/kuka_iiwa/meshes/coarse/link_3.stl b/data/kuka_iiwa/meshes/coarse/link_3.stl new file mode 100644 index 000000000..32d6d5282 Binary files /dev/null and b/data/kuka_iiwa/meshes/coarse/link_3.stl differ diff --git a/data/kuka_iiwa/meshes/coarse/link_4.stl b/data/kuka_iiwa/meshes/coarse/link_4.stl new file mode 100644 index 000000000..35d192181 Binary files /dev/null and b/data/kuka_iiwa/meshes/coarse/link_4.stl differ diff --git a/data/kuka_iiwa/meshes/coarse/link_5.stl b/data/kuka_iiwa/meshes/coarse/link_5.stl new file mode 100644 index 000000000..35aa1245d Binary files /dev/null and b/data/kuka_iiwa/meshes/coarse/link_5.stl differ diff --git a/data/kuka_iiwa/meshes/coarse/link_6.stl b/data/kuka_iiwa/meshes/coarse/link_6.stl new file mode 100644 index 000000000..bce349eb2 Binary files /dev/null and b/data/kuka_iiwa/meshes/coarse/link_6.stl differ diff --git a/data/kuka_iiwa/meshes/coarse/link_7.stl b/data/kuka_iiwa/meshes/coarse/link_7.stl new file mode 100644 index 000000000..2d5d6ecfb Binary files /dev/null and b/data/kuka_iiwa/meshes/coarse/link_7.stl differ diff --git a/data/kuka_iiwa/meshes/link_0.stl b/data/kuka_iiwa/meshes/link_0.stl new file mode 100644 index 000000000..84b8ea5d2 Binary files /dev/null and b/data/kuka_iiwa/meshes/link_0.stl differ diff --git a/data/kuka_iiwa/meshes/link_1.stl b/data/kuka_iiwa/meshes/link_1.stl new file mode 100644 index 000000000..e8e37de9f Binary files /dev/null and b/data/kuka_iiwa/meshes/link_1.stl differ diff --git a/data/kuka_iiwa/meshes/link_2.stl b/data/kuka_iiwa/meshes/link_2.stl new file mode 100644 index 000000000..47c7885fc Binary files /dev/null and b/data/kuka_iiwa/meshes/link_2.stl differ diff --git a/data/kuka_iiwa/meshes/link_3.stl b/data/kuka_iiwa/meshes/link_3.stl new file mode 100644 index 000000000..027eb2211 Binary files /dev/null and b/data/kuka_iiwa/meshes/link_3.stl differ diff --git a/data/kuka_iiwa/meshes/link_4.stl b/data/kuka_iiwa/meshes/link_4.stl new file mode 100644 index 000000000..c0c1213c1 Binary files /dev/null and b/data/kuka_iiwa/meshes/link_4.stl differ diff --git a/data/kuka_iiwa/meshes/link_5.stl b/data/kuka_iiwa/meshes/link_5.stl new file mode 100644 index 000000000..82a9337a6 Binary files /dev/null and b/data/kuka_iiwa/meshes/link_5.stl differ diff --git a/data/kuka_iiwa/meshes/link_6.stl b/data/kuka_iiwa/meshes/link_6.stl new file mode 100644 index 000000000..10b558dc5 Binary files /dev/null and b/data/kuka_iiwa/meshes/link_6.stl differ diff --git a/data/kuka_iiwa/meshes/link_7.stl b/data/kuka_iiwa/meshes/link_7.stl new file mode 100644 index 000000000..5909e7e01 Binary files /dev/null and b/data/kuka_iiwa/meshes/link_7.stl differ diff --git a/data/kuka_iiwa/model.sdf b/data/kuka_iiwa/model.sdf new file mode 100644 index 000000000..1b6f8320d --- /dev/null +++ b/data/kuka_iiwa/model.sdf @@ -0,0 +1,410 @@ + + + + + 0 0 0 0 -0 0 + + -0.1 0 0.07 0 -0 0 + 0 + + 0.05 + 0 + 0 + 0.06 + 0 + 0.03 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_0.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_0.stl + + + + + + 0 0 0.1575 0 -0 0 + + 0 -0.03 0.12 0 -0 0 + 4 + + 0.1 + 0 + 0 + 0.09 + 0 + 0.02 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_1.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.stl + + + + + + lbr_iiwa_link_1 + lbr_iiwa_link_0 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.36 1.5708 -0 -3.14159 + + 0.0003 0.059 0.042 0 -0 0 + 4 + + 0.05 + 0 + 0 + 0.018 + 0 + 0.044 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_2.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.stl + + + + + + lbr_iiwa_link_2 + lbr_iiwa_link_1 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.5645 0 0 0 + + 0 0.03 0.13 0 -0 0 + 3 + + 0.08 + 0 + 0 + 0.075 + 0 + 0.01 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_3.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.stl + + + + + + lbr_iiwa_link_3 + lbr_iiwa_link_2 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.78 1.5708 0 0 + + 0 0.067 0.034 0 -0 0 + 2.7 + + 0.03 + 0 + 0 + 0.01 + 0 + 0.029 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_4.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.stl + + + + + + lbr_iiwa_link_4 + lbr_iiwa_link_3 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.9645 0 -0 -3.14159 + + 0.0001 0.021 0.076 0 -0 0 + 1.7 + + 0.02 + 0 + 0 + 0.018 + 0 + 0.005 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_5.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.stl + + + + + + lbr_iiwa_link_5 + lbr_iiwa_link_4 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.18 1.5708 -0 -3.14159 + + 0 0.0006 0.0004 0 -0 0 + 1.8 + + 0.005 + 0 + 0 + 0.0036 + 0 + 0.0047 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_6.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.stl + + + + + + lbr_iiwa_link_6 + lbr_iiwa_link_5 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.261 0 0 0 + + 0 0 0.02 0 -0 0 + 0.3 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_7.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.stl + + + + + + lbr_iiwa_link_7 + lbr_iiwa_link_6 + + 0 0 1 + + -3.05433 + 3.05433 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + + \ No newline at end of file diff --git a/data/kuka_iiwa/model.urdf b/data/kuka_iiwa/model.urdf new file mode 100644 index 000000000..0b290946a --- /dev/null +++ b/data/kuka_iiwa/model.urdf @@ -0,0 +1,284 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/kuka_iiwa/model2.sdf b/data/kuka_iiwa/model2.sdf new file mode 100644 index 000000000..117357cf4 --- /dev/null +++ b/data/kuka_iiwa/model2.sdf @@ -0,0 +1,818 @@ + + + + + 0 0 0 0 -0 0 + + -0.1 0 0.07 0 -0 0 + 0 + + 0.05 + 0 + 0 + 0.06 + 0 + 0.03 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_0.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_0.stl + + + + + + 0 0 0.1575 0 -0 0 + + 0 -0.03 0.12 0 -0 0 + 4 + + 0.1 + 0 + 0 + 0.09 + 0 + 0.02 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_1.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.stl + + + + + + lbr_iiwa_link_1 + lbr_iiwa_link_0 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.36 1.5708 -0 -3.14159 + + 0.0003 0.059 0.042 0 -0 0 + 4 + + 0.05 + 0 + 0 + 0.018 + 0 + 0.044 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_2.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.stl + + + + + + lbr_iiwa_link_2 + lbr_iiwa_link_1 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.5645 0 0 0 + + 0 0.03 0.13 0 -0 0 + 3 + + 0.08 + 0 + 0 + 0.075 + 0 + 0.01 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_3.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.stl + + + + + + lbr_iiwa_link_3 + lbr_iiwa_link_2 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.78 1.5708 0 0 + + 0 0.067 0.034 0 -0 0 + 2.7 + + 0.03 + 0 + 0 + 0.01 + 0 + 0.029 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_4.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.stl + + + + + + lbr_iiwa_link_4 + lbr_iiwa_link_3 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.9645 0 -0 -3.14159 + + 0.0001 0.021 0.076 0 -0 0 + 1.7 + + 0.02 + 0 + 0 + 0.018 + 0 + 0.005 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_5.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.stl + + + + + + lbr_iiwa_link_5 + lbr_iiwa_link_4 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.18 1.5708 -0 -3.14159 + + 0 0.0006 0.0004 0 -0 0 + 1.8 + + 0.005 + 0 + 0 + 0.0036 + 0 + 0.0047 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_6.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.stl + + + + + + lbr_iiwa_link_6 + lbr_iiwa_link_5 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.261 0 0 0 + + 0 0 0.02 0 -0 0 + 0.3 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_7.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.stl + + + + + + lbr_iiwa_link_7 + lbr_iiwa_link_6 + + 0 0 1 + + -3.05433 + 3.05433 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + + + 2 2 0 0 -0 0 + + 0 0 0 0 -0 0 + + -0.1 0 0.07 0 -0 0 + 0 + + 0.05 + 0 + 0 + 0.06 + 0 + 0.03 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_0.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_0.stl + + + + + + 0 0 0.1575 0 -0 0 + + 0 -0.03 0.12 0 -0 0 + 4 + + 0.1 + 0 + 0 + 0.09 + 0 + 0.02 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_1.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.stl + + + + + + lbr_iiwa_link_1 + lbr_iiwa_link_0 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.36 1.5708 -0 -3.14159 + + 0.0003 0.059 0.042 0 -0 0 + 4 + + 0.05 + 0 + 0 + 0.018 + 0 + 0.044 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_2.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.stl + + + + + + lbr_iiwa_link_2 + lbr_iiwa_link_1 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.5645 0 0 0 + + 0 0.03 0.13 0 -0 0 + 3 + + 0.08 + 0 + 0 + 0.075 + 0 + 0.01 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_3.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.stl + + + + + + lbr_iiwa_link_3 + lbr_iiwa_link_2 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.78 1.5708 0 0 + + 0 0.067 0.034 0 -0 0 + 2.7 + + 0.03 + 0 + 0 + 0.01 + 0 + 0.029 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_4.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.stl + + + + + + lbr_iiwa_link_4 + lbr_iiwa_link_3 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.9645 0 -0 -3.14159 + + 0.0001 0.021 0.076 0 -0 0 + 1.7 + + 0.02 + 0 + 0 + 0.018 + 0 + 0.005 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_5.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.stl + + + + + + lbr_iiwa_link_5 + lbr_iiwa_link_4 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.18 1.5708 -0 -3.14159 + + 0 0.0006 0.0004 0 -0 0 + 1.8 + + 0.005 + 0 + 0 + 0.0036 + 0 + 0.0047 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_6.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.stl + + + + + + lbr_iiwa_link_6 + lbr_iiwa_link_5 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.261 0 0 0 + + 0 0 0.02 0 -0 0 + 0.3 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_7.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.stl + + + + + + lbr_iiwa_link_7 + lbr_iiwa_link_6 + + 0 0 1 + + -3.05433 + 3.05433 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + + \ No newline at end of file diff --git a/data/kuka_iiwa/model_for_sdf.urdf b/data/kuka_iiwa/model_for_sdf.urdf new file mode 100644 index 000000000..12a165423 --- /dev/null +++ b/data/kuka_iiwa/model_for_sdf.urdf @@ -0,0 +1,285 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index b95f528a9..848ee699c 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -214,10 +214,8 @@ void ImportSDFSetup::initPhysics() //u2b.printTree(); - btTransform identityTrans; - identityTrans.setIdentity(); - - + btTransform rootTrans; + rootTrans.setIdentity(); for (int m =0; mm_linkTransformInWorld; if (link->m_parentJoint) { @@ -333,7 +333,11 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi } - +bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const +{ + rootTransformInWorld = m_data->m_urdfParser.getModel().m_rootTransformInWorld; + return true; +} void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) { diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index edfafc135..5e1437158 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -40,7 +40,9 @@ public: virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const; - virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const; + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const; + + virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp index 6608a2ad4..5e6ce6fb9 100644 --- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp +++ b/examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp @@ -223,7 +223,7 @@ void ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3 } } -bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const +bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const { jointLowerLimit = 0.f; jointUpperLimit = 0.f; @@ -281,7 +281,9 @@ bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, } } - +bool ROSURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const +{ +} void ROSconvertURDFToVisualShape(const Visual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) { diff --git a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h index ad09339e1..289fcbb8b 100644 --- a/examples/Importers/ImportURDFDemo/ROSURDFImporter.h +++ b/examples/Importers/ImportURDFDemo/ROSURDFImporter.h @@ -33,7 +33,9 @@ public: virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const; - virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit,btScalar& jointDamping, btScalar& jointFriction) const; + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit,btScalar& jointDamping, btScalar& jointFriction) const; + + virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index b934d9000..6ff360788 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -143,7 +143,7 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat } -void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) +void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false) { //b3Printf("start converting/extracting data from URDF interface\n"); @@ -199,11 +199,16 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat btScalar jointFriction; - bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); - - - linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; - + bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); + if (useSDF) + { + parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace; + } + else + { + linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; + } + int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame); btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame); @@ -219,11 +224,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); } */ - //btVector3 localInertiaDiagonal(0, 0, 0); - //if (mass) - //{ - // shape->calculateLocalInertia(mass, localInertiaDiagonal); - //} + if (mass) + { + compoundShape->calculateLocalInertia(mass, localInertiaDiagonal); + } btRigidBody* linkRigidBody = 0; btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame; @@ -404,18 +408,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { int urdfChildLinkIndex = urdfChildIndices[i]; - ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix); + ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF); } } -void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix) +void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix, bool useSDF = false) { URDF2BulletCachedData cache; InitURDF2BulletCache(u2b,cache); int urdfLinkIndex = u2b.getRootLinkIndex(); - ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix); + ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,useSDF); if (world1 && cache.m_bulletMultiBody) { diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index 0d9306848..148535812 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -20,7 +20,8 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world, bool createMultiBody, - const char* pathPrefix); + const char* pathPrefix, + bool useSDF = false); #endif //_URDF2BULLET_H diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 91dc52f31..fd794ba3c 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -37,7 +37,9 @@ public: ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray& childLinkIndices) const =0; - virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0; + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0; + + virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0; ///quick hack: need to rethink the API/dependencies of this virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 25ca7461e..60b40b512 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -77,7 +77,7 @@ static bool parseVector4(btVector4& vec4, const std::string& vector_str) return true; } -static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger) +static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger, bool lastThree = false) { vec3.setZero(); btArray pieces; @@ -90,16 +90,22 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLo rgba.push_back(urdfLexicalCast(pieces[i].c_str())); } } - if (rgba.size() != 3) + if (rgba.size() < 3) { logger->reportWarning("Couldn't parse vector3"); return false; } - vec3.setValue(rgba[0],rgba[1],rgba[2]); + if (lastThree) { + vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]); + } + else + { + vec3.setValue(rgba[0],rgba[1],rgba[2]); + + } return true; } - bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, ErrorLogger* logger) { @@ -143,47 +149,78 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err } -bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger) +bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false) { - tr.setIdentity(); - - { - const char* xyz_str = xml->Attribute("xyz"); - if (xyz_str) - { - parseVector3(tr.getOrigin(),std::string(xyz_str),logger); - } - } - - { - const char* rpy_str = xml->Attribute("rpy"); - if (rpy_str != NULL) - { - btVector3 rpy; - if (parseVector3(rpy,std::string(rpy_str),logger)) - { - double phi, the, psi; - double roll = rpy[0]; - double pitch = rpy[1]; - double yaw = rpy[2]; - - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - - btQuaternion orn( - sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), - cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), - cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), - cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)); - - orn.normalize(); - tr.setRotation(orn); - } - } - } - return true; + tr.setIdentity(); + + if (parseSDF) + { + parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger); + } + else + { + const char* xyz_str = xml->Attribute("xyz"); + if (xyz_str) + { + parseVector3(tr.getOrigin(),std::string(xyz_str),logger); + } + } + + if (parseSDF) + { + btVector3 rpy; + if (parseVector3(rpy,std::string(xml->GetText()),logger,true)) + { + double phi, the, psi; + double roll = rpy[0]; + double pitch = rpy[1]; + double yaw = rpy[2]; + + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 2.0; + + btQuaternion orn( + sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), + cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), + cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), + cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)); + + orn.normalize(); + tr.setRotation(orn); + } + } + else + { + const char* rpy_str = xml->Attribute("rpy"); + if (rpy_str != NULL) + { + btVector3 rpy; + if (parseVector3(rpy,std::string(rpy_str),logger)) + { + double phi, the, psi; + double roll = rpy[0]; + double pitch = rpy[1]; + double yaw = rpy[2]; + + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 2.0; + + btQuaternion orn( + sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), + cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), + cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), + cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)); + + orn.normalize(); + tr.setRotation(orn); + } + } + } + return true; } + bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger) { inertia.m_linkLocalFrame.setIdentity(); @@ -333,21 +370,38 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* else if (type_name == "mesh") { geom.m_type = URDF_GEOM_MESH; - if (!shape->Attribute("filename")) { - logger->reportError("Mesh must contain a filename attribute"); - return false; - } - - geom.m_meshFileName = shape->Attribute("filename"); - - if (shape->Attribute("scale")) - { - parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger); - } else - { - geom.m_meshScale.setValue(1,1,1); - } - + if (m_parseSDF) + { + TiXmlElement* scale = shape->FirstChildElement("scale"); + if (0==scale) + { + geom.m_meshScale.setValue(1,1,1); + } + else + { + parseVector3(geom.m_meshScale,scale->GetText(),logger); + } + + TiXmlElement* filename = shape->FirstChildElement("uri"); + geom.m_meshFileName = filename->GetText(); + } + else + { + if (!shape->Attribute("filename")) { + logger->reportError("Mesh must contain a filename attribute"); + return false; + } + + geom.m_meshFileName = shape->Attribute("filename"); + + if (shape->Attribute("scale")) + { + parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger); + } else + { + geom.m_meshScale.setValue(1,1,1); + } + } } else { @@ -473,7 +527,18 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi return false; } link.m_name = linkName; - + + if (m_parseSDF) { + TiXmlElement* pose = config->FirstChildElement("pose"); + if (0==pose) + { + link.m_linkTransformInWorld.setIdentity(); + } + else + { + parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF); + } + } // Inertial (optional) TiXmlElement *i = config->FirstChildElement("inertial"); @@ -552,35 +617,109 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL joint.m_effortLimit = 0.f; joint.m_velocityLimit = 0.f; - const char* lower_str = config->Attribute("lower"); - if (lower_str) - { - joint.m_lowerLimit = urdfLexicalCast(lower_str); - } - - const char* upper_str = config->Attribute("upper"); - if (upper_str) - { - joint.m_upperLimit = urdfLexicalCast(upper_str); - } - - - // Get joint effort limit - const char* effort_str = config->Attribute("effort"); - if (effort_str) - { - joint.m_effortLimit = urdfLexicalCast(effort_str); - } - - // Get joint velocity limit - const char* velocity_str = config->Attribute("velocity"); - if (velocity_str) - { - joint.m_velocityLimit = urdfLexicalCast(velocity_str); - } + if (m_parseSDF) + { + TiXmlElement *lower_xml = config->FirstChildElement("lower"); + if (lower_xml) { + joint.m_lowerLimit = urdfLexicalCast(lower_xml->GetText()); + } + + TiXmlElement *upper_xml = config->FirstChildElement("upper"); + if (upper_xml) { + joint.m_upperLimit = urdfLexicalCast(upper_xml->GetText()); + } + + TiXmlElement *effort_xml = config->FirstChildElement("effort"); + if (effort_xml) { + joint.m_effortLimit = urdfLexicalCast(effort_xml->GetText()); + } + + TiXmlElement *velocity_xml = config->FirstChildElement("velocity"); + if (velocity_xml) { + joint.m_velocityLimit = urdfLexicalCast(velocity_xml->GetText()); + } + } + else + { + const char* lower_str = config->Attribute("lower"); + if (lower_str) + { + joint.m_lowerLimit = urdfLexicalCast(lower_str); + } + + const char* upper_str = config->Attribute("upper"); + if (upper_str) + { + joint.m_upperLimit = urdfLexicalCast(upper_str); + } + + + // Get joint effort limit + const char* effort_str = config->Attribute("effort"); + if (effort_str) + { + joint.m_effortLimit = urdfLexicalCast(effort_str); + } + + // Get joint velocity limit + const char* velocity_str = config->Attribute("velocity"); + if (velocity_str) + { + joint.m_velocityLimit = urdfLexicalCast(velocity_str); + } + } return true; } + +bool UrdfParser::parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger) +{ + joint.m_jointDamping = 0; + joint.m_jointFriction = 0; + + if (m_parseSDF) { + TiXmlElement *damping_xml = config->FirstChildElement("damping"); + if (damping_xml) { + joint.m_jointDamping = urdfLexicalCast(damping_xml->GetText()); + } + + TiXmlElement *friction_xml = config->FirstChildElement("friction"); + if (friction_xml) { + joint.m_jointFriction = urdfLexicalCast(friction_xml->GetText()); + } + + if (damping_xml == NULL && friction_xml == NULL) + { + logger->reportError("joint dynamics element specified with no damping and no friction"); + return false; + } + } + else + { + // Get joint damping + const char* damping_str = config->Attribute("damping"); + if (damping_str) + { + joint.m_jointDamping = urdfLexicalCast(damping_str); + } + + // Get joint friction + const char* friction_str = config->Attribute("friction"); + if (friction_str) + { + joint.m_jointFriction = urdfLexicalCast(friction_str); + } + + if (damping_str == NULL && friction_str == NULL) + { + logger->reportError("joint dynamics element specified with no damping and no friction"); + return false; + } + } + + return true; +} + bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger) { @@ -611,34 +750,48 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* TiXmlElement *parent_xml = config->FirstChildElement("parent"); if (parent_xml) { - const char *pname = parent_xml->Attribute("link"); - if (!pname) - { - logger->reportError("no parent link name specified for Joint link. this might be the root?"); - logger->reportError(joint.m_name.c_str()); - return false; - } - else - { - joint.m_parentLinkName = std::string(pname); - } + if (m_parseSDF) + { + joint.m_parentLinkName = std::string(parent_xml->GetText()); + } + else + { + const char *pname = parent_xml->Attribute("link"); + if (!pname) + { + logger->reportError("no parent link name specified for Joint link. this might be the root?"); + logger->reportError(joint.m_name.c_str()); + return false; + } + else + { + joint.m_parentLinkName = std::string(pname); + } + } } - + // Get Child Link TiXmlElement *child_xml = config->FirstChildElement("child"); if (child_xml) { - const char *pname = child_xml->Attribute("link"); - if (!pname) - { - logger->reportError("no child link name specified for Joint link [%s]."); - logger->reportError(joint.m_name.c_str()); - return false; - } - else - { - joint.m_childLinkName = std::string(pname); - } + if (m_parseSDF) + { + joint.m_childLinkName = std::string(child_xml->GetText()); + } + else + { + const char *pname = child_xml->Attribute("link"); + if (!pname) + { + logger->reportError("no child link name specified for Joint link [%s]."); + logger->reportError(joint.m_name.c_str()); + return false; + } + else + { + joint.m_childLinkName = std::string(pname); + } + } } // Get Joint type @@ -672,83 +825,146 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* return false; } - // Get Joint Axis - if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint) - { - // axis - TiXmlElement *axis_xml = config->FirstChildElement("axis"); - if (!axis_xml){ - logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis"); - logger->reportWarning(joint.m_name.c_str()); - joint.m_localJointAxis.setValue(1,0,0); - } - else{ - if (axis_xml->Attribute("xyz")) - { - if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger)) - { - logger->reportError("Malformed axis element:"); - logger->reportError(joint.m_name.c_str()); - logger->reportError(" for joint:"); - logger->reportError(axis_xml->Attribute("xyz")); - return false; - } - } - } - } - - // Get limit - TiXmlElement *limit_xml = config->FirstChildElement("limit"); - if (limit_xml) - { - if (!parseJointLimits(joint, limit_xml,logger)) - { - logger->reportError("Could not parse limit element for joint:"); - logger->reportError(joint.m_name.c_str()); - return false; - } - } - else if (joint.m_type == URDFRevoluteJoint) - { - logger->reportError("Joint is of type REVOLUTE but it does not specify limits"); - logger->reportError(joint.m_name.c_str()); - return false; - } - else if (joint.m_type == URDFPrismaticJoint) - { - logger->reportError("Joint is of type PRISMATIC without limits"); - logger->reportError( joint.m_name.c_str()); - return false; - } - - joint.m_jointDamping = 0; - joint.m_jointFriction = 0; - - // Get Dynamics - TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); - if (prop_xml) - { - - // Get joint damping - const char* damping_str = prop_xml->Attribute("damping"); - if (damping_str) - { - joint.m_jointDamping = urdfLexicalCast(damping_str); - } - - // Get joint friction - const char* friction_str = prop_xml->Attribute("friction"); - if (friction_str) - { - joint.m_jointFriction = urdfLexicalCast(friction_str); - } - - if (damping_str == NULL && friction_str == NULL) - { - logger->reportError("joint dynamics element specified with no damping and no friction"); - return false; - } - } + if (m_parseSDF) + { + if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint) + { + // axis + TiXmlElement *axis_xml = config->FirstChildElement("axis"); + if (!axis_xml){ + logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis"); + logger->reportWarning(joint.m_name.c_str()); + joint.m_localJointAxis.setValue(1,0,0); + } + else{ + TiXmlElement *xyz_xml = axis_xml->FirstChildElement("xyz"); + if (xyz_xml) { + if (!parseVector3(joint.m_localJointAxis,std::string(xyz_xml->GetText()),logger)) + { + logger->reportError("Malformed axis element:"); + logger->reportError(joint.m_name.c_str()); + logger->reportError(" for joint:"); + logger->reportError(xyz_xml->GetText()); + return false; + } + } + + TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit"); + if (limit_xml) + { + if (!parseJointLimits(joint, limit_xml,logger)) + { + logger->reportError("Could not parse limit element for joint:"); + logger->reportError(joint.m_name.c_str()); + return false; + } + } + else if (joint.m_type == URDFRevoluteJoint) + { + logger->reportError("Joint is of type REVOLUTE but it does not specify limits"); + logger->reportError(joint.m_name.c_str()); + return false; + } + else if (joint.m_type == URDFPrismaticJoint) + { + logger->reportError("Joint is of type PRISMATIC without limits"); + logger->reportError( joint.m_name.c_str()); + return false; + } + + TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); + if (prop_xml) + { + if (!parseJointDynamics(joint, prop_xml,logger)) + { + logger->reportError("Could not parse dynamics element for joint:"); + logger->reportError(joint.m_name.c_str()); + return false; + } + } + } + } + } + else + { + // Get Joint Axis + if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint) + { + // axis + TiXmlElement *axis_xml = config->FirstChildElement("axis"); + if (!axis_xml){ + logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis"); + logger->reportWarning(joint.m_name.c_str()); + joint.m_localJointAxis.setValue(1,0,0); + } + else{ + if (axis_xml->Attribute("xyz")) + { + if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger)) + { + logger->reportError("Malformed axis element:"); + logger->reportError(joint.m_name.c_str()); + logger->reportError(" for joint:"); + logger->reportError(axis_xml->Attribute("xyz")); + return false; + } + } + } + } + + // Get limit + TiXmlElement *limit_xml = config->FirstChildElement("limit"); + if (limit_xml) + { + if (!parseJointLimits(joint, limit_xml,logger)) + { + logger->reportError("Could not parse limit element for joint:"); + logger->reportError(joint.m_name.c_str()); + return false; + } + } + else if (joint.m_type == URDFRevoluteJoint) + { + logger->reportError("Joint is of type REVOLUTE but it does not specify limits"); + logger->reportError(joint.m_name.c_str()); + return false; + } + else if (joint.m_type == URDFPrismaticJoint) + { + logger->reportError("Joint is of type PRISMATIC without limits"); + logger->reportError( joint.m_name.c_str()); + return false; + } + + joint.m_jointDamping = 0; + joint.m_jointFriction = 0; + + // Get Dynamics + TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); + if (prop_xml) + { + + // Get joint damping + const char* damping_str = prop_xml->Attribute("damping"); + if (damping_str) + { + joint.m_jointDamping = urdfLexicalCast(damping_str); + } + + // Get joint friction + const char* friction_str = prop_xml->Attribute("friction"); + if (friction_str) + { + joint.m_jointFriction = urdfLexicalCast(friction_str); + } + + if (damping_str == NULL && friction_str == NULL) + { + logger->reportError("joint dynamics element specified with no damping and no friction"); + return false; + } + } + } return true; } @@ -801,7 +1017,6 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger) parentLink->m_childJoints.push_back(joint); parentLink->m_childLinks.push_back(childLink); parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str()); - } } @@ -1035,8 +1250,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) } localModel->m_name = name; - - + TiXmlElement* pose_xml = robot_xml->FirstChildElement("pose"); + if (0==pose_xml) + { + localModel->m_rootTransformInWorld.setIdentity(); + } + else + { + parseTransform(localModel->m_rootTransformInWorld,pose_xml,logger,m_parseSDF); + } + // Get all Material elements for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index b966c2761..bb0877172 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -88,6 +88,7 @@ struct UrdfLink { std::string m_name; UrdfInertia m_inertia; + btTransform m_linkTransformInWorld; btArray m_visualArray; btArray m_collisionArray; UrdfLink* m_parentLink; @@ -127,6 +128,7 @@ struct UrdfJoint struct UrdfModel { std::string m_name; + btTransform m_rootTransformInWorld; btHashMap m_materials; btHashMap m_links; btHashMap m_joints; @@ -155,6 +157,7 @@ protected: bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger); bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger); bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger); + bool parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger); bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger); bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index c203a3b36..7169450fb 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -16,7 +16,9 @@ struct MyMotorInfo2 { btScalar m_velTarget; btScalar m_maxForce; + btScalar m_posTarget; int m_uIndex; + int m_qIndex; }; @@ -39,7 +41,8 @@ protected: public: //@todo, add accessor methods - MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS]; + // MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS]; + MyMotorInfo2 m_motorTargetPositions[MAX_NUM_MOTORS]; int m_numMotors; @@ -136,10 +139,17 @@ public: { for (int i=0;i=0) { b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + + int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); + for (int i = 0; i < numJoints; ++i) { + struct b3JointSensorState sensorState; + b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState); + b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque); + } } break; }; @@ -308,7 +326,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) } case CMD_SEND_DESIRED_STATE: { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY); + // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD); prepareControlCommand(command); b3SubmitClientCommand(m_physicsClientHandle, command); break; @@ -330,6 +349,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) break; } + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { + b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle); + b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8); + b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle); + break; + } default: { b3Error("Unknown buttonId"); @@ -387,6 +412,7 @@ void PhysicsClientExample::createButtons() createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger); createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger); createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); + createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); if (m_physicsClientHandle && m_selectedBody>=0) @@ -403,14 +429,20 @@ void PhysicsClientExample::createButtons() if (m_numMotorsm_velTarget = 0.f; + motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; + motorInfo->m_qIndex = info.m_qIndex; - SliderParams slider(motorName,&motorInfo->m_velTarget); - slider.m_minVal=-4; - slider.m_maxVal=4; + // SliderParams slider(motorName,&motorInfo->m_velTarget); + // slider.m_minVal=-4; + // slider.m_maxVal=4; + SliderParams slider(motorName,&motorInfo->m_posTarget); + slider.m_minVal=-4; + slider.m_maxVal=4; if (m_guiHelper && m_guiHelper->getParameterInterface()) { m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);